【c++】Eigen::Isometry3f 位姿与 Sophus::SE3f 位姿互相转换

1、代码

#include <Eigen/Core>
#include <sophus/se3.hpp>

int main() {
    // 定义一个 Sophus::SE3f 变量 se3
    Sophus::SE3f se3;
    cout << "se3.matrix()" << endl;
    cout << se3.matrix() << endl << endl;

    // 将 se3 转换为 Eigen::Isometry3f 变量 iso
    Eigen::Isometry3f iso(se3.matrix());
    cout << "iso.matrix()" << endl;
    cout << iso.matrix() << endl << endl;

    // 将 iso 转换回 Sophus::SE3f 变量 new_se3
    Sophus::SE3f new_se3(iso.matrix());
    cout << "new_se3.matrix()" << endl;
    cout << new_se3.matrix() << endl << endl;
}

2、输出结果

se3.matrix()
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1

iso.matrix()
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1

new_se3.matrix()
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1