【视觉SLAM十四讲】【逐行代码带你解析】【适合纯小白 ps:因为我就是】（持续更新中）
视觉SLAM十四讲学习笔记【逐行代码带你解析】【适合纯小白 ps:因为我就是】（持续更新中）
- 前言
- ch2 初识SLAM
-
- 2.1.什么是SLAM
- 2.2经典视觉SLAM框架
-
- 2.2.1.传感器信息读取
- 2.2.2.前端视觉里程计
- 2.2.3.后端非线性优化
- 2.2.4.回环检测
- 2.2.5.建图
- 2.3.SLAM问题的数学表述
- 2.4.ch2的实践
- ch3 三维空间刚体运动
-
- 3.1.旋转矩阵
-
- 3.1.1.点、向量和坐标系
- 3.1.2.坐标系间的欧氏变换
- 3.1.3.变换矩阵与齐次坐标
- 3.2实践部分：Eigen3
- 3.3.旋转向量与欧拉角
-
- 3.3.1.旋转向量（或轴角/角轴，Axis-Angle）
- 3.3.2.欧拉角（euler angles）
- 3.4.四元数
-
- 3.4.1.四元数的定义
- 3.4.2.四元数的运算（这一部分直接用了书上的截图）
- 3.4.3.四元数表示旋转（这一部分直接用了书上的截图）
- 3.4.4.四元数表示的旋转与其他表示方式的转换
- 3.5.相似、仿射、射影变换
- 3.6.实践：Eigen几何模块
-
- 3.6.1.Eigen几何模块的数据演示
- 3.6.2.实际的坐标变换例子
- 3.7.可视化演示
-
- 3.7.1.Ubuntu20下Pangolin的安装
- 3.7.2.实践：可视化轨迹
- ch4 李群与李代数
-
- 4.1.李群与李代数基础
-
- 4.1.1.群
- 4.1.2.李代数的引出
- 4.1.3.李代数的定义
- 4.1.4.李代数so(3)
- 4.1.5.李代数se(3)
- 4.2.指数与对数映射
-
- 4.2.1.SO(3)上的指数映射
- 4.2.2.SE(3)上的指数映射
- 4.3.李代数求导与扰动模型
-
- 4.3.1.BCH公式近似形式
- 4.3.2.SO(3)上的李代数求导
- 4.3.3.李代数求导
- 4.3.4.扰动模型（左乘）
- 4.3.5.SE(3)上的李代数求导
- 4.4.实践：Sophus
-
- 4.4.1.Sophus的基本使用方法
- 4.4.2.例子：评估轨迹的误差
- ch5 相机与图像
-
- 5.1.相机模型
-
- 5.1.1.针孔相机模型
- 5.1.2.畸变模型
- 5.1.3.双目相机模型
- 5.1.4.RGB-D 相机模型
- 5.2.图像
- 5.3.实践：计算机中的图像
-
- 5.3.1.OpenCV的基本使用方法
-
- 1.安装OpenCV4依赖的库
- 2.下载OpenCV4
- 3.编译安装
- 4.设置库的搜索路径
- 5.将/etc/ld.so.conf.d中的数据读入缓存
- 6.添加pkg-config环境变量
- 7.cpp代码
- 8.CMakeLists.txt
- 9.执行代码
- 5.3.2.图像去畸变
- 5.4.实践：3D视觉
-
- 5.4.1.双目视觉
- 5.4.2.RGB-D视觉
- ch6 非线性优化
-
- 6.1.状态估计问题
-
- 6.1.1.批量状态估计与最大后验状态估计
- 6.1.2. 最小二乘的引出
前言
大一大二东学学西学学了一大堆，这个暑假想往难一点的方向探索探索，以后想从事机器人行业，所以先从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问题的数学表述
第一个方程是运动方程，xk是当前位置，xk-1是前一时刻的位置，uk是当前输入的数据，wk是噪声。
第二个方程是观测方程，zk,j是观测数据，观测方程描述的是，当机器人在xk位置上看到某个路标点yj时，产生了一个观测数据zk,j，vk,j是这次观测里的噪声。由于观测所用的传感器形式更多，这里的观测数据z及观测方程h也有许多不同的形式。
有了这两个方程，我们就成功节=的将SLAM“问题建模成了一个状态估计问题。
2.4.ch2的实践
书里这部分讲了如何安装ubuntu，并配置c++环境，我对这部分比较熟悉，就不写了。
ch3 三维空间刚体运动
这部分花的时间还是挺多的，如果上天再给我一次机会，我一定在大二上学期认认真真的好好的学线代。
3.1.旋转矩阵
3.1.1.点、向量和坐标系
内积：
外积：
3.1.2.坐标系间的欧氏变换
两个坐标系间的运动由一个旋转加一个平移组成，这种运动我们称之为刚体运动。
我们说这两个坐标系之间，相差了一个欧氏变换。
我们从两个方面来学习这个破玩意儿。
旋转
同一个向量a在两个坐标系下的坐标为(a1,a2,a3)T和(a1’,a2’,a3’)T，这两个坐标的基向量为(e1,e2,e3)和(e1’,e2’,e3’)，所以根据坐标的定义，有：
对等式左右同时左乘
得到了：
中间的这个3X3的矩阵R，我们就称之为旋转矩阵，该矩阵的各个分量是两个坐标基系的内积，由于基向量长度为1，所以实际上是各基向量夹角的余弦值，所以这个矩阵也称方向余弦矩阵。
旋转矩阵还是一个行列式为+1的正交矩阵，反之，行列式为+1的正交矩阵也是一个旋转矩阵。将n维旋转矩阵的集合定义如下：
称之为特殊正交群。
旋转矩阵的逆（即转置）描述了一个相反的旋转，有：
（正交矩阵的性质：逆=转置）
这里附上我丑丑的手写笔记（别笑话我）
在欧氏变换中，除了旋转，还有一个东东：平移
平移记作向量t，那么把旋转和平移合到一起，就有了：
a1 = R12a2+t12 式1
至此，我们终于能描述坐标系的变换啦！！
3.1.3.变换矩阵与齐次坐标
如果，我们参照上面得出的式子a1 = R12a2+t12，当我们要做两次变换时（例如a3变到a2再变到a1），就有以下推导公式：
a2 = R23a3+t23
a1 = R12a2+t12
联立得：a1 = R12(R23a3+t23)+t12
这样子的形式在变换多次时就会显得很麻烦，所以我们引入齐次坐标和变换矩阵，改写式1得到：
矩阵T我们称之为变换矩阵。
这时两次变换（如上）就可以改写为a1= T2T3a3
变换矩阵的特点：左上角为旋转矩阵，右上角为平移向量，左下角为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 就是未知行列数的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()对矩阵进行类型转换。
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 终端打印出来的结果：
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、R1、R2），PR1代表在R1坐标系下的坐标，我们要把他转换成在R2坐标系中的坐标，但是我们题目中对于一号和二号给出的四元数和平移向量是相对于世界坐标系的，我们能先把PR1转换成PW，也就是所求点相对于世界的坐标系：
PW = TW,R1 * PR1
进一步，我们就能求出所求点相对于世界的坐标：
PR2 = TR2,W * PW
综合这两式子：
PR2 = TR2,W * TW,R1 * PR1
接下来是程序部分：
//include各种库
#include 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 是一个用于分配 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)是定义在**R3**上的向量，可以生成一个反对称矩阵：
此李代数的李括号为：
李代数so（3）：
4.1.5.李代数se(3)
特殊欧氏群SE(3)对应的李代数se(3)位于**R6**空间中：
此李代数的李括号为：
4.2.指数与对数映射
4.2.1.SO(3)上的指数映射
exp(A)，这个东西是数学表达式来着，我最开始也不知道他是啥东东。其实他就是e的A次方。也就是：
exp(A) = eA
对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 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 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 是C++标准库中用于处理时间的头文件。它提供了一组类和函数，用于测量时间间隔、计时和延时等操作。
#include 是OpenCV库中用于图形用户界面 (GUI) 的头文件之一。它包含了图像显示、
窗口管理和用户交互等功能的定义。
在使用OpenCV进行图像处理和图形界面操作时，通常需要包含头文件，以便
使用其中定义的类和函数。
以下是一些常用的类和功能：
cv::imshow：用于显示图像的函数，可以将图像数据显示在屏幕上的窗口中。
cv::waitKey：等待键盘输入的函数，通常与cv::imshow一起使用，用于在图像显示期间等待按键触发。
cv::namedWindow：创建一个指定名称的图像窗口。
cv::destroyAllWindows：关闭所有图像窗口。
cv::imwrite：将图像数据保存为图像文件。
cv::VideoCapture：用于从摄像头或视频文件中捕获视频帧。
cv::VideoWriter：用于将视频帧写入视频文件。
除了上述功能，头文件还包含了其他一些与图形界面相关的函数和数据结构。通
过包含这个头文件，可以使用OpenCV库提供的图形界面功能，例如图像显示、窗口管理、用户交互和视频处理等
操作。
是OpenCV库中的核心头文件之一，它包含了一些最基本的图像处理功能和数据结构
的定义。
在使用OpenCV进行图像处理时，通常需要包含头文件，以便使用其中定义的类和函数。
以下是一些常用的类和功能：
cv::Mat：表示图像或多维数组的数据结构。它是OpenCV中最常用的类之一，用于存储和操作图像数据。
cv::Size：表示图像或矩形区域的尺寸，包含宽度和高度。
cv::Point：表示二维平面上的点的坐标，包含x和y。
cv::Scalar：表示颜色或数值的数据结构，包含多个通道的值。
cv::imread：用于读取图像文件并返回cv::Mat对象。
cv::imshow：显示图像窗口，将图像数据显示在屏幕上。
cv::waitKey：等待键盘输入的函数，通常与cv::imshow一起使用。
cv::destroyAllWindows：关闭所有图像窗口。
除了上述功能，头文件还包含了其他一些与图像处理相关的数据结构和函数。通过包含
这个头文件，可以使用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 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 是一个指向 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 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模型，由一个运动方程和一个观测方程组成：
在运动和观测方程中，我们通常假设两个噪声项：wk, vk,j, 满足零均值的高斯分布：
在噪声的影响下，我们希望通过带噪声的数据z和u推断位姿x地图y(以及他们的概率分布)，这就构成了一个状态估计的问题。
处理这个状态估计问题，大致分为两种方法，由于在slam的过程中，这些数据是随时间逐渐到来的，所以我们应该持有一个当前时刻的估计状态，然后用新的数据来更新它，这种方式我们称之为增量/渐进的方式，或者叫它滤波器，另一种方式则是把数据攒起来并处理，这种方式称为批量的方式。
大体来说，增量方式仅关心当前时刻的状态估计 xk。 而对之前的状态则不多考虑。
相对的批量方法可以在更大范围内达到最优化，被认为优于传统的滤波器，而成为当前视觉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. 最小二乘的引出
我们如何求最大似然估计呢？在高斯分布的假设下，最大似然有较简单的形式：
由于我们假设了噪声项vk~N(0，Qk,j)，所以观测数据的条件概率为：
这也是一个高斯分布，考虑单次观测的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然。
高斯分布在负对数下有较好的数学表达形式，而且他能为我们省下很多计算过程。
考虑任意高维高斯分布，他的概率密度函数展开形式为：
对其取负对数：
对数函数是单调递增的，因此求原函数的最大化就是求最小化负对数，看上面的式子，第一项跟x无关，省略，因此，只要求右侧的二次型项，就得到了对状态的最大似然估计。代入SLAM的观测模型，相当于在求：