Rotation matrix to rotation vector (Rodrigues function)

asked 2016-01-25 07:54:16 -0600

aripod gravatar image

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?

edit retag flag offensive close merge delete

Comments

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.

Tetragramm gravatar imageTetragramm ( 2016-01-25 14:42:58 -0600 )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.

aripod gravatar imagearipod ( 2016-01-26 09:22:53 -0600 )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.

Tetragramm gravatar imageTetragramm ( 2016-01-26 17:58:03 -0600 )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.

zar zar gravatar imagezar zar ( 2018-11-26 04:44:25 -0600 )edit

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

berak gravatar imageberak ( 2018-11-26 05:00:55 -0600 )edit

Where's the implementation of Rodrigues fromula in c++ itself ?

Yaroslav Solomentsev gravatar imageYaroslav Solomentsev ( 2020-11-25 09:10:04 -0600 )edit

@Yaroslav Solomentsev please don't post answers here, if you have a question or comment, thank you.

berak gravatar imageberak ( 2020-11-25 09:19:10 -0600 )edit