Sophus与Eigen之间矩阵转换

Sophus与Eigen之间矩阵转换

// 资料备份：
// Sophus库提供了将Sophus类型转换为Eigen类型的功能。
// 可以使用Sophus::SE3d::matrix()方法将Sophus的SE3类型转换为Eigen的4x4矩阵类型。
// 同样，可以使用Sophus::SO3d::matrix()方法将Sophus的SO3类型转换为Eigen的3x3矩阵类型。

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

// 创建一个Sophus的SE3类型
Sophus::SE3d se3(Sophus::SO3d::rotX(0.1), Eigen::Vector3d(1, 2, 3));

// 将Sophus的SE3类型转换为Eigen的4x4矩阵类型
Eigen::Matrix4d eigenMatrix = se3.matrix();

// 创建一个Sophus的SO3类型
Sophus::SO3d so3 = Sophus::SO3d::rotX(0.1);

// 将Sophus的SO3类型转换为Eigen的3x3矩阵类型
Eigen::Matrix3d eigenMatrix = so3.matrix();

// 输出转换后的Eigen矩阵
std::cout << "Eigen Matrix:\n" << eigenMatrix << std::endl;

// 参考：https://blog.csdn.net/qq_38410730/article/details/132744272
// 先定义轴角、旋转矩阵、四元数、变换矩阵
Eigen::AngleAxisd A(M_PI / 2, Eigen::Vector3d(0, 0, 1));     // 旋转轴：(0,0,1)，角度：180度
Eigen::Matrix3d R = A.matrix();
Eigen::Quaterniond Q(A);
Eigen::Vector3d t(1, 0, 0);
Eigen::Isometry3d T= Eigen::Isometry3d::Identity();          // 虽然称为3d，实质上是4＊4的矩阵
T.rotate(R);
T.pretranslate(t);

// 初始化部分
Sophus::SO3d SO3_R(R);       // 从旋转矩阵初始化
Sophus::SO3d SO3_Q(Q);       // 从四元数初始化
// Sophus::SO3d SO3_A(A);       // 从轴角初始化，错误！！！！

Sophus::SE3d SE3_R(R, t);            // 从旋转矩阵初始化
Sophus::SE3d SE3_Q(Q, t);            // 从四元数初始化
Sophus::SE3d SE3_SO3(SO3_R, t);      // 从SO3初始化
Sophus::SE3d SE3_T(T.matrix());      // 从变换矩阵初始化

// 相互转化
Sophus::SO3d SO3_SE3 = SE3_R.so3();				// 从SE3转换到SO3
Eigen::Vector3d t_SE3 = SE3_R.translation(); 	// 从SE3转换到t
Eigen::Matrix3d R_SE3 = SE3_R.rotationMatrix();	// 从SE3转换到R
Eigen::Matrix4d T_SE3 = SE3_R.matrix();			// 从SE3转换到T

// 李群李代数的转换部分
Eigen::Vector3d so3(0, 0, M_PI / 2);            // so3在Eigen中用Vector3d表示
Sophus::SO3d SO3_so3 = Sophus::SO3d::exp(so3);
so3 = SO3_so3.log();

Eigen::Vector6d se3(0, 0 , 1, 0, 0, M_PI / 2);  // se3在Eigen中用Vector6d表示
Sophus::SE3d SE3_se3 = Sophus::SE3d::exp(se3);
se3 = SE3_se3.log();

// 在扰动模型中：
Eigen::AngleAxisd A(M_PI / 2, Eigen::Vector3d(0, 0, 1));
Eigen::Vector3d t(1, 0, 0);

Sophus::SO3d SO3_R(A.matrix());
Eigen::Vector3d update_so3(1e-4, 0, 0);                 // 假设更新量为这么多
Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;

Sophus::SE3d SE3_R(A.matrix(), t);
Sophus::Vector6d update_se3(1e-4, 0, 0, 0, 0, 0);       // 假设更新量为这么多
Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_R;

// 李代数与反对称矩阵的转换部分:
Eigen::Vector3d so3(0, 0, M_PI / 2);
Eigen::Matrix3d so3_hat = Sophus::SO3d::hat(so3);
so3 = Sophus::SO3d::vee(so3_hat);

Eigen::Vector6d se3(0, 0 , 1, 0, 0, M_PI / 2);
Eigen::Matrix6d se3_hat = Sophus::SE3d::hat(se3);
se3 = Sophus::SE3d::vee(se3_hat);

参考链接：https://blog.csdn.net/qq_38410730/article/details/132744272