Revision history [back]

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);
        }
    }
}