Sophus安装指南

一、安装前的准备

在安装Sophus之前，需要完成以下准备工作：

1、确保CMake版本为3.0以上。

cmake --version

2、确保Eigen3库已经安装。

sudo apt-get install libeigen3-dev

3、确保安装了对应版本的SUITESPARSE库。

sudo apt-get install libsuitesparse-dev

完成上述准备后，就可以开始安装Sophus库了。

二、安装Sophus库

下载Sophus库最新版本的源代码，解压缩后进入目录：

wget https://github.com/strasdat/Sophus/archive/master.zip
unzip master.zip
cd Sophus-master/

在Sophus目录下创建build文件夹：

mkdir build
cd build/

使用CMake生成Makefile：

cmake ..

使用Make编译Sophus库：

make

若想通过命令行运行Sophus的例子，可以使用以下命令：

make check

最后，使用以下命令安装Sophus库：

sudo make install

安装完成后，就可以在代码中使用Sophus库了。

三、使用Sophus库

以下是一个使用Sophus库的例子：

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

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
    // SO(3)操作
    Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix(); //　旋转矩阵
    Sophus::SO3 SO3_R(R);                                                 // 用旋转矩阵初始化SO(3)表示的姿态
    Sophus::SO3 SO3_v = Sophus::SO3::rotX(M_PI / 2) * Sophus::SO3::rotY(-M_PI / 2); // 用旋转向量初始化SO(3)表示的姿态

    cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
    cout << "SO(3) from vector:\n" << SO3_v.matrix() << endl;
    cout << "so(3) = " << SO3_R.log() << endl; // 输出旋转向量

    Vector3d so3 = SO3_R.log();
    cout << "so3 hat=\n"
         << Sophus::SO3::hat(so3) << endl; //　hat操作
    cout << "so3 hat vee= " << Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose() << endl; // vee操作

    // SE(3)操作
    Vector3d t(1, 0, 0); // 平移向量
    Sophus::SE3 SE3_Rt(R, t); // 用旋转矩阵和平移向量初始化SE(3)表示的变换
    Sophus::SE3 SE3_qt(Sophus::SO3::rotX(M_PI / 2), Vector3d(0, 0, 0)); // 用旋转向量和平移向量初始化SE(3)表示的变换
    cout << "SE3 from R,t= \n" << SE3_Rt.matrix() << endl;
    cout << "SE3 from q,t= \n" << SE3_qt.matrix() << endl;

    typedef Eigen::Matrix
    Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout << "se3 = " << se3.transpose() << endl; // se(3)向量
    cout << "se3 hat = " << endl << Sophus::SE3::hat(se3) << endl; // hat操作
    cout << "se3 hat vee = " << Sophus::SE3::vee(Sophus::SE3::hat(se3)).transpose() << endl; // vee操作

    // 李代数的更新
    Vector6d update_se3;
    update_se3.setZero();
    update_se3(0, 0) = 1e-4d;
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3) * SE3_Rt;
    cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;

    return 0;
}
   

四、常见错误解决方案

1、Eigen库和Sophus库版本不匹配

使用Sophus库需要先安装Eigen库，在使用Sophus库时需要注意Eigen和Sophus的版本是否匹配，否则会出现一些错误，例如传入的矩阵和向量维数不匹配，会提示调用无效的构造函数，并抛出异常。

2、SUITESPARSE库未安装

使用Sophus库需要安装SUITESPARSE库，否则在生成Makefile时会报错，并提示找不到对应的库。

3、编译时提示undefined reference

在链接程序时，需要添加对SUITESPARSE库的链接，例如在CMakeLists.txt中添加以下内容：

find_package(SUITESPARSE REQUIRED)
target_link_libraries(main ${SUITESPARSE_LIBRARIES})

4、安装位置不正确

若Sophus库安装位置不在默认位置，则需要手动添加Sophus库的安装路径至环境变量中，例如在~/.bashrc文件中加入以下内容：

export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/path/to/sophus/install/lib