视觉SLAM十四讲从理论到实践第二版源码调试笔记（理论基础1-6章）

2019-2020-2学期机器人工程专业需要开设SLAM技术课程，使用教材为视觉SLAM十四讲从理论到实践第二版。

为方便学生学习课程知识，将Arduino、ROS1、ROS2和SLAM集成到课程定制版镜像中。链接如下：

• https://blog.csdn.net/ZhangRelay/article/details/102596056

---

第一章 预备知识

关键词：数学、C++、Linux

学有余力的同学，请完成课后习题。

第二章 初识SLAM

关键词：SLAM、g++、IDE

g++ helloSLAM.cpp -o hello_slam

熟练使用：mkdir build , cmake .. , make等。

IDE环境使用KDevelop或者VSCode等都行。

第三章 三维空间刚体运动

关键词：矩阵、四元数、欧拉角、Eigen

全部内容：

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
[1,2,3;4,5,6]*[4,5,6]: 32 77
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
times 10: 
 6.80375   5.9688 -3.29554
-2.11234  8.23295  5.36459
 5.66198 -6.04897 -4.44451
inverse: 
-0.198521   2.22739    2.8357
  1.00605 -0.555135  -1.41603
 -1.62213   3.59308   3.28973
det: 0.208598
Eigen values = 
0.0242899
 0.992154
  1.80558
Eigen vectors = 
-0.549013 -0.735943  0.396198
 0.253452 -0.598296 -0.760134
-0.796459  0.316906 -0.514998
time of normal inverse is 0.312ms
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.182ms
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.054ms
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

./eigenGeometry

./coordinateTransform

./plotTrajectory

图示如下：

./visualizeGeometry 

图示如下：

第四章 李群与李代数

关键词：SO(3)、SE(3)、Sophus

./useSophus 

编译需要注意“sophus/se3.hpp”，路径要配置正确，如果不想配置，就sudo make install也可以。

全部内容如下：

SO(3) from matrix:
2.22045e-16          -1           0
          1 2.22045e-16           0
          0           0           1
SO(3) from quaternion:
2.22045e-16          -1           0
          1 2.22045e-16           0
          0           0           1
they are equal
so3 =      0      0 1.5708
so3 hat=
      0 -1.5708       0
 1.5708       0      -0
     -0       0       0
so3 hat vee=      0      0 1.5708
SO3 updated = 
          0          -1           0
          1           0     -0.0001
     0.0001 2.03288e-20           1
*******************************
SE3 from R,t= 
2.22045e-16          -1           0           1
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1
SE3 from q,t= 
2.22045e-16          -1           0           1
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1
se3 =  0.785398 -0.785398         0         0         0    1.5708
se3 hat = 
        0   -1.5708         0  0.785398
   1.5708         0        -0 -0.785398
       -0         0         0         0
        0         0         0         0
se3 hat vee =  0.785398 -0.785398         0         0         0    1.5708
SE3 updated = 
2.22045e-16          -1           0      1.0001
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1

./trajectoryError 

第五章 相机与图像

关键词：OpenCV

./imageBasics ubuntu.png

./undistortImage

 ./stereoVision

./joinMap

第六章 非线性优化

关键词：最小二乘法、高斯牛顿法、列文博格-马夸尔特法、Ceres、g2o

./gaussNewton

./ceresCurveFitting

如果出现：./g2oCurveFitting: error while loading shared libraries: libg2o_core.so: cannot open shared object file: No such file or directory

使用如下方式：

$ sudo gedit /etc/ld.so.conf

添加：/usr/local/lib

运行：sudo ldconfig

再次运行：

./g2oCurveFitting

对比求解时间：

• gaussNewton：solve time cost = 0.000938872 seconds.
• ceresCurveFitting：solve time cost = 0.00942216 seconds.
• g2oCurveFitting ：solve time cost = 0.000908874 seconds. 

依据电脑配置会有差异。

作业为完成1-6章课后习题。

---