【视觉SLAM入门】1. 基础知识,运动观测,旋转（旋转矩阵，轴角，欧拉角，四元数）和eigen库基础
"山薮藏疾"
-
- 1. 运动与观测
-
- 1.1 通用运动方程
- 1.2 通用观测方程
- 1.3 对SLAM的认识
- 2. 三维运动
-
- 2.1 旋转与平移
- 2.2 变换矩阵
- 2.3 矩阵知识补充
- 2.4 旋转向量
- 2.5 欧拉角
- 2.6 四元数
- 2.7 其他变换
- 3. 编程基础
-
- 3.1 链接库说明
- 3.2 eigen库
- 本节及下一节讨论围绕这个问题展开 机器人如何表示自身位姿？ \quad\large\textcolor{red}{机器人如何表示自身位姿？} 机器人如何表示自身位姿？
注： 以后的方程中如未说明，x表示机器人自身位置，y表示路标（地图中的点），观测数据用z表示，将这个过程看做离散的，则时间从1到…
轨迹：由x构成的集合；
1. 运动与观测
1.1 通用运动方程
x k = f ( x k − 1 , u k , w k ) \qquad\qquad\qquad x_k = f(x_{k-1}, u_k, w_k) xk=f(xk−1,uk,wk)
- 上式表示 x k − 1 到 x k x_{k-1}到x_k xk−1到xk的过程， f f f是通用的， u k u_k uk表示输入(运动传感器的读数)：例如机器人自身的轮速计等， w k w_k wk表示误差
参数化举例：
设机器人在二维平面中运动，则位姿可以由x,y,和转角来描述，即 x k = [ x , y , θ ] k T x_k = [x, y, \theta]_k^T xk=[x,y,θ]kT,同时机器人身上的编码器等传感器可得 u k = [ Δ x , Δ y , Δ θ ] k T u_k = [\Delta x, \Delta y, \Delta \theta]_k^T uk=[Δx,Δy,Δθ]kT，则带入上述运动方程可得：
[ x y θ ] k = [ x y θ ] k − 1 + [ Δ x Δ y Δ θ ] k + w k \begin{bmatrix} x\\y\\ \theta \end{bmatrix}_k= \begin{bmatrix} x\\y\\ \theta \end{bmatrix}_{k-1} + \begin{bmatrix} \Delta x\\\Delta y\\\Delta \theta \end{bmatrix}_k + w_k xyθ k= xyθ k−1+ ΔxΔyΔθ k+wk
1.2 通用观测方程
z k , j = h ( y j , x k , v k , j ) \qquad\qquad\qquad z_{k,j} = h(y_j, x_k, v_{k, j}) zk,j=h(yj,xk,vk,j)
- 上式表示在 x k \;x_k xk 上看到路标 y j \;y_j yj 产生了观测数据 z k , j \;z_{k,j} zk,j， v k , j \;v_{k,j} vk,j表示误差
参数化举例：
设机器人装有一个2D激光雷达，能读到距离 r \;r r 和 夹角 θ \; \theta θ 两个值，在2D世界中，记一个landmark(路标点)为 y = [ p x , p y ] T \;y = [p_x, p_y]^T y=[px,py]T 观测数据为 z = [ r , ϕ ] T \; z = [r, \phi]^T z=[r,ϕ]T， 则观测方程可如下：
[ r ϕ ] = [ ( p x − x ) 2 + ( p y − y ) 2 arctan ( p y − y p x − x ) ] + v \begin{bmatrix} r\\ \phi \end{bmatrix} = \begin{bmatrix} \sqrt{(p_x - x)^2 + (p_y - y)^2} \\ \arctan(\frac{p_y - y}{p_x-x}) \end{bmatrix} + v [rϕ]=[(px−x)2+(py−y)2arctan(px−xpy−y)]+v
对视觉SLAM而言，观测方程就是 “对路标点拍摄后，得到了图
像的（具体是图像中的像素）” 的过程
1.3 对SLAM的认识
知道运动测量的数据 u \;u u，传感器的数据 z \; z z ，求解定位 x \; x x 和建图 y \; y y 的问题
⇓ ⇓ ⇓ \qquad \qquad \qquad \qquad \qquad \Downarrow \Downarrow \Downarrow ⇓⇓⇓
状态估计问题：带有噪声的数据 ⟹ \; \Longrightarrow\; ⟹内部隐藏的状态变量
噪声： 分为 G u a s s i a n Guassian Guassian 和 N o n − G u a s s i a n Non-Guassian Non−Guassian
两个方程： 分为 L i n e a r Linear Linear 和 N o n − L i n e a r Non-Linear Non−Linear
**线性高斯系统(LG)**是无偏的(样本均值近似等于总体均值)，可以用KF求解；
非线性非高斯(NLNG)系统，现在主要用EKF->粒子滤波器->图优化（时间顺序），图优化占资源，且效果明显好，优先选择。
2. 三维运动
2.1 旋转与平移
a 和 a ‘ a和a` a和a‘分别是两个坐标系下的同一个点，它们在各自空间用坐标和基底表示如下：
[ e 1 e 2 e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ‘ e 2 ‘ e 3 ‘ ] [ a 1 ‘ a 2 ‘ a 3 ‘ ] \begin{bmatrix} e_1&e_2&e_3 \end{bmatrix}\begin{bmatrix} a_1\\a_2\\a_3 \end{bmatrix}= \begin{bmatrix} e_1^`&e_2^`&e_3^`\end{bmatrix}\begin{bmatrix} a_1^`\\a_2^`\\a_3^`\end{bmatrix} [e1e2e3] a1a2a3 =[e1‘e2‘e3‘] a1‘a2‘a3‘
变换如下：
中间的 R \;R R 称为旋转矩阵，是行列式为1的正交阵，定义 R \;R R 如下：
S O ( n ) \qquad \qquad \qquad SO(n) SO(n) = { R ∈ R n x n ∣ R R T = I , d e t ( R ) = 1 R \in \R^{nxn} | RR^T=I, det(R)=1 R∈Rnxn∣RRT=I,det(R)=1}
解释： S O ( n ) SO(n) SO(n)是特殊正交群(Special Orthogonal Group)。
则带上平移量的变换为：
a ′ = R a + t a'=Ra + t a′=Ra+t 相反旋转为： a = R T a ′ − t = R − 1 a ′ − t a = R^T a' -t= R^{-1}a'-t a=RTa′−t=R−1a′−t
2.2 变换矩阵
T T T(Transform matrix)
避免如上表示多次变换的形式过长，引入变换矩阵T，如 b = T 1 a c = T 2 b ⟹ c = T 2 T 1 a \quad b=T_1a\quad c=T_2b \Longrightarrow c=T_2T_1a b=T1ac=T2b⟹c=T2T1a
它可以定义如下(引入齐次坐标后)：
S E ( 3 ) \qquad \qquad \qquad SE(3) SE(3) = { T = [ R t 0 T 1 ] ∈ R 4 x 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 T= \begin{bmatrix} \quad R&t&\\\quad0^T&1 \end{bmatrix} \in \R^{4x4} | R \in SO(3), t \in \R^3 T=[R0Tt1]∈R4x4∣R∈SO(3),t∈R3}
同样 T − 1 \quad T^{-1} \quad T−1 表示一个反向的变换，不区分齐次坐标 a ~ \;\tilde{a}\; a~和非齐次坐标 a \;a\; a的区别，默认是符合我们运算的那种。
2.3 矩阵知识补充
- 正交矩阵的转置 = = =它的逆
- 转置性质 ( A B ) T = B T A T \; (AB)^T = B^TA^T (AB)T=BTAT
- 反对称矩阵 A = − A T A=-A^T A=−AT，且主对角线元素均为0
- 实矩阵乘以它的转置得到的矩阵是实对称矩阵
- 矩阵可以用来解方程，直接求逆： A x = b ⟹ x = b A − 1 Ax=b \Longrightarrow x = b A^{-1} Ax=b⟹x=bA−1 (费时间)
- 用QR分解: A x = b ⇒ A = Q R ⇒ Q R x = b ⇒ R x = Q T b Ax=b \Rightarrow A=QR \Rightarrow QRx=b \Rightarrow Rx=Q^Tb Ax=b⇒A=QR⇒QRx=b⇒Rx=QTb (Q是N阶正交阵，R是上三角)(省一半时间)
2.4 旋转向量
S O ( 3 ) SO(3) SO(3)用9个量表示旋转，正交且 d e t ( R ) det(R) det(R)为1是它的约束， S E ( 3 ) SE(3) SE(3)也是一样的。因此不够紧凑、约束条件对求解的限制等都成为了它的问题。
旋转向量： 一个三维向量，用一个旋转轴 n \; n n 和 旋转角 θ \; \theta θ 表示.
变换矩阵 \;\;\; 在这种表示下 一个旋转向量+一个平移向量就可以表示。
转换关系: \;\; 通过罗德里格斯(Rodrigues’s Formula)公式
\qquad \qquad \qquad\qquad \qquad 旋转向量 ⟹ \Longrightarrow ⟹ 旋转矩阵
- 设一旋转轴 n \;n n，角度 θ \;\theta θ 的旋转表示为 n θ \; n\theta nθ，和旋转矩阵 R R R 表示转换如下：
R = cos θ ⋅ I + ( 1 − cos θ ) n n T + sin θ ⋅ n \qquad \qquad \qquad R = \cos\theta \cdot I + (1-\cos\theta)nn^T+\sin\theta \cdot n R=cosθ⋅I+(1−cosθ)nnT+sinθ⋅n^
- 其中^表示向量的反对称运算，参考视觉SLAM14讲中外积的表示
\qquad \qquad \qquad\qquad \qquad 旋转矩阵 ⟹ \Longrightarrow ⟹ 旋转向量
- 如下：
θ = arccos ( t r ( R ) − 1 2 ) \qquad\qquad\qquad\qquad\theta = \arccos(\frac{tr(R)-1}{2}) θ=arccos(2tr(R)−1)
R n = n \qquad\qquad\qquad\qquad \qquad Rn = n Rn=n
- 转轴在旋转前后不发生变化。转轴 n 是矩阵 R 特征值 1 对应的特征向量。求解此方程，再归一化，就得到了旋转轴。
2.5 欧拉角
\qquad 直观，但是有奇异性—“万向锁”（Gimbal Lock），调试观察时可转为欧拉角，一般不用于计算，以下旋转是有先后顺序的，以一种为例(Z-Y-X)，表示如下：
y a w yaw yaw - 偏航角 - 绕 Z Z Z 轴转
p i t c h pitch pitch - 俯仰角 - 绕旋转之后的 Y Y Y 轴转
r o l l roll roll - 偏航角 - 绕旋转之后的 X X X 轴转
2.6 四元数
Quaternion，消除欧拉角的奇异性且紧凑的表示法。以地球纬度 ± 9 0 o \pm 90^o ±90o为例，仅用两个坐标无法表示。四元数用四个数表示旋转。一个实部，三个虚部(或向量)。
q = q 0 + q 1 i + q 2 j + q 3 k \qquad \qquad\qquad \qquad\qquad \qquad\qquad q=q_0 +q_1i+q_2j+q_3k q=q0+q1i+q2j+q3k
满足
f ( x ) = { i 2 = j 2 = k 2 = − 1 i j = k , j i = − k j k = i , k j = − i k i = j , i k = − j f(x)= \begin{cases} i^2=j^2=k^2=-1 \\ij=k,ji=-k \\jk=i,kj=-i \\ki=j,ik=-j \end{cases} f(x)=⎩ ⎨ ⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
用四元数表示旋转
\qquad 假设某个旋转是绕单位向量 n = [ n x , n y , n z ] T \;n = [nx , ny , nz ]^T n=[nx,ny,nz]T 进行了角度为 θ \;\theta θ 的旋转，那么这个旋转的四元数形式为：
q = [ cos θ 2 , n x sin θ 2 , n y sin θ 2 , n z sin θ 2 ] ( 1 ) \qquad\qquad\qquad\qquad q=\begin{bmatrix}\cos {\frac{\theta}{2}}, n_x\sin {\frac{\theta}{2}}, n_y\sin {\frac{\theta}{2}}, n_z\sin {\frac{\theta}{2}}\end{bmatrix} \qquad\qquad(1) q=[cos2θ,nxsin2θ,nysin2θ,nzsin2θ](1)
\qquad 要对点 p = [ x , y , z ] p=[x,y,z] p=[x,y,z]进行旋转，轴角法 n , θ n, \theta n,θ，表示这个旋转过程如下：
- 首先用式1表述这个点(三个虚轴)
p = [ 0 , x , y , z ] \qquad\qquad\qquad\qquad p=[0,x,y,z] p=[0,x,y,z] - 旋转后的点 p ′ \;p' p′
p ′ = q p q − 1 \qquad\qquad\qquad\qquad p'=qpq^{-1} p′=qpq−1
- 计算结果实部为0，故是纯虚四元数，虚部三个分量表示旋转后的3D点的坐标。
- 同理，四元数 ⟺ \Longleftrightarrow ⟺ 旋转向量 ⟺ \Longleftrightarrow ⟺ 旋转矩阵 也可相互转化，较为容易。
2.7 其他变换
以上都是欧式变换，除此之外还存在如下几种SLAM中可能用到的变换
- **相似变换：**允许物体均匀缩放；
- 仿射变换；
- **射影变换；**等
3. 编程基础
3.1 链接库说明
静态库.a和动态库.so：所有库都是一些函数打包后的集合，差别在于静态库每次被调用都会生成一个副本，而共享库则只有一个副本，更省空间。
库+头文件的意义: 库文件是一个压缩包，里头带有编译好的二进制函数。为了让别人（或
者自己）使用这个库，我们需要提供一个头文件，基本是这个库的使用说明啦。因此，对于库
的使用者，只要拿到了头文件和库文件，就可以调用这个库。
构建动态库：在CMakeLists.txt中，动态库的编译如下:参数1是名称，3是要被编译成的文件
add_library( slam_shared SHARED SLAM.cpp )
使用该库：如下：
add_executable(useHello useHello.cpp)
target_link_libraries(useHello slam_shared)
3.2 eigen库
是一个很有意思的库，纯用头文件编写，没有.so,.a等库，所以调用的时候只要保证头文件的路径正确即可。在很多资料中头文件包含是这样的
- 到
/usr/include/eigen3/Eigen/下将eigen3改名为Eigen，且将原Eigen的文件全部上移一层到Eigen下 - 将该路径加入头文件搜索路径中,在VSCODE中很容易实现；或在CmakeLists.txt中加入
include_directories( "/usr/include/eigen3" ) - 改一个头文件包含方式，前边
如果eigen有关的编译报错，直接找报错信息的大写部分就大概能知道。
用eigen库表示一般的几种表示旋转的函数
