SLAM⼊门之《SLAM14讲》笔记
Simon's《SLAM 14讲》笔记
本⼈⼩⽩⼀枚，因选了搭建3D虚拟环境的课题需要学习SLAM相关知识，被迫在⼀个星期之内学完了《SLAM 14讲》，从此感觉到了SLAM的有趣和神奇。为了时常能温习书中所涉及的原理与模型，也为了能帮助⼴⼤刚⼊这个坑的⼩⽩们，便想写这篇书中每章内容的摘要与⼀些知识的理解。
2. 第⼆讲：初识SLAM
视觉SLAM框架
Linux下程序的编译与运⾏
2.1. SLAM框架
很清很清的什么视觉⾥程计（Visual Odometry，VO）：前端，估算相邻图像间相机的运动。
⾮线性优化（Optimization）：后端，优化得到的相机位姿。
回环检测（Loop Closing）：判断相机是否到过先前位置，传给后端以修正误差。
建图（Mapping）：根据估计轨迹建⽴所需的地图。
2.2. Linux下程序的编译
参见
3. 第三讲：三维空间刚体运动
业余赚刚体在三维空间⾥的旋转
刚体在三维空间⾥的旋转加平移
本章代码使⽤了Eigen须在 “.cpp” ⽂件开头加上：
#include <Eigen/Core>
#include <Eigen/Den>
#include <Eigen/Geometry>
3.1. 刚体在三维空间⾥的旋转
Eigen中矩阵的基本操作：
Eigen ::Matrix <type , row , colon > //typedef: Matrix3d,
形容女生的词
Eigen ::MatrixXd //unkown size matrix
matrix (i , j ) //the value of i th row and j th colon
//the operations
matrix .transpo (); matrix .trace (); matrix .sum (); matrix .inver (); matrix .determinant ();
//find the proper values and vectors
Eigen ::SelfAdjointEigenSolver <Eigen ::Matrix3d > solver (matrix .transport ()*matrix );
solver .eigenvalues ();
solver .eigenvectors ();
//solve Matrix_NN * x = v_Nd
x = matrix_NN .colpivHouholderQr ().solve (v_Nd );
基本数学知识：
3.1.1. 旋转矩阵 设刚体上某点 ，在经过旋转之后变成 ，我们可以⽤旋转矩阵 来表⽰该旋转，则有 。旋转矩阵 是⼀个正交矩阵（Matrice Orthogonale），即有性质 .3.1.2. 旋转向量 旋转向量 意为刚体绕 旋转 。根据 Rodrigues’s Formula ， 旋转矩阵与旋转向量的转化关系如下:
Eigen ::AngleAxisd rotation_vector (theta , Eigen ::Vector3d (x , y , z )); //rotation vector
rotation_matrix = rotation_vector .toRotationMatrix (); //transform rotation vector to rotation matrix
x_rotated = rotation_vector *x ; //calculate rotated vector x'
小米vr
x_rotated = rotation_matrix *x ; //calculate rotated vector x'
3.1.3. 四元数（Quaternion ）
以下与书中相同，设四元数第⼀项为实数项。设在三维空间某⼀点 ，旋转向量 可记为 。则旋转后的向量 。
四元数与旋转矩阵的转换关系如下（并不唯⼀）：
设 [x y z ]=T p , p =Λ⎣⎡0z −y −z 0x
y −x 0⎦⎤R
x =[x ,x ,x ]123T x =′[x ,x ,x ]1′2′3′
T R x =′R .x R R R =T I θn
θn n θR =cos (θ)I +(1−cos (θ)nn +T sin (θ)n )
Λ{
tr (R )=1+2cos (θ)
Rn =n p =[0,x ,y ,z ]θn q =[cos (θ),n sin (θ)]p =′qpq −1q =q +0q i +1q j +2q k
3
则有
或者
q = Eigen ::Quaterniond (w , x , y , z ) //create a quaternion from four values
q = Eigen ::Quaterniond (rotation_vector ) //create a quaternion from rotaion vector
q = Eigen ::Quaterniond (rotation_matrix ) //create a quaternion from rotaion matrix
等红灯
x_rotated = q * x //calculate rotated vector x'
3.2. 刚体在三维空间⾥的旋转加平移
3.2.1. 变换矩阵 设刚体上某点 ，在经过旋转平移之后变成 ，则有 ，这⾥的 为平移向量。我们记 ，然后把 与 写成齐次坐标的形式， 即 ，则有 。
//create a transform matrix
Eigen ::Isometry3d T = Eigen ::Isometry3d ::Identity ();
T .rotate (rotation_vector );
/*
*/
T .pretranslate (Eigen ::Vector3d (x , y , z ));
4. 第四讲：李群与李代数李群与李代数
BCH公式与近似形式
扰动模型（左扰动）
本章代码使⽤了Eigen须在 “.cpp” ⽂件开头加上：
#include "sophus/so3.h"
#include "sophus/3.h"
4.1. 李群与李代数
三维旋转矩阵构成了特殊正交群：，
变换矩阵构成了特殊欧式群：，
R =⎣⎡1−2q −q 2232
2q q −2q q 12032q q +2q q 13022q q +2q q 12031−2q −2q 12322q q −2q q 23012q q −2q q 13022q q +2q q 23011−2q −2q 1222⎦⎤q =0,q =2tr (R )+1
1,q =4q 0m −m 23322,q =
4q 0m −m 311334q 0m −m 1221T
x =[x ,x ,x ]123T x =′[x ,x ,x ]1′2′3′
T x =′Rx +t t T =[R 0T t 1]x ′x x =[x ,x ,x ,1],x =123T ′[x ,x ,x ,1]1′2′3′T x =′Tx SO (3)={R ∈R ∣R R =3×3T I ,det (R )=1}SE (3)={T =∈[R 0T t 1]R ∣R ∈4×4SO (3),t ∈R }3
⼀个⼩公式： 李代数：
，
指数和对数映射：
与 ：
与 ：
Sophus ::SO3 SO3_R (R ); //construct SO_R by a rotation matrix
Sophus ::SO3 SO3_v (0, 0, M_PI /2); //construct SO_R by a rotation vector, using <cmath> std::M_PI/2Sophus ::SO3 SO3_q (q ); //construct SO_R by a quaternion
土木工程专业课Eigen ::Vector3d so3 = SO3_R .log (); //logarithm map
Sophus ::SO3::hat (so3); //so3^
Sophus ::SO3::vee (SO3); //SO3v
Sophus ::SO3::exp (so3); //exp(so3^)
山东职业学院排名
Sophus ::SE3 SE3_Rt (R , t ); //construct SE_Rt by a rotation matrix and a translation vector(Eigen::Vector3d)Sophus ::SE3 SE3_qt (q , t ); //construct SE_Rt by a quaternion and a translation vector
//how to update
Eigen ::Matrix <double , 6, 1> update_3;
update_3.tZero ();
update_3(0, 0) = 1e-4d ;
Sophus ::SE3 SE3_updated = Sophus ::SE3::exp (update_3)*SE3_Rt ;
4.2. BCH 公式与近似模型
BCH近似公式：
，，
微⼩扰动公式：
a =ΛA =,A =⎣⎡0
a 3−a 2
−a 30a 1a 2−a 10⎦⎤V a so (3)={ϕ∈R ,Φ=3ϕ∈ΛR }3×3 (3)={ξ=∈[ρϕ]R ,ρ∈6R ,ϕ∈3so (3),ξ=Λ∈[ϕΛ0T ρ0]R }
4×4SO (3)so (3)exp (θa )=Λcos (θ)I +(1−cos (θ)aa +T sin (θ)a )
Λθ=arccos ,Ra =2tr (R )−1a
SE (3) (3)exp (ξ)=Λ,J =[exp (ϕ)Λ0
T Jρ
1]I +θsin (θ)(1−)aa +θsin (θ)T a θ1−cos (θ)Λθ=arccos ,Ra =2tr (R )−1a ,t =Jρ
ln (exp (ϕ)exp (ϕ))≈1Λ2ΛV {J (ϕ)ϕ+ϕ2) if ϕ is small l 2−111J (ϕ)ϕ+ϕ1) if ϕ is small
r 1−122J =l J =
I +θsin (θ)(1−)aa +θsin (θ)T a θ1−cos (θ)ΛJ =l −1cot ()I +2θ2θ(1−cot ())aa −2θ2θT a 2θΛJ (ϕ)=r J (−ϕ)
l exp (Δϕ)exp (ϕ)=ΛΛexp ((ϕ+J (ϕ)Δϕ))l −1
Λ
中国吴江4.3. 扰动模型（左扰动）
上的李代数求导：
上的李代数求导：
5. 第五讲：相机与图像相机模型
相机的标定
本章要⽤到OpenCV，PCL与boost，PCL⽤于拼接云点图，OpenCV的具体⽤法请参照。使⽤boost快速读取图⽚：
#include <boost/format.hpp>
boost ::format fmt (./%s /%d .%s );
cv ::imread ((fmt %"director_name"%(number )%"file_type").str (), -1);
5.1. 相机模型
5.1.1. 针孔相机模型
设像素平⾯上某点 ，其对应的在世界坐标系下的某点 ，在相机归⼀化平⾯上对应的点为 。
我们有：
即使 化为齐次形式 ，exp ((ϕ+Δϕ))=Λexp ((J Δϕ))exp (ϕ)=l ΛΛexp (ϕ)exp ((J Δϕ))
Λr Λexp (Δξ)exp (ξ)=ΛΛexp ((ξ+J Δξ))
l −1
Λexp (ξ)exp (Δξ)=ΛΛexp ((ξ+J Δξ))r −1ΛSO (3)=∂φ∂Rp =φ→0lim φexp (φ)exp (ϕ)p −exp (ϕ)p ΛΛΛ=φ→0lim φ(1+φ)exp (ϕ)p −exp (ϕ)p ΛΛΛ=φ→0lim φφRp Λ−(Rp )ΛSE (3)=∂δξ∂Tp [I 0T −(Rp +t )Λ
0T ]
P =uv [u ,v ]T P =w [X ,Y ,Z ]T P =c [x ,y ,1]T ⎩⎨⎧u =f +c x Z X
x v =f +c y Z Y y P =uv [u ,v ]T P =uv [u ,v ,1]T
