从本征旋转和平移矩阵创建 Sophus::SE3 对象并返回

Question

我正在为我的项目使用 Eigen 和 Sophus。我想要做的就是根据 Eigen 中定义的旋转和平移创建 Sophus SE3 对象，并从 SE3 获得旋转和平移，但我得到不同的矩阵。这是我的代码：

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

当我运行这个程序时，我得到以下结果。

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

我能够得到平移向量，但旋转矩阵不正确。你们能指出我做错的正确方向吗？谢谢。

Answer 1

我在 sophus github 页面的帮助下找到了解决方案：

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

我希望它能帮助面临同样问题的人。实际上我创建了一个无效的旋转矩阵。所以我使用这段代码创建了一个有效的旋转矩阵：

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

结果也符合预期。我希望它会对某人有所帮助。

从本征旋转和平移矩阵创建 Sophus::SE3 对象并返回

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

c++

computer-vision

c++11

eigen3