Slam14讲学习，再新版本的sophus的ch4代码

1.修改后的代码

#include <iostream> #include <cmath> #include <Eigen/Core> #include <Eigen/Geometry> #include <sophus/so3.hpp> #include <sophus/se3.hpp> using namespace std; using namespace Eigen; int main(int argc, char **argv) { // 旋转矩阵 (绕Z轴旋转90度) Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix(); // 四元数 Quaterniond q(R); // 平移向量 // ---------- SO(3) ---------- // 从旋转矩阵构造 Sophus::SO3<double> SO3_R(R); // 从旋转向量构造 (使用指数映射) Vector3d v(0, 0, M_PI / 2); Sophus::SO3<double> SO3_v = Sophus::SO3<double>::exp(v); // 从四元数构造 Sophus::SO3<double> SO3_q(q); // 输出时需转换为矩阵或对数 cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl; cout << "SO(3) from vector (exp):\n" << SO3_v.matrix() << endl; cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl; // 对数映射 (李代数) Vector3d so3 = SO3_R.log(); cout << "so3 = " << so3.transpose() << endl; // hat 和 vee cout << "so3 hat =\n" << Sophus::SO3<double>::hat(so3) << endl; cout << "so3 hat vee = " << Sophus::SO3<double>::vee(Sophus::SO3<double>::hat(so3)).transpose() << endl; // 扰动更新 (左乘) Vector3d update_so3(1e-4, 0, 0); Sophus::SO3<double> SO3_updated = Sophus::SO3<double>::exp(update_so3) * SO3_R; cout << "SO3 updated matrix:\n" << SO3_updated.matrix() << endl; cout<<"************我是分割线*************"<<endl; // ---------- SE(3) ---------- Vector3d t(1, 0, 0); Sophus::SE3<double> SE3_Rt(R, t); // 从 R, t 构造 Sophus::SE3<double> SE3_qt(q, t); // 从 q, t 构造 cout << "SE3 from R,t (matrix):\n" << SE3_Rt.matrix() << endl; cout << "SE3 from q,t (matrix):\n" << SE3_qt.matrix() << endl; // 李代数 se(3) typedef Matrix<double, 6, 1> Vector6d; Vector6d se3 = SE3_Rt.log(); cout << "se3 = " << se3.transpose() << endl; // hat 和 vee cout << "se3 hat =\n" << Sophus::SE3<double>::hat(se3) << endl; cout << "se3 hat vee = " << Sophus::SE3<double>::vee(Sophus::SE3<double>::hat(se3)).transpose() << endl; // 扰动更新 Vector6d update_se3 = Vector6d::Zero(); update_se3(0) = 1e-4; Sophus::SE3<double> SE3_updated = Sophus::SE3<double>::exp(update_se3) * SE3_Rt; cout << "SE3 updated matrix:\n" << SE3_updated.matrix() << endl; return 0; }

CMakeList.txt

cmake_minimum_required( VERSION 3.10 ) project( useSophus ) # Sophus set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(Eigen3 REQUIRED) find_package( Sophus REQUIRED ) add_executable( useSophus useSophus.cpp ) # 现代写法：自动带入头文件路径+依赖，不需要手动include_directories target_link_libraries( useSophus Sophus::Sophus )

2. 修改不同