其他章节：
第二讲：初识SLAM
三维空间刚体运动
本讲介绍视觉SLAM的基本问题之一：一个刚体在三维空间中的运动是如何描述的。
旋转矩阵
为了用数学语言描述物体的坐标，引入点和向量：在一个确定的坐标系下，也就是一个线性空间的基为( e 1 , e 2 , e 3 ) (e_1,e_2,e_3)(e1,e2,e3)，那么向量a aa的坐标为：
a = [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = a 1 e 1 + a 2 e 2 + a 3 e 3 a=[e_1,e_2,e_3]\begin{bmatrix}a_1 \\ a_2 \\ a_3 \end{bmatrix} = a_1e_1 + a_2e_2 + a_3e_3a=[e1,e2,e3]⎣⎡a1a2a3⎦⎤=a1e1+a2e2+a3e3
描述两个坐标系之间的旋转后平移的关系，统称为坐标系之间的变换关系。而欧式变换指的是：同一个向量在各个坐标系下的长度和夹角都不会发生变化。
设某个单位正交基( e 1 , e 2 , e 3 ) (e_1,e_2,e_3)(e1,e2,e3)经过一次选择变成了( e 1 ′ , e 2 ′ , e 3 ′ ) (e_1',e_2',e_3')(e1′,e2′,e3′)
，对于同一个向量a aa在两个坐标系下的坐标为[ a 1 , a 2 , a 3 ] T [a_1,a_2,a_3]^T[a1,a2,a3]T和[ a 1 ′ , a 2 ′ , a 3 ′ ] T [a_1',a_2',a_3']^T[a1′,a2′,a3′]T。根据坐标的定义：
同时左乘e T e^TeT，得到了矩阵R RR，也就是旋转矩阵。
根据旋转矩阵的性质，可定义旋转矩阵的集合，也就是特殊正交群：
S O ( n ) = { R ∈ R n ∗ n ∣ R R T = I , d e t ( R ) = 1 } SO(n)=\{R \in \Reals^{n\ast n} | RR^T = I, det(R) = 1\}SO(n)={R∈Rn∗n∣RRT=I,det(R)=1}
关于旋转矩阵是正交矩阵的证明，可以看：证明：旋转矩阵是正交矩阵
在欧氏变换中，除了旋转之外还有平移。那么世界坐标系中的向量a aa经过一次旋转(R RR)和一次平移(t tt)后，得到了a ′ a'a′，即：a ′ = R A + t a'=RA+ta′=RA+t。其中，t tt称为平移向量。
由于多次变换会导致形式越来越复杂， 因此，引入齐次坐标和变换矩阵重写式，这样的话就把旋转和平移写在同一个矩阵里了。其中，T TT称为变换矩阵，记a ~ = [ a 1 ] \widetilde{a} = {a \brack 1}a=[1a]。
旋转向量与欧拉角
虽然已经有旋转矩阵，但是有表达冗余和正交矩阵的约束的缺点，因此，引入轴角来刻画。首先，旋转向量：使用一个向量，其方向与旋转轴一致，而长度等于旋转角。
转换关系由罗德里格斯公式表明：
转角θ = a r c c o s ( t r ( R ) − 1 2 ) \theta=arccos(\frac{tr(R)-1}{2})θ=arccos(2tr(R)−1)；旋转轴R n = n Rn=nRn=n。
欧拉角是吧一个旋转分解成三次绕不同轴的旋转，比较常见的使用“偏航-俯仰-滚转(yaw-pitch-roll)”来描述
四元数
四元数是一种扩展的复数，有三个虚部，可以表达三维空间中的旋转：q = q 0 + q 1 i + q 2 j + q 3 k q=q_0+q_1i+q_2j+q_3kq=q0+q1i+q2j+q3k，有时候也用一个标量和一个向量来表达：q = [ s , v ] , s = q 0 ∈ R , v = [ q 1 , q 2 , q 3 ] T ∈ R 3 q=[s,v], s=q_0 \in R, v=[q_1,q_2,q_3]^T \in R^3q=[s,v],s=q0∈R,v=[q1,q2,q3]T∈R3。
虚部之间的关系：
小结表格来源：高翔视觉slam十四讲书籍习题(第三讲)
实践
Eigen
编程部分参考书，里面每个部分都有很详细的解释，CMakeLists的写法可以参考官方代码仓库：
cmake_minimum_required( VERSION 2.8 )
project( useEigen )
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
add_executable( eigenMatrix eigenMatrix.cpp )
使用cmake .生成Makefile，再用make编译。
运行结果如下，为了便于理解，多加了一些解释性的输出：
Eigen几何模块
运行结果如下：