【slam十四讲第二版】【课本例题代码向】【第三~四讲刚体运动、李群和李代数】【eigen3.3.4和pangolin安装，Sophus及fim的安装使用】【绘制轨迹】【计算以及介绍轨迹误差】

0前言

在前两天的基础上学习视觉slam

Sophus的使用，大部分教程是根据第一版的书，使用的是非模板类，但是这里使用的是模板类的sophus，主要是参考了下面的文章：

1. 安装

1.1 eigen3.3.4 和 Pangolin的安装

第一步 eigen3.3.4

• 安装3.3.4方法一，好像安装ros会自动安装

sudo apt-get install libeigen3-dev

• 安装3.3.4方法二，要确保自己的没有安装eigen，这个方法我没有尝试，自取安装包链接: https://pan.baidu.com/s/1-_pDkOWKx1kd2FnjIwDwEw 提取码: 96fh

mkdir build
cd build
cmake ..
sudo make install

• 查看版本

sudo gedit /usr/include/eigen3/Eigen/src/Core/util/Macros.h

eigen3的版本一定要在3.3以上，如果自己的不是，就卸载重装，第一次装当我没说
Ubuntu16.04下怎么查看自己的eigen版本号
https://blog.csdn.net/Felaim/article/details/100155997

如果哪天自己犹豫，就eigen3从官网下*，Pangolin按照上面orb的文章下，Sophus按照下面第一种下

eigen官网
https://eigen.tuxfamily.org/index.php?title=Main_Page

• 当然，这里也提供一种从官网自己下载eigen的方法，也就是下面文章《Sophus库安装踩坑（SLAM十四讲）》里面提到的安装方法，选择一种即可

第二步 Pangolin

• 我的网盘有Pangolin：链接: https://pan.baidu.com/s/1BIsPEMpWcgn5l8R8O-UiuA 提取码: q3sh

• 参考Ubuntu18.04下 ORB_SLAM2的安装与配置（其中有eigen3.3.4 和 Pangolin的安装）

• 安装sudo make install之后，一定要执行：sudo ldconfig

• pangolin安装

cd Pangolin
mkdir build
cd build
cmake .. 
make -j8 # 量力而行
sudo make install
sudo ldconfig

• Pangolin的使用在CMakeLists.txt中：

find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
add_executable(xxx xxx.cpp)
target_link_libraries(xxx ${Pangolin_LIBRARIES})

1.2 Sophus库的安装

第三步 fim8.1.1 安装

• 如果你是安装Sophus发现错误，不用担心，补充安装fim8.1.1就可以了

• 使用下面第一种安装的时候，fim安装需要去官网下载 fim8.1.1版本的，错误参考fmt/core.h:1711:3: error: static assertion failed: Cannot format an argument.解决方案

• fim官网 https://fmt.dev/8.1.1/index.html

• 除此之外，我的网盘里面也有8.1.1，自取链接: https://pan.baidu.com/s/1gycaCHvqb-SVOgGhoAydjw 提取码: cte3

• 安装流程可以参考里面的fim安装命令 https://blog.csdn.net/weixin_44684139/article/details/104803225

• 安装

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

第四步 模板类Sophus安装

Sophus库安装踩坑（SLAM十四讲）
https://blog.csdn.net/weixin_44684139/article/details/104803225
或者
自取：链接: https://pan.baidu.com/s/1IwfWoOWYR9Ti2zIaj-pBOg 提取码: jts0

• 安装

cd Sophus/
mkdir build
cd build
cmake ..
make
sudo make install

2. 实践：Sophus Sophus的基本使用方法

2.1 前言

由于第一版的好像没有使用模板类，所以进行修改，不修改运行不了，头文件尾缀都变了

SLAM十四讲ch4代码调整参考
https://blog.csdn.net/weixin_43747622/article/details/117749523

• 这个简单，就不提供我的代码包了

2.1 sophus的使用说明

• 参考【Sophus】【Sophus实践】【Sophus的学习使用记录】

2.2 示例

2.2.1 useSophus.cpp

useSophus.cpp内容为

#include 
#include 
#include 
#include 
#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;
}

2.2.2 CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(useSophus)

include_directories("/usr/include/eigen3")

# 为使用 sophus，需要使用find_package命令找到它
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

# Eigen

add_executable(useSophus useSophus.cpp)
target_link_libraries(useSophus ${Sophus_LIBRARIES} fmt)

2.2.3 输出结果

/home/bupo/my_study/slam14/slam14_my/cap4/useSophus/cmake-build-debug/useSophus
SO(3) from matrix:
     0
     0
1.5708
SO(3) from quaternion:
     0
     0
1.5708
so3 =      0      0 1.5708
so3 hat=
      0 -1.5708       0
 1.5708       0      -0
     -0       0       0
so3 hat vee=      0      0 1.5708
SO3 updated = 
 7.85398e-05
-7.85398e-05
      1.5708
************我是分割线*************
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
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
SE3 updated = 
2.22045e-16          -1           0      1.0001
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1

进程已结束,退出代码0

3 显示运动轨迹

3.1 前言

• 运行需要一个trajectory.txt，自取：链接：https://pan.baidu.com/s/11KoqrZ06lDKND1hZ9eKhKw
  提取码：mi6c
  –来自百度网盘超级会员V3的分享

3.2 draw_trajectory.cpp

#include 
#include 
#include 

// 本例演示了如何画出一个预先存储的轨迹

using namespace std;
using namespace Eigen;

// path to trajectory file
string trajectory_file = "../src/trajectory.txt";

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

int main(int argc, char **argv) {

    vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
    ifstream fin(trajectory_file);
    if (!fin) {
        cout << "cannot find trajectory file at " << trajectory_file << endl;
        return 1;
    }

    while (!fin.eof()) {
        double time, tx, ty, tz, qx, qy, qz, qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
        Twr.pretranslate(Vector3d(tx, ty, tz));
        poses.push_back(Twr);
    }
    cout << "read total " << poses.size() << " pose entries" << endl;

    // draw trajectory in pangolin
    DrawTrajectory(poses);
    return 0;
}

/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
    // create pangolin window and plot the trajectory
    /*
     * 接下来，我们使用CreateWindowAndBind命令创建了一个视窗对象，
     * 函数的入口的参数依次为视窗的名称、宽度和高度，
     * 该命令类似于OpenCV中的namedWindow，即创建一个用于显示的窗体。
     */
    // 创建名称为“Trajectory Viewer”的GUI窗口，尺寸为640×640
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    //启动深度测试。同时，我们启动了深度测试功能，
    // 该功能会使得pangolin只会绘制朝向镜头的那一面像素点，避免容易混淆的透视关系出现，因此在任何3D可视化中都应该开启该功能。
    glEnable(GL_DEPTH_TEST);

    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    // 创建一个观察相机视图
    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );
    // 创建一个观察相机视图
    // ProjectMatrix(int w, int h, int fu, int fv, int cu, int cv, int znear, int zfar)
    //      参数依次为观察相机的图像宽度、高度、4个内参以及最近和最远视距
    // ModelViewLookAt(double x, double y, double z,double lx, double ly, double lz, AxisDirection Up)
    //      参数依次为相机所在的位置，以及相机所看的视点位置(一般会设置在原点)

    //在完成视窗的创建后，我们需要在视窗中“放置”一个摄像机（注意这里的摄像机是用于观察的摄像机而非SLAM中的相机），
    // 我们需要给出摄像机的内参矩阵ProjectionMatrix从而在我们对摄像机进行交互操作时，
    // Pangolin会自动根据内参矩阵完成对应的透视变换。
    // 此外，我们还需要给出摄像机初始时刻所处的位置，摄像机的视点位置（即摄像机的光轴朝向哪一个点）以及摄像机的本身哪一轴朝上。

    // 创建交互视图
    //pangolin::Handler3D handler(s_cam); //交互相机视图句柄
    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));//.SetHandler(&handler);
    //接下来我们需要创建一个交互式视图（view）用于显示上一步摄像机所“拍摄”到的内容，这一步类似于OpenGL中的viewport处理。
    // setBounds()函数前四个参数依次表示视图在视窗中的范围（下、上、左、右），可以采用相对坐标（0~1）以及绝对坐标（使用Attach对象）。


    while (pangolin::ShouldQuit() == false) {
        // 清空颜色和深度缓存
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);//激活
        //在完成了上述所有准备工作之后，我们就可以开始绘制我们需要的图形了，
        // 首先我们使用glclear命令分别清空色彩缓存和深度缓存
        // 并激活之前设定好的视窗对象（否则视窗内会保留上一帧的图形，这种“多重曝光”效果通常并不是我们需要的）


        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);//设定背景的颜色，此处为白色

        glLineWidth(2);//定义其中线条的宽度
        for (size_t i = 0; i < poses.size(); i++) {
            // 画每个位姿的三个坐标轴
            Vector3d Ow = poses[i].translation();
            Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
            Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
            Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
            glBegin(GL_LINES);
            glColor3f(1.0, 0.0, 0.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Xw[0], Xw[1], Xw[2]);
            glColor3f(0.0, 1.0, 0.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Yw[0], Yw[1], Yw[2]);
            glColor3f(0.0, 0.0, 1.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Zw[0], Zw[1], Zw[2]);
            glEnd();
        }
        // 画出连线
        for (size_t i = 0; i < poses.size(); i++) {
            glColor3f(0.0, 0.0, 0.0);
            glBegin(GL_LINES);
            auto p1 = poses[i], p2 = poses[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        //在绘制完成后，需要使用FinishFrame命令刷新视窗。
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
}

3.3 CMakeLists.txt

cmake_minimum_required(VERSION 3.0)

project(DrawTrajectory)

include_directories("/usr/include/eigen3")

find_package(Pangolin REQUIRED)
find_package(Sophus REQUIRED)

include_directories(${Pangolin_INCLUDE_DIRS})
include_directories(${Sophus_INCLUDE_DIRS})

add_executable(draw_trajectory src/draw_trajectory.cpp)
target_link_libraries(draw_trajectory ${Pangolin_LIBRARIES} ${Sophus_LIBRARIES} fmt)

3.4 输出结果

• 参考【Eigen】【Eigen实践】【Eigen的使用学习记录】的5 显示运动轨迹

4 评估轨迹的误差

4.1 前言

• 代码运行需要两个txt文件，自取：链接：https://pan.baidu.com/s/1XP1vNoZ43qOx2LNz6IWujA
  提取码：edif
  –来自百度网盘超级会员V3的分享

4.2 轨迹的误差指标介绍（slam14第二版课本p89）

4.2.1 绝对轨迹误差（Absolute Trajectory Error，ATE）

形如
$${\mathrm{ATE}}_{\mathrm{all}}=\sqrt{\frac{1}{N}\sum _{i=1}^N{\Vert \log {\left({\mathbf{T}}_{\mathrm{gt},i}^{-1}{\mathbf{T}}_{\mathrm{esti},i}\right)}^{\vee }\Vert }_2^2}$$

• 这实际上也是每个位姿李代数的均方根误差(Root-Mean-Squared Error, RMSE)。这种误差可以刻画这两条轨迹的旋转和平移误差。
• 同时也有文献仅考虑平移误差，从而可以定义绝对平移误差(Average Translational Error):
  $$\mathrm{A}T{\mathrm{E}}_{\mathrm{trans}}=\sqrt{\frac{1}{N}\sum _{i=1}^N{\Vert \mathrm{trans}\left({\mathbf{T}}_{\mathrm{gt},i}^{-1}{\mathbf{T}}_{\mathrm{esti},i}\right)\Vert }_2^2}$$
• 其中trans表示取括号内部变量的平移部分。因为从整条轨迹上看，旋转出现误差后，随后的轨迹在平移上也会出现误差，所以两种指标在世纪中都适用。

4.2.2 相对位姿误差（Relative Pose Error，RPE）

• 考虑i时刻i+detat时刻的运动，那么 对位姿误差（Relative Pose Error，RPE）可以定义为：
  $${\mathrm{RPE}}_{\mathrm{all}}=\sqrt{{\frac{1}{N-\Delta t}\sum _{i=1}^{N-\Delta t}\Vert \log {\left(\left({\mathbf{T}}_{\mathrm{gt},i}^{-1}{\mathbf{T}}_{\mathrm{gt},i+\Delta t}\right)\right)}^{-1}\left({\mathbf{T}}_{\mathrm{est},i}^{-1}{\mathbf{T}}_{\mathrm{est},i+\Delta t}\right))}^{\vee }{\Vert }_2^2}$$

• 同样的，也可以只取平移部分
  $${\mathrm{RPE}}_{\text{trans }}=\sqrt{\frac{1}{N-\Delta t}\sum _{i=1}^{N-\Delta t}\Vert \mathrm{trans}{\left(\left(T_{\mathrm{gt},i}^{-1}{\mathbf{T}}_{\mathrm{gt},i+\Delta t}\right)\right)}^{-1}\left({\mathbf{T}}_{\text{esti },i}^{-1}{\mathbf{T}}_{\text{esti, },i+\Delta t}\right)){\Vert }_2^2}$$

4.3 trajectoryError.cpp

#include 
#include 
#include 
#include 
#include 

using namespace Sophus;
using namespace std;

string groundtruth_file = "../src/groundtruth.txt"; //这个路径是相对于build下的可执行文件trajectoryError而言的
string estimated_file = "../src/estimated.txt";

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;

void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti);

TrajectoryType ReadTrajectory(const string &path);

int main(int argc, char **argv) {
  TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
  TrajectoryType estimated = ReadTrajectory(estimated_file);
  assert(!groundtruth.empty() && !estimated.empty());
  assert(groundtruth.size() == estimated.size());

  // compute rmse
  double rmse = 0;
  for (size_t i = 0; i < estimated.size(); i++) {
    Sophus::SE3d p1 = estimated[i], p2 = groundtruth[i];
    double error = (p2.inverse() * p1).log().norm();
    rmse += error * error;
  }
  rmse = rmse / double(estimated.size());
  rmse = sqrt(rmse);
  cout << "RMSE = " << rmse << endl;

  DrawTrajectory(groundtruth, estimated);
  return 0;
}

TrajectoryType ReadTrajectory(const string &path) {
  ifstream fin(path);
  TrajectoryType trajectory;
  if (!fin) {
    cerr << "trajectory " << path << " not found." << endl;
    return trajectory;
  }

  while (!fin.eof()) {
    double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Sophus::SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
    trajectory.push_back(p1);
  }
  return trajectory;
}

void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
      pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
      pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
      .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
      .SetHandler(new pangolin::Handler3D(s_cam));


  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

    glLineWidth(2);
    for (size_t i = 0; i < gt.size() - 1; i++) {
      glColor3f(0.0f, 0.0f, 1.0f);  // blue for ground truth
      glBegin(GL_LINES);
      auto p1 = gt[i], p2 = gt[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }

    for (size_t i = 0; i < esti.size() - 1; i++) {
      glColor3f(1.0f, 0.0f, 0.0f);  // red for estimated
      glBegin(GL_LINES);
      auto p1 = esti[i], p2 = esti[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }

}

4.4 CMakeLists.txt

cmake_minimum_required(VERSION 3.0)

project(TrajectoryError)

find_package(Pangolin REQUIRED)
find_package(Sophus REQUIRED)

include_directories(${Pangolin_INCLUDE_DIRS})
include_directories(${Sophus_INCLUDE_DIRS})

add_executable(trajectoryError src/trajectoryError.cpp)
target_link_libraries(trajectoryError ${Pangolin_LIBRARIES} ${Sophus_LIBRARIES} fmt)

4.5 输出

/home/bupo/my_study/slam14/slam14_my/cap4/TrajectoryError/cmake-build-debug/trajectoryError
RMSE = 2.20727