SLAM第四讲实践：李代数Sophus库的安装和使用

2022 9.26 bionic

1.fim8.1.1 安装

自取一波

链接：https://pan.baidu.com/s/1S5DB6nO86bg_FyPhvxLb8Q 
提取码：mc33

1.1 安装命令

cd fmt-8.1.1/
mkdir build
cd build
cmake ..
make
sudo make install

2 Sophus库安装

查看自己Eigen的版本号，因为Sophus依赖于Eigen，所以如果版本过低需要卸载重装（最低2.91.0）
Found Eigen3: /usr/include/eigen3 (Required is at least version "2.91.0")

2.1进入终端

cat /usr/include/eigen3/Eigen/src/Core/util/Macros.h

我的版本号是3.3.4 满足要求

如果你的版本过低，正确方法：
（1）在官网（http://eigen.tuxfamily.org/index.php?title=Main_Page）下载安装包：eigen-3.3.7.tar.bz2 ，然后提取到此处
（2）进入文件夹eigen-3.3.7，右键在终端打开。
（3）进行安装

安装方法就不说了，看到这里应该没人不会吧哈哈

紧接着

3 去github上打开slambook2master包，点到sopuhs里去，找到原作者，直接从他那下载zip

https://github.com/strasdat/Sophus/tree/13fb3288311485dc94e3226b69c9b59cd06ff94e

这是他用来做教程的包，里面有他自己写的程序包，也有他直接链接别人写好的包，比如sopuhs，这种包的原作者不是高翔，你就载github上打开slambook2master包，点到sopuhs里去，找到原作者，直接从他那下载zip
因为高翔也是调用别人的包,所以下载后解压出来是空的，直接点到原始作者发布sophus包的地方,再下载就行了

mkdir build
cd build
cmake ..
make
sudo make install

4.实现

我这里使用的是slambook2的代码

有关于

同样因为时模板类的Sophus，源码中的所有Sophus::SO3都需要声明为Sophus::SO3d 或者
Sophus::SO3.这个问题好像不存在，貌似只存在于slambook1

如果你是slambook1的代码，请去源码修改：

#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);            // 或者四元数
    Sophus::SO3d SO3_R(R);               // Sophus::SO(3)可以直接从旋转矩阵构造
    Sophus::SO3<double> SO3_q( q );

    // 上述表达方式都是等价的
    // 输出SO(3)时，以so(3)形式输出
    cout<<"SO(3) from matrix: "<<SO3_R.log()<<endl;
    cout<<"SO(3) from quaternion :"<<SO3_q.log()<<endl;
    
    // 使用对数映射获得它的李代数
    Vector3d so3 = SO3_R.log();
    cout<<"so3 = "<<so3.transpose()<<endl;
    // 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纯粹是为了输出美观一些
    
    // 增量扰动模型的更新
    Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
    Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3)*SO3_R;
    cout<<"SO3 updated = "<<SO3_updated.log()<<endl;
    
    /********************萌萌的分割线*****************************/
    cout<<"************我是分割线*************"<<endl;
    // 对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)
    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)的平移在前，旋转在后.
    // 同样的，有hat和vee两个算符
    cout<<"se3 hat = "<<endl<<Sophus::SE3d::hat(se3)<<endl;
    cout<<"se3 hat vee = "<<Sophus::SE3d::vee( Sophus::SE3d::hat(se3) ).transpose()<<endl;
    
    // 最后，演示一下更新
    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;   
    
    return 0;
}

其他问题可以去看看这个博主的文章

https://blog.csdn.net/weixin_43747622/article/details/117749523

他的问题我其实没有遇到，但是不代表以后换台机子不会遇到，收藏起来哈哈

最后

以上就是糊涂哈密瓜为你收集整理的SLAM第四讲实践：李代数Sophus库的安装和使用的全部内容，希望文章能够帮你解决SLAM第四讲实践：李代数Sophus库的安装和使用所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。