视觉SLAM十四讲笔记-4-3

四夕小一冰

已于 2022-05-22 16:47:17 修改

阅读量347

点赞数

分类专栏：视觉slam十四讲笔记文章标签：经验分享

于 2022-05-03 11:04:06 首次发布

本文链接：https://blog.csdn.net/qq_35588369/article/details/124541029

版权

视觉slam十四讲笔记专栏收录该内容

34 篇文章 10 订阅

订阅专栏

视觉SLAM十四讲笔记-4-3

文章目录

视觉SLAM十四讲笔记-4-3

4.4 实践Sophus

4.4.1 安装过程记录

参考链接 link，首先下载 release版本的 fmt (链接： link)

cd fmt
mkdir build
cd build
cmake ..
make
sudo make install

使用上述命令进行安装。
然后安装 Sophus,

git clone https://github.com/strasdat/Sophus.git
cd Sophus/

mkdir build
cd build
cmake ..
make
sudo make install

在运行到 $\;..$ ，报错：
请添加图片描述
发现目前通过终端直接安装的 eigen 版本为 3.2.92，而 Sophus　至少要求 eigen 版本为 3.3。
因此需要重新安装 eigen。
首先先卸载原有 eigen 版本：

//查看当前版本
pkg-config --modversion eigen3
//查看eigen3相关位置
sudo updatedb
locate eigen3
//删除eigen3相关文件
sudo rm -rf /usr/include/eigen3
sudo rm -rf /usr/lib/cmake/eigen3
sudo rm -rf /usr/local/include/eigen3
sudo rm -rf /usr/share/doc/libeigen3-dev 
sudo rm -rf /usr/local/share/pkgconfig/eigen3.pc /usr/share/pkgconfig/eigen3.pc /var/lib/dpkg/info/libeigen3-dev.list /var/lib/dpkg/info/libeigen3-dev.md5sums
sudo rm -rf /usr/local/lib/pkgconfig/eigen3.pc
sudo rm -rf /usr/local/share/eigen3
//刷新查看是否删除彻底
sudo updatedb
locate eigen3
pkg-config --modversion eigen3
//如果正确删除会显示　No package 'eigen3' found

然后安装新的版本，这里去官网下载 3.3.7 版本，链接: link
然后进入文件夹中使用下面 cmake 命令：

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

进行安装。
安装完后默认安装在 /usr/local/include 目录下，
请添加图片描述
而当时在终端下直接用命令安装的默认是安装在 /usr/include　目录下，这里直接复制过去：

sudo cp -r /usr/local/include/eigen3 /usr/include

安装完成

//检测当前版本
pkg-config --modversion eigen3
//输出3.3.7

运行以前依赖 eigen 和 Pangolin 的工程都可以正常运行。
下面继续安装 Sophus。

git clone https://github.com/strasdat/Sophus.git
cd Sophus/

mkdir build
cd build
cmake ..
make
sudo make install

请添加图片描述
到此，Sophus 安装成功。
总结步骤：
1.安装3.3版本以上的 eigen;
2.安装release版本的 fmt ;
3.安装带模板的Sophus库。

4.4.2 Sophus的基本用法

Eigen 库是一个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。Eigen 库提供了几何模块，但没有提供李代数的支持。一个较好的李群和李代数的库是Sophus 库，它很好的支持了 SO(3),so(3),SE(3)和 se(3)。Sophus 库是基于 Eigen 基础上开发的，继承了 Eigen 库中的定义的各个类。因此在使用 Eigen 库中的类时，既可以使用 Eigen 命名空间，也可以使用 Sophus 命名空间。

Eigen：：Matrix3d和Sophus：：Matrix3d
Eigen：：Vector3d和Sophus：：Vector3d

此外，为了方便说明SE(4)和se(4)，Sophus库还typedef了Vector4d、Matrix4d、Vector6d和Matrix6d等，即：

Sophus：：Vector4d
Sophus：：Matrix4d
Sophus：：Vector6d
Sophus：：Matrix6d

参考链接：link
在这里插入图片描述
下面这个例子简单的演示了 Sophus 库中的 SO(3) 和 SE(3) 运算：

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

//本程序演示sophus的基本用法
int main(int argc, char **argv)
{
    //沿Z轴旋转0度的旋转矩阵
    //先使用AngleAxis构造旋转向量，然后使用toRotationMatrix()转换为旋转矩阵
    Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
    //或者四元数
    Quaterniond q(R);
    Sophus::SO3d SO3_R(R); // Sophus::SD3d可以直接从旋转矩阵构造
    Sophus::SO3d SO3_q(q); //也可以通过四元数构造，两者是等价的
    cout << "SO(3) from matrix:\n"
         << SO3_R.matrix() << endl;
    cout << "SO(3) from quaternion:\n"
         << SO3_q.matrix() << endl;

    //使用对数映射获得它的李代数，程序直接获取旋转矩阵对应的李代数
    Vector3d so3 = SO3_R.log();
    cout << "so = " << 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;

    //增量扰动模型的更新
    Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
    Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
    //用matrix()转换为矩阵
    cout << "SO3 updated = \n"
         << SO3_updated.matrix() << endl;

    cout << "***************************************************" << endl;

    //对SE(3)操作大同小异
    Vector3d t(1, 0, 0);       //沿X轴平移１
    Sophus::SE3d SE3_Rt(R, t); //从R,t构造SE(3)
    Sophus::SE3d SE3_qt(q, t); //从q,t构造SE(3)
    cout << "SE(3) from R,t = \n"
         << SE3_Rt.matrix() << endl;
    cout << "SE(3) from q,t = \n"
         << SE3_qt.matrix() << endl;
    //李代数se(3)是一个六维向量，方便起见先typpedef一下
    typedef Eigen::Matrix<double, 6, 1> Vector6d;
    //使用对数映射获得它的李代数
    Vector6d se3 = SE3_Rt.log();
    cout << "se = " << se3.transpose() << endl;

    //观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后
    //同样地，有hat和vee两个算符
    // hat为向量到反对称矩阵
    cout << "set hat = \n"
         << Sophus::SE3d::hat(se3) << endl;
    //相对地，vee为反对称到向量
    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;
}

在 VS Code 中新建工程，这里不再介绍了，直接使用 link ，
这里的 CMakeLists.txt 中要添加 Sophus 库路径，这里使用 find_package 命令：
==find_package 命令是 cmake 提供的寻找某个库的头文件与库文件的指令。如果 cmake 能够找到它，就会提供头文件和库文件所在的目录的变量。==在 Sophus 这个例子中，就是 Sophus_INCLUDE_DIRS。基于模板的 Sophus 库和 Eigen 库一样，是仅包含头文件而没有源文件的。因此，该工程的 CMakeLists.txt 文件为：

cmake_minimum_required(VERSION 3.0)
project(USESOPHUS)

#在g++编译时，添加编译参数，比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")

#一定要加上这句话，加上这个生成的可执行文件才是可以Debug的，不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)

##在程序发布时将上述两个set设置语句变为下面两个语句：
#set(CMAKE_BUILD_TYPE "Release")
#set(CMAKE_CXX_FLAGS "-O3")

# 为使用 sophus，需要使用find_package命令找到它
find_package(Sophus REQUIRED)
include_directories( ${Sophus_INCLUDE_DIRS} )
# Eigen
include_directories("/usr/include/eigen3")
add_executable(useSophus useSophus.cpp)
target_link_libraries(useSophus Sophus::Sophus)

程序输出：

SO(3) from matrix:
2.22045e-16          -1           0
          1 2.22045e-16           0
          0           0           1
SO(3) from quaternion:
2.22045e-16          -1           0
          1 2.22045e-16           0
          0           0           1
so =      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 = 
          0          -1           0
          1           0     -0.0001
     0.0001 2.03288e-20           1
***************************************************
SE(3) from R,t = 
2.22045e-16          -1           0           1
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1
SE(3) from q,t = 
2.22045e-16          -1           0           1
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1
se =  0.785398 -0.785398         0         0         0    1.5708
set 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

4.4.3 例子：评估轨迹的误差

在实际工程中，经常需要评估一个算法的估计轨迹与真实轨迹的差异来评价算法的精度。真实轨迹往往通过更高精度的系统获得，而估计轨迹是由待评价的算法计算得到的。本节考虑如何计算两条轨迹的误差。考虑一条估计轨迹 $T_{esti,i}$ 和真实轨迹 $T_{gt,i}$ ，其中 $i = 1, 2, . . ., N$ 。
可以定义一些误差指标来描述它们之间的差别。
误差指标可以有很多种，常见的有绝对轨迹误差(Absolute Trajectory Error, ATE):
$ATE_{all} = \sqrt{\frac{1}{N}\sum_{i=1}^{N}\left \| log(T_{gt,i}^{-1}T_{esti,i})^\vee \right \| _{2}^2 }$
这实际上是每个位姿李代数的均方根误差(Root-Mean-Squared, RMSE)。这种误差可以刻画两条轨迹的旋转和平移误差。同时，也有文献只考虑绝对平移误差(Aveage Translational Error):
$ATE_{trans} = \sqrt{\frac{1}{N}\sum_{i=1}^{N}\left \| trans(T_{gt,i}^{-1}T_{esti,i}) \right \| _{2}^2 }$
其中 $t r a n s$ 表示括号内部变量的平移部分。因为从整条轨迹上看，旋转出现误差后，随后的轨迹在平移上也会出现误差，所以两种指标在实际中都适用。
同时，也可以定义相对的误差，例如，考虑 $i$ 时刻到 $i+\Delta t$ 时刻的运动，那么相对位姿误差(Relative Pose Error, RPE)可定义为：
$RPE_{all} = \sqrt{\frac{1}{N-\Delta t}\sum_{i=1}^{N-\Delta t}\left \| log((T_{gt,i}^{-1}T_{gt,i+\Delta t})^{-1}(T_{esti,i}^{-1}T_{esti,i+ \Delta t}))^\vee \right \|_{2}^2 }$
同样，也可只取平移部分：
$RPE_{all} = \sqrt{\frac{1}{N-\Delta t}\sum_{i=1}^{N-\Delta t}\left \| trans((T_{gt,i}^{-1}T_{gt,i+\Delta t})^{-1}(T_{esti,i}^{-1}T_{esti,i+ \Delta t})) \right \|_{2}^2 }$
利用 Sophus 库，很容易实现这部分计算。
首先，创建工程文件夹，并在该文件夹下打开 VS Code:

mkdir trajectoryError
cd trajectoryError/
code .

在工程下新建 trajectoryError.cpp, CMakeLists.txt文件，build文件夹，以及luanch.json, tasks.json，各个文件内容如下：

//launch.json
{
    // Use IntelliSense to learn about possible attributes.
    // Hover to view descriptions of existing attributes.
    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
    "version": "0.2.0",
    "configurations": [
        {
            "name": "g++ - 生成和调试活动文件",
            "type": "cppdbg",
            "request":"launch",
            "program":"${workspaceFolder}/build/trajectoryError",
            "args": [],
            "stopAtEntry": false,
            "cwd": "${workspaceFolder}",
            "environment": [],
            "externalConsole": false,
            "MIMode": "gdb",
            "setupCommands": [
                {
                    "description": "为 gdb 启动整齐打印",
                    "text": "-enable-pretty-printing",
                    "ignoreFailures": true
                }
            ],
            "preLaunchTask": "Build",
            "miDebuggerPath": "/usr/bin/gdb"
        }
    ]
}

//tasks.json
{
	"version": "2.0.0",
	"options":{
		"cwd": "${workspaceFolder}/build"   //指明在哪个文件夹下做下面这些指令
	},
	"tasks": [
		{
			"type": "shell",
			"label": "cmake",   //label就是这个task的名字,这个task的名字叫cmake
			"command": "cmake", //command就是要执行什么命令,这个task要执行的任务是cmake
			"args":[
				".."
			]
		},
		{
			"label": "make",  //这个task的名字叫make
			"group": {
				"kind": "build",
				"isDefault": true
			},
			"command": "make",  //这个task要执行的任务是make
			"args": [

			]
		},
		{
			"label": "Build",
			"dependsOrder": "sequence", //按列出的顺序执行任务依赖项
			"dependsOn":[				//这个label依赖于上面两个label
				"cmake",
				"make"
			]
		}
	]
}

//CMakeLists.txt
cmake_minimum_required(VERSION 3.0)
project(USESOPHUS)

#在g++编译时，添加编译参数，比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")

#一定要加上这句话，加上这个生成的可执行文件才是可以Debug的，不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)

##在程序发布时将上述两个set设置语句变为下面两个语句：
#set(CMAKE_BUILD_TYPE "Release")
#set(CMAKE_CXX_FLAGS "-O3")

# 为使用 sophus，需要使用find_package命令找到它
find_package(Sophus REQUIRED)
include_directories( ${Sophus_INCLUDE_DIRS} )
# Eigen
include_directories("/usr/include/eigen3")

#添加Pangolin依赖
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS}) 

add_executable(trajectoryError trajectoryError.cpp)
target_link_libraries(trajectoryError Sophus::Sophus)
# target_link_libraries( useSophus ${Sophus_LIBRARIES} fmt )
target_link_libraries(trajectoryError ${Pangolin_LIBRARIES})

//trajectoryError.cpp
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>

using namespace Sophus;
using namespace std;

string groundtruth_file = "./groundtruth.txt";
string estimated_file = "./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
  }

}

运行结果：
请添加图片描述

RMSE = 2.20728

4.5 相似变换群与李代数

这节介绍在单目视觉中使用的相似变换群 Sim(3) 以及对应的李代数 sim(3)，
单目具有尺度不确定性，如果在单目 SLAM 中使用 SE(3) 表示位姿，那么由于尺度不确定性与尺度漂移，整个 SLAM 过程中的尺度会发生变化，这在 SE(3) 中未能体现出来．因此，在单目情况下一般会显式地把尺度因子表达出来．
用数学语言表示，对于位于空间的点 $p$ ，在相机坐标系下要经过一个相似变换，而非欧氏变化：
$p'=\begin{bmatrix} sR & t\\ 0^T &1 \end{bmatrix}p = sRp+t$
在相似变换中，把尺度 $s$ 表示出来了．它同时作用在 $p$ 的三个坐标之上，对 $p$ 进行了缩放．与 SO(3), SE(3) 相似，相似变换也对矩阵乘法构成群，称为相似变换群 Sim(3)．
$Sim(3)=\left \{ S = \begin{bmatrix} sR & t\\ 0^T &1 \end{bmatrix} \in R^{4*4}\right \}$
同样地， Sim(3) 也有对应的李代数，指数映射，对数映射等．
李代数 sim(3) 元素是一个7维向量 $\zeta$ ．它的前６维与 se(3) 相同，最后多了一项 $\sigma$ ，
$\left \{ \zeta | \zeta = \begin{bmatrix} \rho \\ \phi \\ \sigma \end{bmatrix}\in R^7, \zeta ^ \wedge = \begin{bmatrix} \sigma I+\phi ^\wedge & \rho \\ 0^T &0 \end{bmatrix}\in R^{4*4} \right \}$
它比 se(3) 多了一项 $\sigma$ ，
关联 Sim(3) 和 sim(3) 的仍然是指数映射和对数映射．指数映射为：
$exp(\varsigma^\wedge )=\begin{bmatrix} e ^\sigma exp(\phi ^\wedge ) & J_s\rho \\ 0^T &1 \end{bmatrix}$
其中， $J_s$ 的形式为：
$J_s = \frac{e^\sigma -1}{\sigma} I + \frac{\sigma e^\sigma sin \theta + (1 - \sigma e^\sigma cos \theta)\theta}{\sigma^2+\theta^2}a^\wedge \\+ (\frac{e^\sigma - 1}{\sigma} - \frac{(e^\sigma cos \theta - 1)\sigma + (e^\sigma sin \theta) \theta }{\sigma^2+\theta^2} ) a^\wedge a^\wedge$
通过指数映射，能够找到李代数和李群的关系．对于李代数 $\zeta$ ,它与李群的对应关系为：
$e^\sigma ,R = exp(\phi^\wedge ),t = J_s\rho$
Sim(3) 的 BCH 近似与 SE(3) 是类似的．可以讨论一个点 $p$ 经过相似变换 $S p$ 后，相对于 $S$ 的导数．同样地存在微分模型和扰动模型，这里直接给出扰动模型的结果：
设在 $S p$ 左侧一个小扰动 $\zeta^\wedge)$ ，并求 $S p$ 对于扰动的导数．因为 $S p$ 是４维的齐次坐标， $\zeta$ 是7维向量，所以该导数应为4*7的雅克比矩阵，方便起见，记 Sp 的前３维组成向量为 q，那么：
$\frac{\partial Ｓp}{\partial \zeta } = \begin{bmatrix} I & -q^\wedge & q\\ 0^T & 0^T &0 \end{bmatrix}$

四夕小一冰

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
视觉SLAM十四讲笔记-4-3

视觉SLAM十四讲笔记-4-34.4 实践Sophus4.4.1 安装过程记录参考链接 link，首先下载 release版本的 fmt (链接： link)cd fmtmkdir buildcd buildcmake ..makesudo make install使用上述命令进行安装。然后安装 Sophus,git clone https://github.com/strasdat/Sophus.gitcd Sophus/mkdir buildcd buildcmake
复制链接

扫一扫