Your problem is in Rodrigues function : It does not give you an angle but a vector You don't need to divide by M_PI and multiply by 180 :Rodrigues vector points along the axis of the rotation, and its magnitude is the tangent of half the angle of the rotation
Results are :
Mean marker distance22.0907
Essai 0 with 1241points
rodrigues [-0.008186903247588993, -0.007724206343463332, -0.007796571200079321]
translation [-0.792428255173678, 0.607350228303508, -0.05641950533350303]
Essai 1 with 1141points
rodrigues [-0.006422562844045941, -0.007930323721579204, -0.007210073468255511]
translation [-0.7848951038350637, 0.6194026473826763, -0.01673428788675164]
Essai 2 with 1051points
rodrigues [-0.007931756116646859, -0.008254818133971985, -0.007837048508731704]
translation [-0.7885528566096911, 0.6133322518870682, -0.04481005610165935]
Essai 3 with 971points
rodrigues [-0.005932638328178316, -0.007302101937579567, -0.006471234288512953]
translation [-0.7865388205016264, 0.6173586807921311, -0.01499810303038783]
Essai 4 with 901points
rodrigues [-0.04190340485114951, -0.05506477514402782, -0.005574481988185056]
translation [0.05958445984982116, -0.03116133096247916, -0.9977367706950827]
Essai 5 with 841points
rodrigues [-0.007732722496448771, -0.008182869208508877, -0.007570432016673033]
translation [-0.7867221935703506, 0.6156568405101853, -0.04510925489156264]
Essai 6 with 791points
rodrigues [-0.006090562167324761, -0.007260625571239707, -0.006922225724916698]
translation [-0.7856904222816165, 0.6181434310759613, -0.02427465659022878]
Essai 7 with 751points
rodrigues [-0.006480131550192582, -0.007292119308853618, -0.006526800541123394]
translation [-0.7906105072805985, 0.6120260023408047, -0.01895252585404144]
Essai 8 with 721points
rodrigues [-0.002347222579832763, -0.007466597396004799, -0.005961284930281934]
translation [-0.7572073975061766, 0.6529732600544519, 0.01621353804029994]
Essai 9 with 701points
rodrigues [-0.006929786646756651, -0.007944380422144397, -0.007456723468566674]
translation [-0.7846584369744524, 0.6189268459772316, -0.0352235235813468]
with this program :
#include<opencv2/opencv.hpp>
using namespace std;
using namespace cv;
#define M_PI acos(-1.0)
int main(int argc, char *argv[])
{
vector<cv::Point2d> static_feature_point_t;
std::vector<cv::Point2d> static_feature_point_tmdelta;
FileStorage f1("static_feature_point_t.yml", FileStorage::READ);
f1["feature_point"] >> static_feature_point_t;
f1.release();
FileStorage f2("static_feature_point_tmdelta.yml", FileStorage::READ);
f2["feature_point"] >> static_feature_point_tmdelta;
f2.release();
Mat x(500, 500, CV_8UC3, Scalar(0, 0, 0));
Mat y(500, 500, CV_8UC3, Scalar(0, 0, 0));
double d = 0;
for (int i = 0; i < static_feature_point_t.size(); i++)
{
circle(x, static_feature_point_t[i], 3, Scalar(0, 0, 255));
circle(y, static_feature_point_tmdelta[i], 3, Scalar(0, 255, 255));
d += norm(static_feature_point_t[i] - static_feature_point_tmdelta[i]);
}
cout << "Mean marker distance" << d / static_feature_point_t.size() << "\n";
imshow("ptx", x);
imshow("pty", y);
waitKey();
for (int i=0;i<10;i++)
{
double focal = 300.;
cv::Point2d camera_principal_point(320, 240);
cv::Mat essential_matrix = cv::findEssentialMat(static_feature_point_t, static_feature_point_tmdelta, focal, camera_principal_point, cv::LMEDS);
cv::Mat rotation=Mat::zeros(3,3,CV_64F), translation=Mat::zeros(3, 1, CV_64F);
cv::recoverPose(essential_matrix, static_feature_point_t, static_feature_point_tmdelta, rotation, translation, focal, camera_principal_point);
cv::Mat rot(3, 1, CV_64F);
cv::Rodrigues(rotation, rot);
cout << "Essai " << i << " with " << static_feature_point_t.size() << "points\n";
//std::cout << "E " << essential_matrix << std::endl;
//std::cout << "rotation " << rotation << std::endl;
std::cout << "rodrigues " << rot.t() << std::endl;
std::cout << "translation " << translation.t() << std::endl;
for (int j = i; j < 10; j++)
{
static_feature_point_t.erase(static_feature_point_t.begin() + j * 20, static_feature_point_t.begin() + j * 20 + 10);
static_feature_point_tmdelta.erase(static_feature_point_tmdelta.begin() + j * 20, static_feature_point_tmdelta.begin() + j * 20 + 10);
}
}
}
please post your code with findEssentialMat
I added it.
Did you solve your problem ? can you post data in yml format?
No, I didn't. I added data and I changed the code according to the .yml files. If you run the code with all points, it works but if you remove the two last points in static_feature_point_t and static_feature_point_tmdelta, the result is totally different (there are 1241 feature points).