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::SO3double SO3_R(R); // 从旋转向量构造 (使用指数映射) Vector3d v(0, 0, M_PI / 2); Sophus::SO3double SO3_v Sophus::SO3double::exp(v); // 从四元数构造 Sophus::SO3double 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::SO3double::hat(so3) endl; cout so3 hat vee Sophus::SO3double::vee(Sophus::SO3double::hat(so3)).transpose() endl; // 扰动更新 (左乘) Vector3d update_so3(1e-4, 0, 0); Sophus::SO3double SO3_updated Sophus::SO3double::exp(update_so3) * SO3_R; cout SO3 updated matrix:\n SO3_updated.matrix() endl; cout************我是分割线*************endl; // ---------- SE(3) ---------- Vector3d t(1, 0, 0); Sophus::SE3double SE3_Rt(R, t); // 从 R, t 构造 Sophus::SE3double 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 Matrixdouble, 6, 1 Vector6d; Vector6d se3 SE3_Rt.log(); cout se3 se3.transpose() endl; // hat 和 vee cout se3 hat \n Sophus::SE3double::hat(se3) endl; cout se3 hat vee Sophus::SE3double::vee(Sophus::SE3double::hat(se3)).transpose() endl; // 扰动更新 Vector6d update_se3 Vector6d::Zero(); update_se3(0) 1e-4; Sophus::SE3double SE3_updated Sophus::SE3double::exp(update_se3) * SE3_Rt; cout SE3 updated matrix:\n SE3_updated.matrix() endl; return 0; }CMakeList.txtcmake_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. 修改不同方面原始代码书中修正后代码适配新版类型声明Sophus::SO3 R;无模板参数Sophus::SO3double R;必须指定标量类型从旋转向量构造Sophus::SO3 SO3_v(0, 0, M_PI/2);改为auto SO3_v Sophus::SO3double::exp(Vector3d(0,0,M_PI/2));使用指数映射输出对象cout SO3_R endl;直接流输出cout SO3_R.matrix() endl;调用.matrix()或.log()等静态方法调用Sophus::SO3::hat(so3)Sophus::SO3double::hat(so3)也要加模板参数头文件#include sophus/so3.h#include sophus/so3.hpp但新版也兼容.h旧名更推荐.hppC 标准默认 C11/14必须在 CMake 中设置CMAKE_CXX_STANDARD 17依赖项仅需 Eigen额外需要fmt库新版依赖