【视觉SLAM十四讲】【逐行代码带你解析】【适合纯小白 ps:因为我就是】（持续更新中）

前言

       大一大二东学学西学学了一大堆，这个暑假想往难一点的方向探索探索，以后想从事机器人行业，所以先从SLAM开始，希望这个暑假能把他更新完。
ps1.0：对于这个方向我也只是初学者，大家看的时候一定要带辩证的思维看，因为我也不敢说我写的这个内容是正确的，同时我也希望大家对我做出一些建议和反馈。
ps2.0：2030/8/15 这两天写到第六讲非线性优化得时候，着实对自己得智商产生了怀疑，这一章对于我来说真的是太难了，之前上概率论和线性代数得时候学校讲的太水了，这一章又有很多关于这部分的基础知识，现在我对于这个暑假能不能学完这本书已经产生怀疑了，但是不管多难，我都决心更新下去，把这本书啃透，啃完！！！！

       原书代码Github链接: link
       笔者代码Github链接: link

ch2 初识SLAM

2.1.什么是SLAM

       SLAM是指Simultaneous Localization and Mapping，翻译过来就是同时定位与地图构建模型，他解决了定位与地图构建的问题，说人话就是：
       1.我在什么地方——定位
       2.周围环境怎么样——建图

2.2经典视觉SLAM框架

       视觉SLAM的流程分为以下步骤：

2.2.1.传感器信息读取

2.2.2.前端视觉里程计

       这个就是读取了前后两张（两张只是一个假设）图片的数据，然后根据图片信息的变化估计出运动的轨迹，但是仅仅通过这种视觉里程计来估计轨迹，不可避免的就会产生误差，这个误差我们这里叫他飘移，而这种误差是累计性的，也就是估计的越多，误差就会越大，这里我们称之为累积飘移（Accumulating Drift），这将导致我们定位也不准，建图也不准，于是，我们引入了下面两种东西：后端优化和回环检测。

2.2.3.后端非线性优化

        我们原始数据的读取来自于各式各样的传感器，但是传感器的数据不可能完全精确，会有误差，这里我们称之为噪声，后端优化要考虑的问题，就是如何从这些带有噪声的数据中去估计整个系统的状态，以及这个状态估计的不确定性有多大，这里的状态包括机器人自身的轨迹，也包含地图。

        在视觉SLAM中，前端负责图像的特征提取与匹配等等，后端则主要是滤波和非线性优化算法

2.2.4.回环检测

        回环检测，又称闭环检测，主要解决位置估计随时间漂移的问题。与后端优化任务目标相同，但是使用的方法有所差异。

        回环检测与“定位”和“建图”二者都有密切的关系。事实上，我们认为，地图存在的主要意义是让机器人知晓自己到过的地方。为了实现回环检测，我们需要让机器人具有识别到过的场景的能力。它的实现手段有很多。

        视觉回环检测实质上是一种计算图像数据相似性的算法。由于图像的信息非常丰富，使得正确检测回环的难度降低了不少。

        在检测到回环之后，我们会把“A与B 是同一个点”这样的信息告诉后端优化算法。然后后端根据这些新的信息，把轨迹和地图调整到符合回环检测结果的样子。这样，如果我们有充分而且正确的回环检测，则可以消除累积误差，得到全局一致的轨迹和地图。

2.2.5.建图

        建图是指构建地图的过程。地图的形式随SLAM的应用场合而定，大体上分为度量地图与拓扑地图

        度量地图
        度量地图通常用稀疏和稠密分类，稀疏地图进行了一定程度上的抽象，稠密地图着重于建模所有看到的东西，定位时用稀疏路标地图就足够了，而导航往往需要稠密地图。

        拓扑地图
        相比于度量地图的精确性拓扑地图更强调地图元素之间的关系。拓扑地图是一个图(Graph).由节点和边组成，只考虑节点间的连通性，例如只关注 A、B 点是连通的，而不考虑如何从 A点到达 B点。它放松了地图对精确位置的需要，去掉了地图的细节，是一种更为紧凑的表达方式然而，拓扑地图不擅长表达具有复杂结构的地图。如何对地图进行分割，形成节点与边，又如何使用拓扑地图进行导航与路径规划，仍是有待研究的问题。

2.3.SLAM问题的数学表述

        第一个方程是运动方程，x_k是当前位置，x_k-1是前一时刻的位置，u_k是当前输入的数据，w_k是噪声。

        第二个方程是观测方程，z_k,j是观测数据，观测方程描述的是，当机器人在x_k位置上看到某个路标点y_j时，产生了一个观测数据z_k,j，v_k,j是这次观测里的噪声。由于观测所用的传感器形式更多，这里的观测数据z及观测方程h也有许多不同的形式。

        有了这两个方程，我们就成功节=的将SLAM“问题建模成了一个状态估计问题。

2.4.ch2的实践

        书里这部分讲了如何安装ubuntu，并配置c++环境，我对这部分比较熟悉，就不写了。

ch3 三维空间刚体运动

        这部分花的时间还是挺多的，如果上天再给我一次机会，我一定在大二上学期认认真真的好好的学线代。

3.1.旋转矩阵

3.1.1.点、向量和坐标系

        内积：

        外积：

3.1.2.坐标系间的欧氏变换

        两个坐标系间的运动由一个旋转加一个平移组成，这种运动我们称之为刚体运动。

        我们说这两个坐标系之间，相差了一个欧氏变换。
        我们从两个方面来学习这个破玩意儿。
        旋转
        同一个向量a在两个坐标系下的坐标为(a₁,a₂,a₃)^T和(a₁^’,a₂^’,a₃^’)^T，这两个坐标的基向量为(e₁,e₂,e₃)和(e₁^’,e₂^’,e₃^’)，所以根据坐标的定义，有：

        对等式左右同时左乘

        得到了：

        中间的这个3X3的矩阵R，我们就称之为旋转矩阵，该矩阵的各个分量是两个坐标基系的内积，由于基向量长度为1，所以实际上是各基向量夹角的余弦值，所以这个矩阵也称方向余弦矩阵。

        旋转矩阵还是一个行列式为+1的正交矩阵，反之，行列式为+1的正交矩阵也是一个旋转矩阵。将n维旋转矩阵的集合定义如下：
        
        称之为特殊正交群。

        旋转矩阵的逆（即转置）描述了一个相反的旋转，有：
        
                （正交矩阵的性质：逆=转置）

        这里附上我丑丑的手写笔记（别笑话我）

        在欧氏变换中，除了旋转，还有一个东东：平移
        平移记作向量t，那么把旋转和平移合到一起，就有了：
                                                a₁ = R₁₂a₂+t₁₂                     式1

        至此，我们终于能描述坐标系的变换啦！！

3.1.3.变换矩阵与齐次坐标

        如果，我们参照上面得出的式子a₁ = R₁₂a₂+t₁₂，当我们要做两次变换时（例如a₃变到a₂再变到a₁），就有以下推导公式：

        a₂ = R₂₃a₃+t₂₃
        a₁ = R₁₂a₂+t₁₂
        联立得：a₁ = R₁₂(R₂₃a₃+t₂₃)+t₁₂

        这样子的形式在变换多次时就会显得很麻烦，所以我们引入齐次坐标和变换矩阵，改写式1得到：
                

        矩阵T我们称之为变换矩阵。
        这时两次变换（如上）就可以改写为a₁= T₂T₃a₃

        变换矩阵的特点：左上角为旋转矩阵，右上角为平移向量，左下角为0向量，右下角为1.这种矩阵又称之为特殊欧氏群。
        
        与SO(3)一样，求解该矩阵的逆表示一个反向的变换。

        

3.2实践部分：Eigen3

        Eigen是一个用于求解矩阵问题的C++库，直接在终端通过apt去下载这个库。

sudo apt install libeigen3-dev

        如果没有定位到软件包那大概率就是你没有及时更新软件源，还是终端输入

sudo apt update
sudo apt upgrade

        如果失败那大概率是你没有换源，关于换源网络上的教程很多，这里给一个链接大家去看看就会了，比较简单这里就不写了。
链接: link

        下载成功过后这个库默认是安装在路径/usr/include/eigen3下的，下载完可以去验证一下。

        与其他的C++库比较，他有一个比较特殊的地方，就是这个库是由头文件组成的，因此在使用是，你并不需要在CMakeLists.txt中去链接库温文件（因为他就没有库文件），下面写一段代码来简单试用一下这个库。

#include <iostream>
//用于定时的库
#include <ctime>
//Eigen的核心部分
#include <Eigen/Core>
//Eigen用于矩阵运算的部分
#include <Eigen/Dense>
//使用命名空间
using namespace std;
using namespace Eigen;
//矩阵最大长度，后面用到的
#define MATRIX_SIZE 100

int main(int argc,char **argv){
	//如果不在前面使用命名空间的话这里应该谢写为Eigen::Matrix..，float是声明了你创建了的矩阵的数据类型，后面依次是行数和列数。
    Matrix<float,2,3> matrix_23;
    //这个比较特殊，这个库里面有一些给你写好的矩阵类型，例如Vector3d就是double类型的三维向量，本质上他也是Eigen::Matrix..，只不过封装的更好了。
    Vector3d v_3d;
    Matrix<float,3,1> vd_3d;
    //这个代表3X3的double型矩阵，Matrix3d::Zero()是Eigen中的一个函数，意思是将矩阵所有元素的置都赋值为0.
    Matrix3d matrix_33 = Matrix3d::Zero();
    //Dynamic是动态的意思，也可以用-1代替，Matrix<double,Dynamic,Dynamic>就是未知行列数的double型矩阵（动态的矩阵）。在我们定义矩阵的时候，如果矩阵的类型我们是已知的，那么我们就尽量在定义的时候写清楚，这样的话程序在运行的时候效率会高很多。
    Matrix<double,Dynamic,Dynamic> matrix_dynamic;
    //MatrixXd就是未知行列数的double型方阵。
    MatrixXd matrix_x;
    //这是给矩阵赋值。
    matrix_23 << 1,2,3,4,5,6 ;
    cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << "\n\n" <<endl;

	//这里是通过遍历打印出矩阵里的所有元素，访问矩阵的时候用（）去访问
    cout << "print matrix 2x3:" << endl;
    for (int i =0 ;i<2 ;i++ ){
        for (int j=0 ;j<3;j++){
            cout << matrix_23(i,j) << "\t";
        }
        cout <<endl;
    }

    v_3d << 3,2,1;
    vd_3d << 4,5,6;
	
	//Eigen库定义的矩阵在运算时必须严格数据类型，不同的数据类型之间不能进行计算，当我们想让不同数据类型的矩阵进行运算的时候需要用.cast<type>()对矩阵进行类型转换。
    Matrix<double,2,1> result = matrix_23.cast<double>() * v_3d;
    cout << "[1,2,3;4,5,6] * [3;2;1] = \n" << result <<"\n\n" <<endl;

	//这里的赋值是给矩阵里的所有元素都赋值一个随机数。
    matrix_33 = Matrix3d::Random();
    //打印矩阵
    cout << "Random matrix :\n" << matrix_33 <<"\n\n" <<endl;
    //打印矩阵的转置
    cout << "Transpose : \n" << matrix_33.transpose() << "\n\n" <<endl;
    //打印矩阵的所有元素的和
    cout << "sum: \n" << matrix_33.sum() <<"\n\n" <<endl;
    //打印矩阵的迹（也就是主对角线所有元素的和）
    cout << "trace: \n" << matrix_33.trace() << "\n\n" <<endl;
    //打印矩阵的逆
    cout << "inverse: \n" << matrix_33.inverse() << "\n\n" <<endl;
    //打印矩阵的行列式的值
    cout << "det: \n" << matrix_33.determinant() << "\n\n" <<endl;


	//这里是求解矩阵的特征值和特征向量，eigen_solve(matrix_33.transpose() * matrix_33是为了保证对角化成功。
    SelfAdjointEigenSolver<Matrix3d> eigen_solve(matrix_33.transpose() * matrix_33);
    //打印矩阵的特征值
    cout << "Eigen value: \n" << eigen_solve.eigenvalues() <<"\n\n" << endl;
    //打印矩阵的特征向量
    cout << "Eigen vector: \n" << eigen_solve.eigenvectors() << "\n\n" <<endl;


	//这里是一个求解方程的例子，求解的方程为matrix_NN * X = v_Nd
	//我们通过三种方法来求解，并计算一下求解时间
    Matrix<double,MATRIX_SIZE,MATRIX_SIZE> matrix_NN = MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    matrix_NN = matrix_NN * matrix_NN.transpose();
    Matrix<double, MATRIX_SIZE,1> v_Nd = MatrixXd::Random(MATRIX_SIZE,1);
	
	//记录当前的时间，储存在time_stt 里
    clock_t time_stt = clock();
    //利用直接求逆的方法求X
    Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse() * v_Nd;
    //进行计时    ps：这里的CLOCKS_PER_SEC的意思是你计算机一秒计时的周期数，我刚开始也不知带这个是什么东东，如果不知道我在说啥那就去搜一下，下面我也会给出链接
    cout << "time of normal inverse is :"
         << 1000 * (clock()-time_stt) / (double) CLOCKS_PER_SEC
         << "ms"
         << endl;
    cout << "x = " << x.transpose() << "\n\n" <<endl;

    time_stt = clock();
    //利用QR分解法求X         （鬼知道这是什么东西，没有去深入了解，感觉应该也不用深入了解是怎么分解的，如果我学到后面发现真需要搞懂，我会回来补坑的）
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time of Qr decomposition is :"
         << 1000 * (clock()-time_stt) / (double) CLOCKS_PER_SEC
         << "ms"
         << endl;
    cout << "x = " << x.transpose() << "\n\n" <<endl;

    time_stt = clock();
    //利用cholesky分解法求X   （鬼知道这是什么东西，没有去深入了解，感觉应该也不用深入了解是怎么分解的，如果我学到后面发现真需要搞懂，我会回来补坑的）
    x = matrix_NN.ldlt().solve(v_Nd);
    cout << "time of ldlt decomposition is :"
         << 1000 * (clock()-time_stt) / (double) CLOCKS_PER_SEC
         << "ms"
         << endl;
    cout << "x = " << x.transpose() << "\n\n" <<endl;

	//书里说在QR分解法是最快的，试了一下确实如此
    return 0;

}

当然，代码有了，编译的时候也要记得去CmakeLists.txt里面添加一下这个头文件的路径

include_directories("/usr/include/eigen3")

完整的CMakeLists.txt文件如下：

cmake_minimum_required( VERSION 2.8 )

project( useEigen )

set( CMAKE_BUILD_TYPE "Release")

add_executable( main /home/psj/Desktop/slam_study/ch3/src/eigenMatrix.cpp)

include_directories("/usr/include/eigen3")

后面我的学习代码可能会同步在github上，现在先不搞这个。

终端打印出来的结果：

matrix 2x3 from 1 to 6: 
1 2 3
4 5 6
print matrix 2x3:
1       2       3
4       5       6
[1,2,3;4,5,6] * [3;2;1] = 
10
28

Random matrix :
 0.680375   0.59688 -0.329554
-0.211234  0.823295  0.536459
 0.566198 -0.604897 -0.444451
Transpose : 
 0.680375 -0.211234  0.566198
  0.59688  0.823295 -0.604897
-0.329554  0.536459 -0.444451
sum: 
1.61307
trace: 
1.05922
inverse: 
-0.198521   2.22739    2.8357
  1.00605 -0.555135  -1.41603
 -1.62213   3.59308   3.28973
det: 
0.208598
Eigen value: 
0.0242899
 0.992154
  1.80558
Eigen vector: 
-0.549013 -0.735943  0.396198
 0.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998

time of normal inverse is :0.11ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734


time of Qr decomposition is :0.092ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734


time of ldlt decomposition is :0.028ms
x = -55.7896 -298.793  130.113 -388.455 -159.312  160.654 -40.0416 -193.561  155.844  181.144  185.125 -62.7786  19.8333 -30.8772 -200.746  55.8385 -206.604  26.3559 -14.6789  122.719 -221.449   26.233  -318.95 -78.6931  50.1446  87.1986 -194.922  132.319  -171.78 -4.19736   11.876 -171.779  48.3047  84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237  28.9419  111.421  92.1237 -288.248 -23.3478  -275.22 -292.062  -92.698  5.96847 -93.6244  109.734

3.3.旋转向量与欧拉角

3.3.1.旋转向量（或轴角/角轴，Axis-Angle）

        So(3)的旋转矩阵有九个量，九个量表示一个旋转（一次旋转只有三个自由度），这种表达方式是冗余的，同理，变换矩阵用16个量表达了6个自由度的变换，也是冗余的，所以我们引入更加紧凑的表达——旋转向量。

        任何旋转都可以用一个旋转轴和一个旋转角来表示，如图：

        这个向量我们就称之为旋转向量。

        旋转向量也可以装换为旋转矩阵，用罗德里格斯公式：

        其中R为旋转矩阵，西塔为旋转的角度，n为旋转轴。

        反之，我们也可以从旋转矩阵求出旋转向量，这个也有公式：

        解这些式子就得出了旋转向量的两个值：旋转角（西塔）和旋转轴（n）。

        转轴n是矩阵R特征值1对应的特征向量。

3.3.2.欧拉角（euler angles）

        还有另外一种表示旋转的就是欧拉角，欧拉角使用了三个分离的转角，大部分领域在使用欧拉角的时候都有各自的坐标方向和顺序上的习惯。

        欧拉角里面比较常用的一种，便是使用“偏航-俯仰-滚转”（yaw-pitch-roll)三个角度来描述一个旋转。他等价于ZYX轴的旋转。

        欧拉角有一个重大的缺点，就是会遇到著名的万向锁问题，这被称之为奇异性问题，理论上证明，只要想用三个实数来表达三维旋转，都会不可避免的遇到奇异性问题，因此我们很少在SLAM程序中直接使用欧拉角表示姿态。同样不会在滤波或优化中使用欧拉角表示旋转，因为它具有奇异性。不过当我们验证自己的算法时，转换成欧拉角能够帮你快速分辨结果是否正确。某些主体主要为2D运动的场合，我们也可以把旋转分解为三个欧拉角，然后把其中一个（例如偏航角）拿出来作为定位信息输出。旋转向量也具有奇异性。

        万像锁问题不太好理解，建议大家去搜个视屏看看，能很直观的一下子就了解了。

3.4.四元数

3.4.1.四元数的定义

        上面我们知道了旋转矩阵跟旋转向量还有欧拉角都带有奇异性，所以我们引入四元数。用四元数来表达三维空间旋转时它既是紧凑的，也没有奇异性。它是一种类似于复数的代数。但是四元数也有缺点，它不够直观，而且运算稍微比较复杂。

        想要了解什么是四元数，我们得先了解复平面，下面给出了一个复平面的学习链接。大家如果不清楚什么是复平面的话，可以去看一下：
        链接: link
        以下是我学习到这里的时候做的一点小笔记（不许嫌弃我字丑！！！）：

        下面开始讲四元数：

        一个四元数q拥有一个实部和三个虚部

        其中i,j,k为四元数的三个虚部，这三个虚部满足以下关系式：

        它类似于三维空间中的差积。

        有时人们也用一个标量和一个向量来表达四元数：

        在这里，s称为四元数的实部，而v称为它的虚部，如果一个四元数的虚部为零，则称为实四元数，反之。若它的实部为零，则称为虚四元数。

        我们可以用单位四元数表示三维空间中任意一个旋转。

3.4.2.四元数的运算（这一部分直接用了书上的截图）

        四元数常见的有四则运算、共轭、求逆、数乘等。

3.4.3.四元数表示旋转（这一部分直接用了书上的截图）

3.4.4.四元数表示的旋转与其他表示方式的转换

        看过一遍知道怎么回事就行了，因为在我们写代码的时候，很多转换都给我们封装好了。

        四元数相比于角轴、欧拉角的优势：紧凑、无奇异性

3.5.相似、仿射、射影变换

（这部分书里标了星号，本人只是看了一遍，没有去细究他就往下了，如果学到后面发现他用处很大，那我再回来补坑。）

3.6.实践：Eigen几何模块

3.6.1.Eigen几何模块的数据演示

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

int main(int argc ,char **argv){
	//Eigen/Geometry模块提供了各种旋转和平移的表示
	//3d旋转矩阵直接使用Matrix3d或Matrix3f，这里的Identity()函数将矩阵初始化为单位矩阵
    Matrix3d rotation_matrix = Matrix3d::Identity();
    //AngleAxisd（旋转向量），此句创建了一个绕着z轴旋转45度的旋转向量。其中，M_PI/4表示旋转角度，即π/4，Vector3d(0,0,1)表示旋转轴，即z轴的单位向量。
    AngleAxisd rotation_vector(M_PI / 4 , Vector3d(0,0,1));
    //将输出流中浮点数的小数部分保留3位，其余部分按默认方式处理。不懂的我后面给个链接嘻嘻
    cout.precision(3);
    //打印出这个旋转向量
    cout << "rotation_matrix = \n" << rotation_vector.matrix() << endl;
     
    //一条语句，将旋转向量直接转换为旋转矩阵，是不是很方便哈哈哈 
    rotation_matrix = rotation_vector.toRotationMatrix();
    //再打印出来
    cout << "rotation_matrix = \n" << rotation_matrix << endl;
	
	//这条语句定义了旋转前点v的坐标v（1，0，0）
    Vector3d v(1,0,0);
    //看不懂这一句的回去前面讲旋转向量那里再看看，旋转向量乘以原坐标得出旋转后的坐标（这是因为重载了运算符）
    Vector3d v_rotated = rotation_vector * v;
    cout << "(1,0,0) after rotation (by rotation_vector) = " << v_rotated.transpose() << endl;
	
	//旋转矩阵乘以原坐标得出旋转后的坐标（这是因为重载了运算符）
    v_rotated = rotation_matrix * v;
    cout << "(1,0,0) after rotation (by rotation_matrix) = " << v_rotated.transpose() << endl;

	//欧拉角：可以直接将旋转矩阵转换成欧拉角
	/*
	在Eigen库中，Matrix3d类和Quaterniond类都提供了eulerAngles()方法，用于将
	旋转矩阵或四元数转换为欧拉角（Euler angles）。其中，eulerAngles()方法接受
	三个参数，分别表示旋转顺序。在这个三个参数中，每个参数都是一个整数，用于表示绕
	哪个坐标轴旋转的角度。数值从0到2，分别代表x轴、y轴、z轴。例如，(2,1,0)表示先
	绕z轴旋转，再绕y轴旋转，最后绕x轴旋转。这个旋转顺序通常被称为“ZYX”旋转顺序。
	使用不同的旋转顺序，会得到不同的欧拉角表示。在机器人控制和导航中，常常需要根据
	具体的应用场景，选择合适的旋转顺序来描述物体的旋转姿态。
	*/
    Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);
    //打印出欧拉角（“偏航-俯仰-滚转”（yaw-pitch-roll)）
    cout << "yaw pitch roll = " << euler_angles.transpose() <<endl; 

	//欧氏变换矩阵用Eigen::Isomestry   
	//忘了什么是欧氏变换矩阵的回去看3.1.3
    Isometry3d T = Isometry3d::Identity();  //三维欧氏变换矩阵是一个4X4的矩阵。这里虽然为3d，但是实质上T是4X4的矩阵
    //还记得欧氏变换矩阵有旋转矩阵和平移向量吧
    T.rotate(rotation_vector);//这一句是赋值了旋转矩阵（按照rotation_vector进行旋转）
    T.pretranslate(Vector3d(1,3,4));//这一句是赋值了平移矩阵（把平移向量设置为1，3，4）
    //打印出来赋完值的旋转矩阵
    cout << "Transform Matrix = \n" << T.matrix() << endl;

	//用变换矩阵进行坐标变换
    Vector3d v_transformed = T * v;//相当于R*v+t
    //打印出变换好的
    cout << "v_transformed = " << v_transformed.matrix() << endl;

	//四元数

	//可以将旋转向量直接转换成相对应的四元数，反之亦然
    Quaterniond q = Quaterniond(rotation_vector);
    //打印出来，这里有一个要注意的地方，就是coeffs（）这个函数将这个四元数转换成了矩阵，并且顺序是（x，y，z，w）其中w为实部，前三者为虚部。
    cout << "quaternion from rotation vector = " << q.coeffs().transpose() << endl;
	
	//也可以将旋转矩阵赋值给他
    q = Quaterniond(rotation_matrix);
    cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
	
	//使用四元数旋转一个向量，使用重载了打乘法
    v_rotated = q * v;//这里的乘法是重载了的，数学上是 q * v * q的逆
    cout << "(1,0,0) after rotation (by quaternion) = " << v_rotated.transpose() << endl;
	
	//这里也给出了不使用重载的函数，用常规的数学运算来求出坐标，但是不建议，看起来就复杂的头皮发麻QAQ
    cout << "should be equal to " << (q * Quaterniond(0,1,0,0) * q.inverse()).coeffs().transpose() << endl;

    return 0;
}

        终端打印出来的结果：

rotation_matrix = 
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
rotation_matrix = 
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
(1,0,0) after rotation (by rotation_vector) = 0.707 0.707     0
(1,0,0) after rotation (by rotation_matrix) = 0.707 0.707     0
yaw pitch roll = 0.785    -0     0
Transform Matrix = 
 0.707 -0.707      0      1
 0.707  0.707      0      3
     0      0      1      4
     0      0      0      1
v_transformed = 1.71
3.71
   4
quaternion from rotation vector =     0     0 0.383 0.924
quaternion from rotation matrix =     0     0 0.383 0.924
(1,0,0) after rotation (by quaternion) = 0.707 0.707     0
should be equal to 0.707 0.707     0     0

关于cout.precision(3):
链接: link

        Eigen库里面一些定义好了的矩阵：

3.6.2.实际的坐标变换例子

        下面这个图片是书上的原例题：

以下是对这道题的解析：

        首先，题目中有三个坐标系，分别是世界、一号、二号（W、R₁、R₂），P_R1代表在R₁坐标系下的坐标，我们要把他转换成在R₂坐标系中的坐标，但是我们题目中对于一号和二号给出的四元数和平移向量是相对于世界坐标系的，我们能先把P_R1转换成P_W，也就是所求点相对于世界的坐标系：

                                                        P_W = T_W,R1 * P_R1

        进一步，我们就能求出所求点相对于世界的坐标：

                                                        P_R2 = T_R2,W * P_W

        综合这两式子：

                                                        P_R2 = T_R2,W * T_W,R1 * P_R1

        接下来是程序部分：

//include各种库
#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <algorithm>

using namespace std;
using namespace Eigen;

int main(int argc ,char **argv){
	
	//定义两个小萝卜的四元数并赋值
    Quaterniond q1(0.35,0.2,0.3,0.1),q2(-0.5,0.4,-0.1,0.2);
    //四元数归一化（！！！！！四元数使用前需要归一化）
    q1.normalize();
    q2.normalize();

	//定义两个小萝卜的平移向量
    Vector3d t1(0.3,0.1,0.1),t2(-0.1,0.5,0.3);

	//小萝卜一号看到的
    Vector3d p1(0.5,0,0.2);
	/*
		不要忘记了欧氏变换矩阵的形式
		| R, T |
		| 0, 1 |
	*/
	//定义两个欧氏变换矩阵并把四元数赋值给他们（也就是相当于初始化变换矩阵的旋转矩阵）【四元数表示旋转】
    Isometry3d T1w(q1),T2w(q2);

	/*
		当然，你也可以这么写
		//将四元数转换为旋转矩阵
		Matrix3d R1 = q1.toRotationMatrix();
		Matrix3d R2 = q2.toRotationMatrix();
		//将旋转矩阵赋值给欧氏变换矩阵
		Isometry3d T1w.rotate(R1);
		Isometry3d T2w.rotate(R2);
	*/

    //将平移向量给到变换矩阵
    T1w.pretranslate(t1);
    T2w.pretranslate(t2);
    //根据刚才推出来的公式进行计算
    //这里为什么要对T1w取逆再运算呢，如果理解不了这一个地方的建议回到3.1去复习一下
    Vector3d p2 = T2w*T1w.inverse() *p1;
    cout << endl << p2.transpose() << endl;
    return 0;
}

3.7.可视化演示

3.7.1.Ubuntu20下Pangolin的安装

        笔者的虚拟机版本是ubuntu20，安装过程中踩了挺多坑的。。。。。建议看一遍下面的再进行安装。

        1.下载功能包、打开终端

git clone https://github.com/stevenlovegrove/Pangolin.git

        2.开始编译

先进入你下载的Pangolin文件夹
mkdir build
cd build
cmake ..
cmake --build .

        这个是编译过程中的报错信息：

        踩了半天的坑，看了很多资料都无法解决。

        看回到报错：

        进入报错的文件夹发现并没有这个文件，看了一圈可能是版本问题

于是去到github上找到他的历史版本：（0.5版本）
GitHub链接: link

        再编译一次：

先进入你下载的Pangolin文件夹
mkdir build
cd build
cmake ..
cmake --build .

        一大丢新的报错，参考了以下链接: link

        谢天谢地，终于编译成功了。。。。。

        3. 编译结束后记得安装

sudo make install

        4.然后就完成了，可以执行例子进行验证

cd Pangolin/build/examples/HelloPangolin
./HelloPangolin

        成功运行

3.7.2.实践：可视化轨迹

CMakeLists.txt

#指定cmake版本
cmake_minimum_required( VERSION 2.8 )
#工程名字
project( plotTrajectory )

set( CMAKE_BUILD_TYPE "Release")
# set( CMAKE_CXX_FLAGS ".03")

add_executable( main /home/psj/Desktop/slam_study/ch3_4/src/plotTrajectory.cpp)

include_directories("/usr/include/eigen3")


find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
target_link_libraries(main ${Pangolin_LIBRARIES})

#set( CMAKE_BUILD_TYPE "Debug")

PlotTrajectory.cpp

//导入pangolin的库
#include <pangolin/pangolin.h>
//导入Eigen的相关库
#include <Eigen/Core>
#include <Eigen/Geometry>
//导入Linux系统中使用的一些系统调用函数。比如usleep函数
#include <unistd.h>

// 本例演示了如何画出一个预先存储的轨迹
using namespace std;
using namespace Eigen;

//轨迹文件的路径
string trajectory_file = "/home/psj/Desktop/slam_study/ch3_4/src/trajectory.txt";

//函数声明
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

//定义主函数
int main(int argc, char **argv) 
{
  /*    使用Eigen库中的Isometry3d类型作为元素的向量。Isometry3d是一个3D变换矩阵类型。

        Eigen::aligned_allocator<Isometry3d> 是一个用于分配 Isometry3d 对象的自定义分配器。它是在使用
        Eigen 库时为了保证内存对齐而提供的。在 C++ 中，内存对齐是指在分配内存时，确保某些类型的对象的地址
        是对齐的，以提高访问效率。对于 Eigen 库中的一些特殊类型，如 Isometry3d，它们需要按照特定的对齐要
        求进行存储，以避免性能损失。

        poses是定义的变量
  */  
  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
  /*
        ifstream fin(trajectory_file) 是在 C++ 中使用输入文件流对象 ifstream 打开一个文件。在这个代码
        中，它用于打开存储轨迹信息的文件 trajectory_file。

        ifstream是C++标准库中用于从文件中读取数据的类。它提供一组用于读取文件内容的成员函数和操作符重载。
        此处, trajectory_file 是一个表示文件路径的字符串，用于指定要打开的轨迹文件的位置。ifstream 对象
        fin 被创建并与该文件关联。

        一旦文件被成功打开，您就可以使用 fin 对象来读取文件中的数据。通常，您可以使用 >> 操作符重载来逐个
        读取文件中的数据项。

        请注意，打开文件之后，应该检查文件是否成功打开。可以通过检查 fin 对象的状态来确定文件是否成功打开。
        例如，可以使用 fin.is_open() 来检查文件是否打开成功。
  */
  ifstream fin(trajectory_file);
  if (!fin) 
  {
    cout << "cannot find trajectory file at " << trajectory_file << endl;
    return 1;
  }
  /*
        !fin.eof() 是对输入文件流对象 fin 的 eof() 成员函数的逻辑非操作。在C++中，eof() 函数用于检查文件
        流的结束标志。
        
        eof() 返回一个布尔值，表示文件流是否已达到文件末尾。当文件流到达文件末尾时，eof() 返回 true，否则
        返回 false。

        此处，!fin.eof() 用于检查文件流 fin 是否还未到达文件末尾。如果文件流还未到达文件末尾，!fin.eof() 
        的结果为真，表示文件流尚未结束。
  */
  while (!fin.eof()) 
  {
    /*
        time：时间
        tx：平移向量的 x 分量
        ty：平移向量的 y 分量
        tz：平移向量的 z 分量
        qx：四元数的 x 分量
        qy：四元数的 y 分量
        qz：四元数的 z 分量
        qw：四元数的 w 分量
    */
    double time, tx, ty, tz, qx, qy, qz, qw;
    //赋值
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    //定义欧氏变换矩阵Twr并用四元数初始化他的旋转矩阵
    Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
    //初始化他的平移矩阵
    Twr.pretranslate(Vector3d(tx, ty, tz));
    //调用 push_back(Twr)，将 Twr 添加到 poses 容器的末尾，扩展容器的大小，并将 Twr 插入到新的位置。
    poses.push_back(Twr);
  }
  //size() 是容器的成员函数之一，用于返回容器中元素的数量。
  cout << "read total " << poses.size() << " pose entries" << endl;

  //画出轨迹
  DrawTrajectory(poses);
  return 0;
}

//画出轨迹的函数
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) 
{
  // create pangolin window and plot the trajectory
  //创建显示窗口
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  /*
        glEnable(GL_DEPTH_TEST); 是OpenGL函数调用，用于启用深度测试。
        在OpenGL中，深度测试是一种用于确定像素是否应该被绘制的技术。它通过比较每个像素的深度值（即离相机的距离）
        与当前已绘制像素的深度值进行判断。只有当像素的深度值小于当前已绘制像素的深度值时，才会将该像素绘制到屏幕
        上，从而实现正确的遮挡关系。
        通过调用 glEnable(GL_DEPTH_TEST)，我们启用了深度测试功能。这意味着在绘制场景时，OpenGL会自动处理深度值，
        确保正确的遮挡关系，并按照深度值绘制像素。  
  */
  glEnable(GL_DEPTH_TEST);
  /*
        glEnable(GL_BLEND); 是OpenGL函数调用，用于启用混合功能。
        在OpenGL中，混合（Blending）是一种将新像素颜色与已存在的像素颜色进行混合的技术。通过启用混合功能，可以实
        现透明效果、颜色混合以及其他特殊效果。
        调用 glEnable(GL_BLEND) 启用了混合功能。这意味着在绘制过程中，OpenGL将根据特定的混合方程式，将新的像素颜
        色与已存在的像素颜色进行混合，并将混合后的结果绘制到屏幕上。
  */
  glEnable(GL_BLEND);
  /*
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 是 OpenGL 函数调用，用于设置混合函数。
        在 OpenGL 中，混合函数用于指定混合因子，控制混合操作中源颜色和目标颜色的权重。glBlendFunc() 函数接受两个参
        数，分别是源混合因子和目标混合因子。
        在这个特定的调用中，GL_SRC_ALPHA 指定源混合因子，表示使用源颜色的alpha值作为权重。GL_ONE_MINUS_SRC_ALPHA
        指定目标混合因子，表示使用目标颜色的 alpha 值的补数作为权重。
        因此，这个混合函数设置将根据源颜色的 alpha 值和目标颜色的 alpha 值的补数来进行混合操作。这通常用于实现透明效
        果，其中源颜色的透明度决定了最终像素的透明度，并与背景进行混合。
        需要注意的是，在调用 glBlendFunc() 之前，应该先启用混合功能（通过调用 glEnable(GL_BLEND)）
  */
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  /*
        pangolin::OpenGlRenderState 是 Pangolin 库中的一个类，它用于管理 OpenGL 渲染状态，包括投影矩阵、模型视图
        矩阵和其他与渲染相关的状态。
        通过调用 pangolin::OpenGlRenderState() 的无参数构造函数，创建了一个名为 s_cam 的 OpenGlRenderState 对象。
        这个对象将用于在 Pangolin 窗口中设置和管理 OpenGL 渲染状态，以便进行后续的渲染操作。
  */
  pangolin::OpenGlRenderState s_cam
  (
  /*
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000) 是一个函数调用，用于创建 Pangolin 
        中的投影矩阵。
        pangolin::ProjectionMatrix() 是 Pangolin 库中的一个函数，它用于创建一个投影矩阵，用于定义场景的投影变换。
        在这个特定的调用中，传递了一系列参数 (1024, 768, 500, 500, 512, 389, 0.1, 1000)，用于定义投影矩阵的属性。
        具体参数的含义如下：
            1024 和 768 是视口的宽度和高度（以像素为单位）。
            -500 和 500 是投影平面的左右边界。
            -512 和 389 是投影平面的上下边界。
            0.1 和 1000 是近裁剪面和远裁剪面的位置。
        通过调用 pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000)，会创建一个投影矩阵，定义了
        场景的投影变换，用于后续的渲染操作和绘制。
  */
    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中的模
        型视图矩阵。
        pangolin::ModelViewLookAt() 是 Pangolin 库中的一个函数，它用于创建一个 LookAt 视图矩阵，用于定义场景的观察变换。
        在这个特定的调用中，传递了一系列参数 (0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)，用于定义模型视图矩阵的属性。
        具体参数的含义如下：
            0, -0.1, -1.8 是相机位置的 x、y、z 坐标。
            0, 0, 0 是观察目标的 x、y、z 坐标。
            0.0, -1.0, 0.0 是相机的上向量的 x、y、z 分量。
        通过调用 pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)，会创建一个模型视图矩阵，定义了场景
        的观察变换，用于后续的渲染操作和绘制。这个观察变换将相机位置设置为 (0, -0.1, -1.8)，观察目标设置为原点 (0, 0, 0)，
        并指定了相机的上向量。
  */
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );
  /*
        在这个语句中，首先通过调用 pangolin::CreateDisplay()创建一个显示窗口，并将返回的pangolin::View对象赋值给d_cam引用。
        接下来，通过调用 SetBounds() 方法设置了窗口的边界。具体的参数 (0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f) 定义了窗口边
        界的范围。前两个参数 0.0 和 1.0 表示窗口的水平边界范围，从左边界到右边界。后两个参数 0.0 和 1.0 表示窗口的垂直边界范围
        ，从下边界到上边界。最后一个参数 -1024.0f / 768.0f 是窗口的纵横比（aspect ratio），用于保持窗口显示的宽高比。
        最后，通过调用 SetHandler() 方法设置了一个 pangolin::Handler3D 对象作为窗口的事件处理器。该处理器使用之前创建的s_cam
        对象作为参数，用于处理与三维交互相关的事件和操作。
        通过这个语句，创建了一个窗口对象 d_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));


  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 < 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();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }
}

执行效果：

ch4 李群与李代数

4.1.李群与李代数基础

        先回去回顾一下前面介绍的两个群，那个时候一笔带过了，现在详细讲

4.1.1.群

        群（Group）是一种集合加上一种运算的代数结构，我们把集合记作A，运算记作·，那么群可以记作G = （A，·）。群要求满足：

        巧记为“凤姐咬我”。

        李群是指具有连续（光滑）性质的群，特殊正交群和特殊欧氏群是李群。

4.1.2.李代数的引出

        这里拿我的手写笔记出场：

4.1.3.李代数的定义

        每一个李群都有自己对应的李代数，李代数描述了李群的局部性质，准确的说，是单位元附近的正切空间。

4.1.4.李代数so(3)

        特殊正交群SO(3)对应的李代数so(3)是定义在**R³**上的向量，可以生成一个反对称矩阵：

        此李代数的李括号为：

        李代数so（3）：

4.1.5.李代数se(3)

        特殊欧氏群SE(3)对应的李代数se(3)位于**R⁶**空间中：

        此李代数的李括号为：

4.2.指数与对数映射

4.2.1.SO(3)上的指数映射

        exp(A)，这个东西是数学表达式来着，我最开始也不知道他是啥东东。其实他就是e的A次方。也就是：

                                                exp(A) = e^A

        对exp(A)，泰勒展开有：

        类似的：




        略去简化过程，有：

式子（3.17）

4.2.2.SE(3)上的指数映射

se(3)上的指数映射形式如下：






总结的图表：

4.3.李代数求导与扰动模型

4.3.1.BCH公式近似形式

        使用李代数的一大动机是为了进行优化，那么李代数的求导就很关键了。
        下面解决一下这个问题，我们知道当进行两次旋转时对应的是两个旋转矩阵相乘，那么他的李代数也是相乘吗，不是的，这里就引出了BCH公式，但是BCH公式不好计算，于是我们又引出了BCH公式的近似形式：

        BCH的线性近似表达：

4.3.2.SO(3)上的李代数求导

4.3.3.李代数求导

4.3.4.扰动模型（左乘）

4.3.5.SE(3)上的李代数求导

4.4.实践：Sophus

        我们之前下载的Eigen库提供了几何模块，但没有提供李代数的支持，一个较好的李代数库是基于Eigen的Sophus库。他不需要安装额外的依赖。在这里Sophus库要怎么安装我就不过多赘述了，因为之前已经安装过几个库了，操作基本是一样的。值得注意的是安装Sophus库需要你提前安装好fmt和 Eigen 这两个库 。fmt和 Sophus两个库的github地址我写在了下面，大家自行安装。

        fmt链接:link
        Sophus链接:link

4.4.1.Sophus的基本使用方法

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
//导入sophus相关的库
#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();
    //将旋转矩阵赋值给四元数q
    Quaterniond q(R);
    //可以由旋转矩阵直接得出特殊正交群
    Sophus::SO3d SO3_R(R);
    //也可以由四元数直接得出特殊正交群
    Sophus::SO3d SO3_Q(q);
    //打印出来
    cout << "SO(3) from rotation vector is \n" << SO3_R.matrix() << endl;
    cout << "SO(3) from quaternion is \n" << SO3_R.matrix() << endl;
    cout << "they are equal \n " << endl;
    
    //.log()函数直接提取特殊正交群的李代数（使用对数映射）
    Vector3d so3 = SO3_R.log();
    //打印出李代数（向量）
    cout << "so3 =    \n" << so3.transpose() << endl;
    //打印出李代数（向量转换为反对称矩阵）hat
    cout << "so3 hat =  \n  " << Sophus::SO3d::hat(so3) << endl;
    //打印出李代数（反对称矩阵转换为向量）vee
    cout << "so3 hat vee =\n    " << 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;
    cout << "SO3 updated is  \n " << SO3_updated.matrix() << endl;


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


    //se3也是大同小异，so3懂了se3自然就看懂了，只不过是多了平移而已。
    Vector3d t(1,0,0);
    Sophus::SE3d SE3_Rt(R,t);
    Sophus::SE3d SE3_Qt(q,t);
    cout << "SE(3) from R,t is \n" << SE3_Rt.matrix() << endl;
    cout << "SE(3) from Q,t is \n" << SE3_Qt.matrix() << endl;

    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout << "se3 =    \n" << se3.transpose() << endl;
    cout << "se3 hat =  \n  " << Sophus::SE3d::hat(se3) << endl;
    cout << "se3 hat vee =\n    " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;

    Vector6d update_se3;
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
    cout << "SE3 updated is  \n " << SE3_updated.matrix() << endl;

    return 0;
}

CMakeLists.txt文件

cmake_minimum_required( VERSION 2.8 )
add_definitions( -std=c++11 )

project( useSophus )


set( CMAKE_BUILD_TYPE "Release")
# set( CMAKE_CXX_FLAGS ".03")

add_executable( main /home/psj/Desktop/slam_study/ch4_1/src/useSophus.cpp)


include_directories("/usr/include/eigen3")


find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
target_link_libraries( main ${Sophus_LIBRARIES} fmt)

#set( CMAKE_BUILD_TYPE "Debug")

4.4.2.例子：评估轨迹的误差

#include <iostream>
#include <fstream>
#include <unistd.h>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
//导入sophus相关的库
#include <sophus/se3.hpp>
#include <pangolin/pangolin.h>

using namespace std;
using namespace Eigen;
using namespace Sophus;

//用字符串存储两个轨迹文件的绝对路径
string groundtruth_file = "/home/psj/Desktop/slam_study/ch4_2/src/estimated.txt";
string estimated_file = "/home/psj/Desktop/slam_study/ch4_2/src/estimated.txt";


//提前定义TrajectoryType变量类型（vector类，类型为SE3d且内存对齐）
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;

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


//读取轨迹的函数，返回TrajectoryType变量，输入值是路径的字符串指针（后面详细写他怎么实现的）
TrajectoryType ReadTrajectory(const string &path);

int main(int argc, char **argv) {
  //读取真实轨迹文件并放在TrajectoryType类型的变量groundtruth中
  TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
  //读取预测轨迹文件并放在TrajectoryType类型的变量estimated中
  TrajectoryType estimated = ReadTrajectory(estimated_file);
  //确保groundtruth和estimated这两个向量都不为空
      //  groundtruth.empty()检查groundtruth是否为空，空的话返回true
      //  estimated.empty()检查estimated是否为空，空的话返回true
      //  assert宏是C++标准库中的一个调试工具，用于在程序执行过程中进行断言检查。断言是
      //一种声明，它表达了在程序运行过程中应该为真的条件。如果断言条件为假，即其中任意一
      //个向量为空，那么assert宏将触发一个断言失败的错误，并终止程序的执行。通过使用断言，
      //这段代码确保了在继续执行后续的轨迹比对和可视化操作之前，必须满足 groundtruth 和
      //estimated向量都不为空的条件。
  assert(!groundtruth.empty() && !estimated.empty());
  //确保groundtruth和estimated这两个向量大小一致
  assert(groundtruth.size() == estimated.size());

  //计算绝对轨迹误差【均方根误差（Root Mean Square Error，RMSE）】
  double rmse = 0;
  //  循环遍历estimated和groundtruth两个向量中的元素。这里使用了size_t类型的循环索引i，
  //从0开始，逐个增加直到estimated向量的大小。
  for (size_t i = 0; i < estimated.size(); i++) {
    Sophus::SE3d p1 = estimated[i], p2 = groundtruth[i];
    //.norm()是求范数
    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变量，输入值是路径的字符串指针
TrajectoryType ReadTrajectory(const string &path) 
{
  //使用ifstream类，我们可以打开文件并从文件中读取数据
  ifstream fin(path);
  //定义TrajectoryType类型的trajectory轨迹变量，后面会把他return回去
  TrajectoryType trajectory;
  //如果文件打开失败，fin的值为false
  if (!fin) 
  {
    //cerr是C++标准库中的一个标准错误输出流，用于输出错误信息到标准错误设备。
    cerr << "trajectory " << path << " not found." << endl;
    return trajectory;
  }
  //!fin.eof()是一个条件表达式，用于检查输入文件流对象fin是否已经到达文件末尾（End of File，EOF）。
  //eof()是ifstream类的一个成员函数，用于检查文件流是否已经到达文件末尾。它返回一个布尔值，若文件流
  //已经到达文件末尾，则返回true，否则返回false。
  while (!fin.eof()) 
  {
    //定义时间戳和平移向量和四元数的变量
    double time, tx, ty, tz, qx, qy, qz, qw;
    //依次将文件里的值写入，一次while写入8个值，对应一个时间戳的数据
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    //定义特殊欧氏群p1,将代表旋转的四元数和代表平移的平移向量赋值给p1
    Sophus::SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
    //将p1插入到轨迹变量的末尾
    trajectory.push_back(p1);
    //当文件读取完后结束while循环
  }
  //返回轨迹变量
  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 j = 0; j < esti.size() - 1; j++) {
      glColor3f(1.0f, 0.0f, 0.0f);  // red for estimated
      glBegin(GL_LINES);
      auto p1 = esti[j], p2 = esti[j + 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
  }

}

cmake_minimum_required( VERSION 2.8 )
add_definitions( -std=c++11 )

project( trajectoryError )


set( CMAKE_BUILD_TYPE "Release")
# set( CMAKE_CXX_FLAGS ".03")

add_executable( main /home/psj/Desktop/slam_study/ch4_2/src/trajectoryError.cpp)


include_directories("/usr/include/eigen3")


find_package(Sophus REQUIRED)
find_package(Pangolin REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

target_link_libraries( main ${Sophus_LIBRARIES} fmt)
target_link_libraries( main ${Pangolin_LIBRARIES})

#set( CMAKE_BUILD_TYPE "Debug")

显示结果：（只显示了一条线，目前没有找到bug只在哪里，后面debug再回来填坑）

ch5 相机与图像

        这一讲讲“机器人如何观测外部世界”，也就是观测方程部分，在相机为主导的slam里，观测主要是指相机成像的过程。

5.1.相机模型

        相机成像的过程，能够用一个几何模型进行描述，其中最简单的称之为针孔模型。同时由于相机镜头上透镜的存在，使得光线投影到成像平面过程中会产生畸变。因此我们使用针孔和畸变两个模型来描述整个投影的过程。

5.1.1.针孔相机模型

        这一小节针孔相机模型比较简单，大家看一遍书。肯定能看懂，这里就不多加叙述。

        这里所有点的单位都可以理解成米，不过在相机中我们最终获得的是一个个的像素，所以这需要我们在成像平面进行采样和量化。我们设在物理成像平面上固定着一个像素平面o-u-v. 我们的像素平面得到了 P’的像素的像素坐标。

        像素坐标系，通常的定义方式是原点o’位于图像的左上角，u轴向右，与X轴平行，v轴向下与Y轴平行，像素坐标系跟成像平面之间相差一个缩放和一个原点的平移

        中间的矩阵K我们称之为相机的内参。

        通常我们认为相机的内参在出厂后是固定的，不会在使用过程中发生变化。有的相机生产厂商会告诉你相机的内参，而有时需要你自己去确定，这个过程就是所谓的标定，鉴于标定算法非常成熟（如著名的单目棋盘格张正友标定法），这里就不介绍了。

        笔者这一部分对于标定算法也是不太清楚。后面如果有空的话再来填这个坑。我会在23年末之前写一篇博客，专门聊一下单目棋盘格张正友标定法，也会把它收录在这一个专题里面。

5.1.2.畸变模型

        由透镜形状引起的畸变（Distortion，也叫失真），称为径向畸变，由于实际加工制作的透镜往往是中心对称的，这使得不规则的畸变通常径向对称，他们主要分为两大类，桶形畸变和枕形畸变。

        除了透镜的形状会引入径向畸变，由于在相机组装过程中，不能使透镜和成像平面严格平行，所以也会引入切向畸变。

5.1.3.双目相机模型

        双目比较好理。

5.1.4.RGB-D 相机模型

5.2.图像

        在数学中，图像可以用一个矩阵来描述，也就是二维数组。

        我们从最简单的图像——灰度图说起，在一张灰度图中，每个像素位置（x，y）对应一个灰度值 I ，可以记为一个函数：

        其中x和y是像素的坐标，然而计算机并不能表达实数空间，所以我们需要对下标和图像读数在某个范围内进行量化。

        例如 x和y通常是从0开始的整数，常见的灰度图中，用0~255的一个整数（unsigned char，一个字节）来表达图像的灰度读数。

        一张宽度为640像素，高度为480像素的分辨率的灰度图就可以表示为：

unsigned char image[480][640];

        为什么这里的二维数组是480×640呢？因为在程序中图像以二维数组形式储存，他的第一个下标是指数组的行，而第二个下标则是列。

        在图像中数组的行数对应图像的高度，而列数对应图像的宽度。

        下面考虑这幅图像的内容，图像是由像素组成的，当访问某一个像素时需要指明它所处的坐标，像素坐标的原点位于图像的左上角，X轴向左，Y轴向下（也就是前面说的u-v坐标）。

        如果他还有第三个轴Z轴，那么根据右手法则，这一轴应该是向前的，这种定义方式跟相机坐标系是一致的。我们平常说的图像的宽度或列数对应着X轴和图像的行数或高度则对应它的Y轴。

        根据这种定义方式，我们如果讨论一个位于（x，y）处的像素，那么它在程序中的访问方式应该是：

unsigned char pixel = image[y,x];

        它对应着灰度值 I（x，y）的读数。请注意这里，X和Y的顺序。虽然我们不厌其烦的讨论坐标系的问题，但是像这种下标顺序的错误，仍会是新手在调试过程中经常遇到的，且具有一定隐蔽性的错误之一。如果在写程序的时候不慎调换了xy的坐标，编译器无法提供任何信息，而你所能看到的只是程序运行中的一个越界错误而已。

        一个像素的灰度可以用8位整数记录，也就是一个0-255的值。当我们要记录的信息更多时，一个字节恐怕就不够了。

        例如在RGB-D相机的深度图中，记录了各个像素与相机之间的距离，这个距离通常以毫米为单位，RGB-D相机的量程通常在十几米左右，超过了255，这时候人们会用16位整数（C++中的unsigned short）来记录深度图的信息，也就是位于0~65535的值，换算成米的话，最大可以表示65米。足够RGB-D相机使用了。

        彩色图像的表示则需要通道（Channel）的概念。在计算机中我们是用红色、绿色和蓝色这三个颜色的组合来表达任意一种色彩。于是对于每一个像素就要记录其R GB三个通道的数字。例如最常见的彩色图像有三个通道，每个通道都有8位整数表示，在这种规定下一个像素占据24位的空间。

        通道的数量顺序是可以自由定义的。在OpenCV的彩色图像中，通道的默认顺序是B-G-R，也就是我们得到一个24位的像素时，前8位表示蓝色，中间八位为绿色，最后八位为红色。同理也可以用RGB的顺序表示一个彩色图，如果还想表达图像的透明度，那就使用RGBA4个通道。

5.3.实践：计算机中的图像

5.3.1.OpenCV的基本使用方法

1.安装OpenCV4依赖的库

        （这里我们用OpenCV4而不是原书中的OpenCV3，因为书是很多年前得了，我们既然学，就学主流，直接上OpenCV4）opencv4.7

sudo apt-get install build-essential libgtk2.0-dev libgtk-3-dev libavcodec-dev libavformat-dev libjpeg-dev libswscale-dev libtiff5-dev

2.下载OpenCV4

git clone https://github.com/opencv/opencv.git

3.编译安装

1）进入到下载好的opencv目录中，新建目录build并进入build目录：

cd opencv
mkdir build
cd build

2）因为OpenCV在debug和release两种模式下生产的库文件不尽相同，debug模式因为添加了很多调试信息，文件大小相对会大一些，所以这里分别编译debug和release两种模式下的OpenCV。 对于debug模式，执行如下命令：

cmake -D CMAKE_BUILD_TYPE=Debug -D OPENCV_GENERATE_PKGCONFIG=YES -D CMAKE_INSTALL_PREFIX=/usr/local/OpenCV/Debug -D WITH_FFMPEG=ON ..

        这里有个地方需要注意，命令最后是两个点，是上一层目录的意思，因为Cmakelist.txt在build的上层目录中。安装路径通过CMAKE_INSTALL_PREFIX设置为/usr/local/OpenCV/Debug，默认是/usr/local。而编译类型通过CMAKE_BUILD_TYPE=DEBUG设为DEBUG，默认是RELEASE。对于Release下的编译，只需要更改这两个部分即可，即：

cmake -D CMAKE_BUILD_TYPE=Release -D OPENCV_GENERATE_PKGCONFIG=YES -D CMAKE_INSTALL_PREFIX=/usr/local/OpenCV/Release -D WITH_FFMPEG=ON ..

        注意，这里要执行两遍make。

3）使用make安装

sudo make install

        至此，opencv4就安装好了。

默认安装路径为：
/usr/local/bin - executable files
/usr/local/lib - libraries (.so)
/usr/local/cmake/opencv4 - cmake package
/usr/local/include/opencv4 - headers
/usr/local/share/opencv4 - other files (e.g. trained cascades in XML format)

4.设置库的搜索路径

        在/etc/ld.so.conf.d/加入对于库的.conf文件，首先新建opencv.conf

sudo gedit /etc/ld.so.conf.d/opencv.conf

        在opencv.conf中加入函数库所在的目录：

/usr/local/OpenCV/Debug/lib
/usr/local/OpenCV/Release/lib

        也可以在ld.so.conf文件中

sudo gedit /etc/ld.so.conf

        加入上述目录。其实ld.so.conf中写着如下代码：

include /etc/ld.so.conf.d/*.conf

        说明ld.so.conf是包含ld.so.conf.d中所有的.conf文件的。

5.将/etc/ld.so.conf.d中的数据读入缓存

        接下来利用ldconfig命令，将/etc/ld.so.conf.d中的数据读入缓存

sudo ldconfig

6.添加pkg-config环境变量

        然后添加pkg-config环境变量，便于pkg-config找到*.pc文件。因为我们生成了两个版本的OpenCV，为了便于区分，分别把Debug和Release下lib/pkgconfig下（/usr/local/OpenCV/Debug/lib/pkgconfig，/usr/local/OpenCV/Release/lib/pkgconfig）对应的*.pc文件改名为opencv-debug和opencv-release。

sudo mv opencv4.pc opencv4_release.pc

sudo mv opencv4.pc opencv4_debug.pc

        然后把这两个路径设置在环境变量中，

sudo gedit /etc/profile

        把以下代码写入文件的最后并保存。

export PKG_CONFIG_PATH=/usr/local/OpenCV/Debug/lib/pkgconfig:$PKG_CONFIG_PATH
export PKG_CONFIG_PATH=/usr/local/OpenCV/Release/lib/pkgconfig:$PKG_CONFIG_PATH

        或者，直接把这两文件放到一起，然后放到某个位置，再把这个路径设置到环境变量中。pkg-config的默认搜索路径是/usr/local/lib/config，所以可以直接放在这个路径下，无需设置环境变量。

        保存并退出后激活：

source /etc/profile

        验证是否成功：

pkg-config --libs opencv4_debug

        终端显示

-L/usr/local/OpenCV/Debug/lib -lopencv_gapi -lopencv_highgui -lopencv_ml -lopencv_objdetect -lopencv_photo -lopencv_stitching -lopencv_video -lopencv_calib3d -lopencv_features2d -lopencv_dnn -lopencv_flann -lopencv_videoio -lopencv_imgcodecs -lopencv_imgproc -lopencv_core

pkg-config --libs opencv4_release

        终端显示

-L/usr/local/OpenCV/Release/lib -lopencv_gapi -lopencv_highgui -lopencv_ml -lopencv_objdetect -lopencv_photo -lopencv_stitching -lopencv_video -lopencv_calib3d -lopencv_features2d -lopencv_dnn -lopencv_flann -lopencv_videoio -lopencv_imgcodecs -lopencv_imgproc -lopencv_core

        这样OpenCV的安装就完成了，可以参考Using OpenCV with gcc and CMake中讲解的OpenCV使用方法尝试测试几个简单的程序试试。

        安装并测试opencv4的参考链接（一遍过）: link

7.cpp代码

#include <iostream>
//<chrono>是C++标准库中用于处理时间的头文件。它提供了一组类和函数，用于测量时间间隔、计时和延时等操作。
#include <chrono>

using namespace std;
//我们用的是opencv4，书里的是opencv3的路径，我们需要改动一下
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
/*
            <opencv2/highgui.hpp>是OpenCV库中用于图形用户界面 (GUI) 的头文件之一。它包含了图像显示、
        窗口管理和用户交互等功能的定义。

            在使用OpenCV进行图像处理和图形界面操作时，通常需要包含<opencv2/highgui.hpp>头文件，以便
        使用其中定义的类和函数。

            以下是一些常用的类和功能：
            cv::imshow：用于显示图像的函数，可以将图像数据显示在屏幕上的窗口中。
            cv::waitKey：等待键盘输入的函数，通常与cv::imshow一起使用，用于在图像显示期间等待按键触发。
            cv::namedWindow：创建一个指定名称的图像窗口。
            cv::destroyAllWindows：关闭所有图像窗口。
            cv::imwrite：将图像数据保存为图像文件。
            cv::VideoCapture：用于从摄像头或视频文件中捕获视频帧。
            cv::VideoWriter：用于将视频帧写入视频文件。

            除了上述功能，<opencv2/highgui.hpp>头文件还包含了其他一些与图形界面相关的函数和数据结构。通
        过包含这个头文件，可以使用OpenCV库提供的图形界面功能，例如图像显示、窗口管理、用户交互和视频处理等
        操作。

            <opencv2/core.hpp>是OpenCV库中的核心头文件之一，它包含了一些最基本的图像处理功能和数据结构
        的定义。

            在使用OpenCV进行图像处理时，通常需要包含<opencv2/core.hpp>头文件，以便使用其中定义的类和函数。

            以下是一些常用的类和功能：
            cv::Mat：表示图像或多维数组的数据结构。它是OpenCV中最常用的类之一，用于存储和操作图像数据。
            cv::Size：表示图像或矩形区域的尺寸，包含宽度和高度。
            cv::Point：表示二维平面上的点的坐标，包含x和y。
            cv::Scalar：表示颜色或数值的数据结构，包含多个通道的值。
            cv::imread：用于读取图像文件并返回cv::Mat对象。
            cv::imshow：显示图像窗口，将图像数据显示在屏幕上。
            cv::waitKey：等待键盘输入的函数，通常与cv::imshow一起使用。
            cv::destroyAllWindows：关闭所有图像窗口。
            
            除了上述功能，<opencv2/core.hpp>头文件还包含了其他一些与图像处理相关的数据结构和函数。通过包含
        这个头文件，可以使用OpenCV库提供的核心功能来进行图像读取、显示、处理和分析等操作。
*/

int main(int argc, char **argv) {
  // 读取argv[1]指定的图像
  cv::Mat image;
  image = cv::imread(argv[1]); //cv::imread函数读取指定路径下的图像

  // 判断图像文件是否正确读取
  if (image.data == nullptr) { //数据不存在,可能是文件不存在
    cerr << "文件" << argv[1] << "不存在." << endl;
    return 0;
  }

  // 文件顺利读取, 首先输出一些基本信息
  cout << "图像宽为" << image.cols << ",高为" << image.rows << ",通道数为" << image.channels() << endl;
  cv::imshow("image", image);      // 用cv::imshow显示图像
  cv::waitKey(0);                  // 暂停程序,等待一个按键输入

  // 判断image的类型
  // 在 OpenCV 中，图像数据类型使用整数值来表示，这些整数值对应于不同的数据类型和通道数。常见的图像数据类型包括：
  //      CV_8UC1：表示8位无符号单通道图像，即灰度图像。
  //      CV_8UC3：表示8位无符号三通道图像，即彩色图像。
  if (image.type() != CV_8UC1 && image.type() != CV_8UC3) {
    // 图像类型不符合要求
    cout << "请输入一张彩色图或灰度图." << endl;
    return 0;
  }

  // 遍历图像, 请注意以下遍历方式亦可使用于随机像素访问
  // 使用 std::chrono 来给算法计时

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//steady_clock::now()返回当前时间点
  for (size_t y = 0; y < image.rows; y++) {
    // 用cv::Mat::ptr获得图像的行指针
    unsigned char *row_ptr = image.ptr<unsigned char>(y);  // row_ptr是第y行的头指针
    for (size_t x = 0; x < image.cols; x++) {
      // 访问位于 x,y 处的像素
      unsigned char *data_ptr = &row_ptr[x * image.channels()]; // data_ptr 指向待访问的像素数据
      // 输出该像素的每个通道,如果是灰度图就只有一个通道
      for (int c = 0; c != image.channels(); c++) {
        unsigned char data = data_ptr[c]; // data为I(x,y)第c个通道的值
      }
    }
  }
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast < chrono::duration < double >> (t2 - t1);
  cout << "遍历图像用时：" << time_used.count() << " 秒。" << endl;

  // 关于 cv::Mat 的拷贝
  // 直接赋值并不会拷贝数据
  cv::Mat image_another = image;
  // 修改 image_another 会导致 image 发生变化
  image_another(cv::Rect(0, 0, 100, 100)).setTo(0); // 将左上角100*100的块置零
  cv::imshow("image", image);
  cv::waitKey(0);

  // 使用clone函数来拷贝数据
  cv::Mat image_clone = image.clone();
  image_clone(cv::Rect(0, 0, 100, 100)).setTo(255);
  cv::imshow("image", image);
  cv::imshow("image_clone", image_clone);
  cv::waitKey(0);

  // 对于图像还有很多基本的操作,如剪切,旋转,缩放等,限于篇幅就不一一介绍了,请参看OpenCV官方文档查询每个函数的调用方法.
  cv::destroyAllWindows();
  return 0;
}

8.CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
add_definitions( -std=c++11 )

project( imageBasics )


set( CMAKE_BUILD_TYPE "Release")
# set( CMAKE_CXX_FLAGS ".03")

add_executable( main /home/psj/Desktop/slam_study/ch5_1/src/imageBasics.cpp)




find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

target_link_libraries( main ${OpenCV_LIBRARIES})

#set( CMAKE_BUILD_TYPE "Debug")

9.执行代码

./main ../src/ubuntu.png

5.3.2.图像去畸变

#include <opencv2/opencv.hpp>
#include <string>

using namespace std;

string image_file = "../src/distorted.png";   // 请确保路径正确

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

  // 本程序实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。
  // 畸变参数
  double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
  // 内参
  double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

  cv::Mat image = cv::imread(image_file, 0);   // 图像是灰度图，CV_8UC1
  int rows = image.rows, cols = image.cols;
  cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 去畸变以后的图

  // 计算去畸变后图像的内容
  for (int v = 0; v < rows; v++) {
    for (int u = 0; u < cols; u++) {
      // 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)
      double x = (u - cx) / fx, y = (v - cy) / fy;
      double r = sqrt(x * x + y * y);
      double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);
      double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;
      double u_distorted = fx * x_distorted + cx;
      double v_distorted = fy * y_distorted + cy;

      // 赋值 (最近邻插值)
      if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
        image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);
      } else {
        image_undistort.at<uchar>(v, u) = 0;
      }
    }
  }

  // 画图去畸变后图像
  cv::imshow("distorted", image);
  cv::imshow("undistorted", image_undistort);
  cv::waitKey();
  return 0;
}

cmake_minimum_required( VERSION 2.8 )
add_definitions( -std=c++11 )

project( undistortImage )


set( CMAKE_BUILD_TYPE "Release")
# set( CMAKE_CXX_FLAGS ".03")

add_executable( main /home/psj/Desktop/slam_study/ch5_2/src/undistortImage.cpp)




find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

target_link_libraries( main ${OpenCV_LIBRARIES})

#set( CMAKE_BUILD_TYPE "Debug")

执行效果

5.4.实践：3D视觉

5.4.1.双目视觉

        这一小节我们从双目视觉的左右图像出发，计算图像对应的视差图，然后计算各像素在相机坐标系下的坐标，他们将构成点云。

        下面是代码部分：

【这一部分代码笔者也看不懂，去看了一下配套的视频，发现作者并没有讲这部分的代码，我估计这只是展示一下的，就直接跳过这部分的代码细读了，后面如果发现得搞明白这部分代码，再回来填这个坑】

#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>

using namespace std;
using namespace Eigen;

// 文件路径
string left_file = "../src/left.png";
string right_file = "../src/right.png";

// 在pangolin中画图，已写好，无需调整
void showPointCloud(
    const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);

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

    // 内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 基线
    double b = 0.573;

    // 读取图像
    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);
    /*
            cv::Ptr 是 OpenCV 中的智能指针类，用于管理动态分配的对象。
            cv::Ptr<cv::StereoSGBM> 是一个指向 cv::StereoSGBM 对象的智能指针，它提供了对 cv::StereoSGBM 对象
        的自动内存管理和生命周期控制。

            cv::StereoSGBM::create() 是 cv::StereoSGBM 类的静态成员函数，用于创建和初始化 cv::StereoSGBM 对象。
        它接受一系列参数来配置立体匹配算法的行为，并返回一个指向配置好的 cv::StereoSGBM 对象的指针。

            在这行代码中，create() 函数的参数如下：
                minDisparity：视差搜索范围的最小值，设置为 0。
                numDisparities：视差搜索范围的大小，设置为 96。
                blockSize：视差计算时使用的块的大小，设置为 9。
                P1：控制视差平滑度的第一个参数，设置为 899。
                P2：控制视差平滑度的第二个参数，设置为 3299。
                disp12MaxDiff：允许的最大视差差异，设置为 1。
                preFilterCap：预处理滤波器的截断值，设置为 63。
                uniquenessRatio：唯一性比率，设置为 10。
                speckleWindowSize：孤立点滤波窗口的大小，设置为 100。
                speckleRange：孤立点的最大视差变化，设置为 32。
            这些参数值是用于配置立体匹配算法的参数，可以根据具体的需求进行调整。不同的参数值可能会对立体匹配结果产生
        影响，因此根据具体情况进行调试和优化是很重要的。
    */
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(0,96,9,8*9*9,32*9*9,1,63,10,100,32);
    cv::Mat disparity_sgbm, disparity;
    sgbm->compute(left, right, disparity_sgbm);
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

    // 生成点云
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;

    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;

            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色

            // 根据双目模型计算 point 的位置
            double x = (u - cx) / fx;
            double y = (v - cy) / fy;
            double depth = fx * b / (disparity.at<float>(v, u));
            point[0] = x * depth;
            point[1] = y * depth;
            point[2] = depth;

            pointcloud.push_back(point);
        }

    cv::imshow("disparity", disparity / 96.0);
    cv::waitKey(0);
    // 画出点云
    showPointCloud(pointcloud);
    return 0;
}

void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud 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);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3f(p[3], p[3], p[3]);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
add_definitions( -std=c++11 )

project( stereoVisoin )


set( CMAKE_BUILD_TYPE "Release")
# set( CMAKE_CXX_FLAGS ".03")

add_executable( main /home/psj/Desktop/slam_study/ch5_3/src/stereoVisoin.cpp)

include_directories("/usr/include/eigen3")


find_package(OpenCV REQUIRED)
find_package(Pangolin REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

target_link_libraries( main ${OpenCV_LIBRARIES})
target_link_libraries( main ${Pangolin_LIBRARIES})

#set( CMAKE_BUILD_TYPE "Debug")

执行效果：

5.4.2.RGB-D视觉

#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <boost/format.hpp>  // for formating strings
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>


using namespace std;
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// 在pangolin中画图，已写好，无需调整
void showPointCloud(
    const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud);

int main(int argc, char **argv) {
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    TrajectoryType poses;         // 相机位姿

    ifstream fin("../src/pose.txt");
    if (!fin) {
        cerr << "请在有pose.txt的目录下运行此程序" << endl;
        return 1;
    }

    for (int i = 0; i < 5; i++) {
        boost::format fmt("../src/%s/%d.%s"); //图像文件格式
        colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));
        depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "pgm").str(), -1)); // 使用-1读取原始图像

        double data[7] = {0};
        for (auto &d:data)
            fin >> d;
        Sophus::SE3d pose(Eigen::Quaterniond(data[6], data[3], data[4], data[5]),
                          Eigen::Vector3d(data[0], data[1], data[2]));
        poses.push_back(pose);
    }

    // 计算点云并拼接
    // 相机内参 
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;
    vector<Vector6d, Eigen::aligned_allocator<Vector6d>> pointcloud;
    pointcloud.reserve(1000000);

    for (int i = 0; i < 5; i++) {
        cout << "转换图像中: " << i + 1 << endl;
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Sophus::SE3d T = poses[i];
        for (int v = 0; v < color.rows; v++)
            for (int u = 0; u < color.cols; u++) {
                unsigned int d = depth.ptr<unsigned short>(v)[u]; // 深度值
                if (d == 0) continue; // 为0表示没有测量到
                Eigen::Vector3d point;
                point[2] = double(d) / depthScale;
                point[0] = (u - cx) * point[2] / fx;
                point[1] = (v - cy) * point[2] / fy;
                Eigen::Vector3d pointWorld = T * point;

                Vector6d p;
                p.head<3>() = pointWorld;
                p[5] = color.data[v * color.step + u * color.channels()];   // blue
                p[4] = color.data[v * color.step + u * color.channels() + 1]; // green
                p[3] = color.data[v * color.step + u * color.channels() + 2]; // red
                pointcloud.push_back(p);
            }
    }

    cout << "点云共有" << pointcloud.size() << "个点." << endl;
    showPointCloud(pointcloud);
    return 0;
}

void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud) {

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud 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);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3d(p[3] / 255.0, p[4] / 255.0, p[5] / 255.0);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}

cmake_minimum_required( VERSION 2.8 )
add_definitions( -std=c++11 )

project( joinMap )


set( CMAKE_BUILD_TYPE "Release")
# set( CMAKE_CXX_FLAGS ".03")

add_executable( main /home/psj/Desktop/slam_study/ch5_4/src/joinMap.cpp)

include_directories("/usr/include/eigen3")

find_package(Sophus REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Pangolin REQUIRED)

include_directories(${OpenCV_INCLUDE_DIRS})
include_directories(${Sophus_INCLUDE_DIRS})

target_link_libraries( main ${OpenCV_LIBRARIES})
target_link_libraries( main ${Pangolin_LIBRARIES})
target_link_libraries( main ${Sophus_LIBRARIES} fmt)

#set( CMAKE_BUILD_TYPE "Debug")

执行效果：

ch6 非线性优化

这一章是我目前觉得最难的一章。。。。。。

6.1.状态估计问题

6.1.1.批量状态估计与最大后验状态估计

        之前讲到：经典的SLAM模型，由一个运动方程和一个观测方程组成：

        在运动和观测方程中，我们通常假设两个噪声项：w_k, v_k,j, 满足零均值的高斯分布：


        在噪声的影响下，我们希望通过带噪声的数据z和u推断位姿x地图y(以及他们的概率分布)，这就构成了一个状态估计的问题。

        处理这个状态估计问题，大致分为两种方法，由于在slam的过程中，这些数据是随时间逐渐到来的，所以我们应该持有一个当前时刻的估计状态，然后用新的数据来更新它，这种方式我们称之为增量/渐进的方式，或者叫它滤波器，另一种方式则是把数据攒起来并处理，这种方式称为批量的方式。

        大体来说，增量方式仅关心当前时刻的状态估计 x_k。 而对之前的状态则不多考虑。

        相对的批量方法可以在更大范围内达到最优化，被认为优于传统的滤波器，而成为当前视觉slam的主流方式。

        理论上批量方式更容易介绍，理解了批量方法，也能更容易理解增量的方法。

        本节重点介绍以非线性优化为主的批量优化方法，卡尔曼滤波器及更深入知识在后面的章节后会进行讨论。

        由于讨论的是批量方法，考虑从1到N的所有时刻，并假设有M个路标点，定义所有时刻的机器人位姿和路标点坐标为：

        同样用不带下标的u表示所有时刻的输入，z表示所有时刻的观测数据，我们说对机器人状态估计从概率学的观点来看，就是已知输入数据u和观测数据z的条件下，求状态x，y的条件分布：

        特别的。当我们不知道控制输入，只有一张张图像的时候，即只考虑观测方程带来的数据时，相当于估计P（x，y | z ）的条件概率分布，此问题也称为SfM，即如何从许多图像中重建三维空间结构。

        为了估计状态变量的条件分布，利用贝叶斯法则，有

        贝叶斯法则左侧称为后验概率。右侧的P(z|x)称为似然（Likehood）。另一部分P(x)称为先验（Prior）。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化则是可行的：

        请注意贝叶斯法则的分母部分与代估计的状态x，y无关，因而可以忽略。

        贝叶斯法则告诉我们，求解最大后验概率等价于最大化似然和先验的乘积。当然我们也可以说：对不起，我不知道机器人位姿或路标大概在什么地方，此时就没有了先验。可以求解最大似然估计（Maximize Likehood Estimation，MLE）：

        直观的讲，似然是指“在现在的位置下可能产生怎样的观测数据？”

        由于我们知道观测数据，所以最大似然估计可以理解成在什么样的状态下，最可能产生现在观测到的数据，这就是最大似然估计的直观意义。

        
        
        

        这一章有很多概率的东西，有一个博主的博客把这方面的一些知识讲得通俗易懂，我也是借助于他的博客才理解的，下面把链接放在下面：

        学习资料链接: link

6.1.2. 最小二乘的引出

        我们如何求最大似然估计呢？在高斯分布的假设下，最大似然有较简单的形式：

        由于我们假设了噪声项v_k~N(0，Q_k,j)，所以观测数据的条件概率为：

        这也是一个高斯分布，考虑单次观测的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然。

高斯分布在负对数下有较好的数学表达形式，而且他能为我们省下很多计算过程。

考虑任意高维高斯分布，他的概率密度函数展开形式为：

对其取负对数：

对数函数是单调递增的，因此求原函数的最大化就是求最小化负对数，看上面的式子，第一项跟x无关，省略，因此，只要求右侧的二次型项，就得到了对状态的最大似然估计。代入SLAM的观测模型，相当于在求：