【Sophus】【Sophus实践】【Sophus的学习使用记录】
0 前言
1 安装
- 参考【slam十四讲第二版】【课本例题代码向】【第三~四讲刚体运动、李群和李代数】【eigen3.3.4和pangolin安装，Sophus及fim的安装使用】【绘制轨迹】【计算以及介绍轨迹误差】的1.2 Sophus库的安装
2. 实践：Sophus Sophus的基本使用方法
2.1 前言
由于第一版的好像没有使用模板类，所以进行修改，不修改运行不了，头文件尾缀都变了
SLAM十四讲ch4代码调整参考
https://blog.csdn.net/weixin_43747622/article/details/117749523
- 这个简单，就不提供我的代码包了
2.1 sophus的基本使用说明
- 参考* 参考【slam十四讲第二版】【课本例题代码向】【第三~四讲刚体运动、李群和李代数】【eigen3.3.4和pangolin安装，Sophus及fim的安装使用】【绘制轨迹】【计算以及介绍轨迹误差】的2.2 示例
2.1.1 头文件和CMakeLists.txt说明
- 头文件包含
#include "sophus/so3.hpp"
#include "sophus/se3.hpp"
- CMakeLists.txt说明，需要安装fmt，参考1.2第三步
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
add_executable(xxx src/xxx.cpp)
target_link_libraries(xxx ${Sophus_LIBRARIES} fmt)
- 如果找不到sophus的情况，可以试试，参考https://blog.csdn.net/weixin_48953899/article/details/124277893：
set(Sophus_LIBRARIES "/usr/local/lib/libSophus.so")
2.1.2 旋转矩阵李群SO3和李代数so3
2.1.2.1 李群初始化（旋转矩阵|四元数->李群SO3）
// 沿Z轴转90度的旋转矩阵
Matrix3d R = Eigen::AngleAxisd(M_PI/2, Vector3d(0,0,1)).toRotationMatrix();//AngleAxisd为旋转向量
Quaterniond q(R); // 或者四元数
Sophus::SO3d SO3_R(R); // Sophus::SO(3)可以直接从旋转矩阵构造
Sophus::SO3<double> SO3_q( q );
// 上述表达方式都是等价的
2.1.2.2 将李群SO3转换为李代数so3输出（SO3.log()）
// 输出SO(3)时，以so(3)形式输出
cout<<"SO(3) from matrix:\n"<<SO3_R.log()<<endl;
cout<<"SO(3) from quaternion:\n"<<SO3_q.log()<<endl;
输出：
SO(3) from matrix:
0
0
1.5708
SO(3) from quaternion:
0
0
1.5708
2.1.2.3 初始化李代数（李群->李代数）
// 使用对数映射获得它的李代数
Vector3d so3 = SO3_R.log();
cout<<"so3 = "<<so3.transpose()<<endl;
输出：
so3 = 0 0 1.5708
2.1.2.4 李代数向量<=>反对陈矩阵SO3.hat()和SO3.vee()
// hat 为向量到反对称矩阵
cout<<"so3 hat=\n"<<Sophus::SO3d::hat(so3)<<endl;
// 相对的，vee为反对称到向量
cout<<"so3 hat vee= "<<Sophus::SO3d::vee( Sophus::SO3d::hat(so3) ).transpose()<<endl; // transpose纯粹是为了输出美观一些
输出：
so3 hat=
0 -1.5708 0
1.5708 0 -0
-0 0 0
so3 hat vee= 0 0 1.5708
2.1.2.5 将李代数so3转换为李群SO3输出（exp(so3)） | 增量扰动模型的更新
// 增量扰动模型的更新
Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3)*SO3_R;
cout<<"SO3 updated = \n"<<SO3_updated.log()<<endl;
输出：
SO3 updated =
7.85398e-05
-7.85398e-05
1.5708
2.1.3 变换矩阵李群SE3和李代数se3
2.1.3.1 初始化李群SE3（旋转矩阵R & 平移t ->变换矩阵李群SE3）
// 对SE(3)操作大同小异
Vector3d t(1,0,0); // 沿X轴平移1
Sophus::SE3d SE3_Rt(R, t); // 从R,t构造SE(3)
Sophus::SE3d SE3_qt(q,t); // 从q,t构造SE(3)
2.1.3.2 初始化李代数se3（李群SE3 -> 李代数se3）SE3.log()
- 会发现李代数：平移在前，旋转在后
cout<<"SE3 from R,t= "<<endl<<SE3_Rt.log()<<endl;
cout<<"SE3 from q,t= "<<endl<<SE3_qt.log()<<endl;
// 李代数se(3) 是一个六维向量，方便起见先typedef一下
typedef Matrix<double,6,1> Vector6d;
Vector6d se3 = SE3_Rt.log();
cout<<"se3 = "<<se3.transpose()<<endl;
// 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
输出：
SE3 from R,t=
0.785398
-0.785398
0
0
0
1.5708
SE3 from q,t=
0.785398
-0.785398
0
0
0
1.5708
se3 = 0.785398 -0.785398 0 0 0 1.5708
2.1.3.3 李代数矩阵<=>反对陈矩阵SE3.hat()<=>SE3.vee()
// 同样的，有hat和vee两个算符
cout<<"se3 hat = "<<endl<<Sophus::SE3d::hat(se3)<<endl;
cout<<"se3 hat vee = "<<Sophus::SE3d::vee( Sophus::SE3d::hat(se3) ).transpose()<<endl;
输出：
se3 hat =
0 -1.5708 0 0.785398
1.5708 0 -0 -0.785398
-0 0 0 0
0 0 0 0
se3 hat vee = 0.785398 -0.785398 0 0 0 1.5708
2.1.3.4 将李代数se3转换为李群SE3输出（exp(se3)） | 增量扰动模型的更新
// 最后，演示一下更新
Vector6d update_se3; //更新量
update_se3.setZero();
update_se3(0,0) = 1e-4;
Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3)*SE3_Rt;
cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;
输出：
SE3 updated =
2.22045e-16 -1 0 1.0001
1 2.22045e-16 0 0
0 0 1 0
0 0 0 1
2.1.3.5 李群SE3->旋转矩阵RSE3.rotationMatrix()|平移向量tSE3.translation()
Sophus::SE3d pose_gn;//位姿（李群）
cout << "R = \n" << pose_gn.rotationMatrix() << endl;
cout << "t = " << pose_gn.translation().transpose() << endl;
2.1.4 使用李群进行旋转
2.1.4.1 对位姿相称
- 要使用李群进行计算
Sophus::SE3d& pose
pose = Sophus::SE3d::exp(dx) * pose; //dx是李代数，要转换为李群
3 报错解决
3.1 未定义的应用
- 报错：
====================[ 构建 | pose_graph_ceres_SE3 | Debug ]=======================
/home/bupo/CLion-2022.1.2/clion-2022.1.2/bin/cmake/linux/bin/cmake --build /home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/cmake-build-debug --target pose_graph_ceres_SE3 -j 6
[1/1] Linking CXX executable pose_graph_ceres_SE3
FAILED: pose_graph_ceres_SE3
: && /usr/bin/c++ -g -rdynamic CMakeFiles/pose_graph_ceres_SE3.dir/src/pose_graph_ceres_SE3.cpp.o -o pose_graph_ceres_SE3 -Wl,-rpath,/usr/local/lib /usr/local/lib/libceres.a -lfmt /usr/local/lib/x86_64-linux-gnu/libglog.a /usr/local/lib/libgflags.a -lpthread -lspqr -ltbb -lcholmod -lccolamd -lcamd -lcolamd -lamd -llapack -lblas -lsuitesparseconfig -lrt /usr/local/lib/libmetis.so -lcxsparse -llapack -lblas -lsuitesparseconfig -lrt /usr/local/lib/libmetis.so -lcxsparse && :
CMakeFiles/pose_graph_ceres_SE3.dir/src/pose_graph_ceres_SE3.cpp.o：在函数‘main’中：
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:194：对‘Sophus::SE3::SE3(Eigen::Quaternion<double, 0> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:194：对‘Sophus::SE3::log() const’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:250：对‘Sophus::SE3::exp(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&)’未定义的引用
CMakeFiles/pose_graph_ceres_SE3.dir/src/pose_graph_ceres_SE3.cpp.o：在函数‘SE3Parameterization::Plus(double const*, double const*, double*) const’中：
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:30：对‘Sophus::SE3::exp(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&)’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:31：对‘Sophus::SE3::exp(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&)’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:34：对‘Sophus::SE3::operator*(Sophus::SE3 const&) const’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:34：对‘Sophus::SE3::log() const’未定义的引用
CMakeFiles/pose_graph_ceres_SE3.dir/src/pose_graph_ceres_SE3.cpp.o：在函数‘PosegraphBA::PosegraphBA(double, double, double, double, double, double, double)’中：
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:61：对‘Sophus::SE3::SE3()’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:64：对‘Sophus::SE3::SE3(Eigen::Quaternion<double, 0> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:64：对‘Sophus::SE3::inverse() const’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:64：对‘Sophus::SE3::operator=(Sophus::SE3 const&)’未定义的引用
CMakeFiles/pose_graph_ceres_SE3.dir/src/pose_graph_ceres_SE3.cpp.o：在函数‘PosegraphBA::Evaluate(double const* const*, double*, double**) const’中：
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:74：对‘Sophus::SE3::exp(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&)’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:78：对‘Sophus::SE3::exp(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&)’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:81：对‘Sophus::SE3::inverse() const’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:81：对‘Sophus::SE3::operator*(Sophus::SE3 const&) const’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:81：对‘Sophus::SE3::operator*(Sophus::SE3 const&) const’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:82：对‘Sophus::SE3::log() const’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:103：对‘Sophus::SO3::log() const’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:103：对‘Sophus::SO3::hat(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:104：对‘Sophus::SO3::hat(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:106：对‘Sophus::SO3::log() const’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:106：对‘Sophus::SO3::hat(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:112：对‘Sophus::SE3::inverse() const’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:112：对‘Sophus::SE3::Adj() const’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:122：对‘Sophus::SE3::inverse() const’未定义的引用
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/src/pose_graph_ceres_SE3.cpp:122：对‘Sophus::SE3::Adj() const’未定义的引用
collect2: error: ld returned 1 exit status
ninja: build stopped: subcommand failed.
- 解决方案：
在CMakeLists.txt里面加上一句，将libSophus.so路径设置到CMakeLists中
set(Sophus_LIBRARIES "/usr/local/lib/libSophus.so")
7186
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
