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