Rotation matrix to rotation vector (Rodrigues function)

edit

Hello,

I have a 3x3 rotation matrix that I obtained from stereoCalibrate (using the ros stereo calibration node). I need to obtain a rotation vector (1x3), therefore I used the rodrigues formula. When I went to check the result that I got with this in matlab using the Pietro Perona - California Institute of Technology I get two different results:

This is the code in cpp:

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <tf/transform_broadcaster.h>
#include <ros/param.h>
#include <iostream>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <cv_bridge/cv_bridge.h>

using namespace std;
using namespace cv;

int main(int argc, char** argv)
{
    std::vector<double> T, R;
    double cols, rows, R_cols, R_rows;
    int i, j;
    cv::Mat rot_vec = Mat::zeros(1,3,CV_64F), rot_mat = Mat::zeros(3,3,CV_64F);

    ros::init(argc, argv, "get_extrinsic");
    ros::NodeHandle node;

    if(!node.getParam("translation_vector/cols", cols))
    {
        ROS_ERROR_STREAM("Translation vector (cols) could not be read.");
        return 0;
    }
    if(!node.getParam("translation_vector/rows", rows))
    {
        ROS_ERROR_STREAM("Translation vector (rows) could not be read.");
        return 0;
    }
    T.reserve(cols*rows);

    if(!node.getParam("rotation_matrix/cols", cols))
    {
        ROS_ERROR_STREAM("Rotation matrix (cols) could not be read.");
        return 0;
    }
    if(!node.getParam("rotation_matrix/rows", rows))
    {
        ROS_ERROR_STREAM("Rotation matrix (rows) could not be read.");
        return 0;
    }
    R.reserve(cols*rows);

    if(!node.getParam("translation_vector/data", T))
    {
        ROS_ERROR_STREAM("Translation vector could not be read.");
        return 0;
    }

    if(!node.getParam("rotation_matrix/data", R))
    {
        ROS_ERROR_STREAM("Rotation matrix could not be read.");
        return 0;
    }

    for(i=0; i<3; i++)
    {
        for(j=0; j<3; j++)
            rot_mat.at<double>(i,j) = R[i*3+j];
    }

    std::cout << "Rotation Matrix:"<<endl;
    for(i=0; i<3; i++)
    {
        for(j=0; j<3; j++)
            std::cout<< rot_mat.at<double>(i,j) << " ";
        std::cout << endl;
    }
    std::cout << endl;

    std::cout << "Rodrigues: "<<endl;
    Rodrigues(rot_mat, rot_vec);

    for(i=0; i<3; i++)
        std::cout << rot_vec.at<double>(1,i) << " ";
    std::cout << endl;

    ros::spin();
    return 0;
};

And its output is:

Rotation Matrix:
-0.999998 -0.00188887 -0.000125644 
0.0018868 -0.999888 0.014822 
-0.000153626 0.0148217 0.99989 

Rodrigues: 
0.0232688 3.13962 4.94066e-324

But when I load the same rotation matrix in matlab and use the rodrigues function I get the following:

R =

   -1.0000   -0.0019   -0.0001
    0.0019   -0.9999    0.0148
   -0.0002    0.0148    0.9999

>> rodrigues(R)

ans =

   -0.0002
    0.0233
    3.1396

I can see that the numbers match, but they are in different positions and there seems to be an issue also with the signs.....Which formula should I trust?