Rotation matrix to rotation vector (Rodrigues function)

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 <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?

edit retag close merge delete

Am I mistaken that Matlab does everything column major? No, I just checked. I don't know if that alters the function you're using, I didn't find that as a built in function.

Try the transpose of R and see if that matches.

( 2016-01-25 14:42:58 -0500 )edit

R is symmetric.....I used the values from the formula but switched their positions so rather than using [0.0232688 3.13962 4.94066e-324] = (X,Y,Z) I use the vector like (Z, X, Y) and checking in rviz the frames, they are like the should be......I just don't get why is ZXY rather than XYZ.

( 2016-01-26 09:22:53 -0500 )edit

Ah, well, that's actually probably the problem. That matrix isn't exactly symmetric, but a rotation matrix that is symmetric is a 180 degree rotation. So there are multiple rotations that get you to the same place.

Try a 90 degree rotation and then check.

( 2016-01-26 17:58:03 -0500 )edit

Dear, I have rvec(rotation vector) and tvec(translation vector). How can I find the camera position (EYE vector). I would like to continue to use to find the Reflectance. Thank you in advanced.

( 2018-11-26 04:44:25 -0500 )edit

@zar zar , please do not post answers here, if you have a question or comment, thank you.

( 2018-11-26 05:00:55 -0500 )edit