Create Sophus::SE3 object from Eigen Rotation and translation matrix and go back

Question

I am using Eigen and Sophus for my project. All I want to do is create Sophus SE3 object from rotation and translation defined in Eigen, and get the rotation and translation from SE3, but I get different matrix. Here is my code:

#include <iostream>
#include <opencv2/core.hpp>
#include <Eigen/Core>
#include <sophus/so3.h>
#include <sophus/se3.h>

using namespace std;
using namespace cv;
using namespace Eigen;
using namespace Sophus;

int main(int argc, char* argv[]){
    Eigen::Matrix<double, 3, 3> R;
    R  << 1, 2, 3, 4, 5, 6, 7, 8, 9;
    cout << "R: " << endl << R << endl;
    Eigen::Matrix<double, 3, 1> t;
    t  << 1, 2, 3;
    cout << "t: " << endl << t << endl;
    Sophus::SE3 SE3_Rt(R, t);   // Create Sophus SE3 from R and t
    cout << "SE3_Rt from R and t: " << endl << SE3_Rt << endl;

    // Get rotation and translation matrix from the SE3_Rt
    typedef Eigen::Matrix<double, 6, 1> Vector6d;
    cout << "R from SE_Rt: " << endl << SE3_Rt.rotation_matrix() << endl;
    cout << "t from SE_Rt: " << endl << SE3_Rt.translation() << endl;
    return 0;
}

When I run this program, I get following result.

R: 
1 2 3
4 5 6
7 8 9
t: 
1
2
3
SE3_Rt from R and t: 
   0.2426 -0.485199    0.2426
1 2 3

R from SE_Rt: 
 0.375  -1.25 -1.875
  0.75   0.75  -1.25
 2.125   0.75  0.375
t from SE_Rt: 
1
2
3

I was able to get the translation vector but the rotation matrix is not correct. Could you guys point me to the right direction where I am doing wrong. Thank you.

Answer 1

I found the solution wit a help from the sophus github page:

https://github.com/strasdat/Sophus/issues/150

I hope it will help someone facing same problem. Actually I created an invalid rotation matrix. So I created a valid rotation matrix using this code:

Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();     // PI/2 rotation along z axis

And the results are as expected. I hope it will help someone.