SLAM基础篇01
SLAM基础篇02
最近在看高翔和张涛写的《视觉SLAM十四讲》,记录一下学习的内容,方便以后复习。这一系列文章更像是这本书的读书笔记,所以结构脉络都与该书相似;更推荐有时间的同学去看原书,写的更加详细易懂。欢迎大家一起探讨学习视觉SLAM相关的知识。
SLAM(simultaneous localization and mapping)全称即时定位与地图构建或并发建图与定位),顾名思义可以解决“定位”和“建图“两件事情。 SLAM 问题的本质:对运动主体自身和周围环境空间不确定性的估计。主流的slam技术应用有两种,分别是激光slam(基于激光雷达lidar 来建图导航)和视觉slam(vslam,基于单/双目摄像头视觉建图导航)。
经典的视觉 SLAM 框架是过去十几年内,研究者们总结的成果。这个框架本身,以及它所包含的算法已经基本定型,并且在许多视觉程序库和机器人程序库中已经提供。依靠这些算法,我们能够构建一个视觉 SLAM 系统,使之在正常的工作环境里实时进行定位与建图。如上图所示,我们八视觉slam分为几步:
由于相机通常是在某些时刻采集数据的,所以我们也只关心这些时刻的位置和地图。这就把一段连续时间的运动变成了离散时刻 t = 1, . . . , K 当中发生的事情。在这些时刻,用 x \bm{x} x 表示小萝卜自身的位置。于是各时刻的位置就记为 x1, . . . , xK,它们构成了机器人的轨迹。地图方面,我们设地图是由许多个路标(Landmark)组成的,而每个时刻,传感器会测量到一部分路标点,得到它们的观测数据。不妨设路标点一共有 N 个,用 y1, . . . , yN表示它们。
机器人的运动方程可用下式表示:
x k = f ( x k − 1 , u k , w k ) \boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}, \boldsymbol{w}_{k}\right) xk=f(xk−1,uk,wk)
其中, u k u_k uk是运动传感器的读数, w k w_k wk为噪声。
机器人的观测方程可用下式表示:
z k , j = h ( y j , x k , v k , j ) \boldsymbol{z}_{k, j}=h\left(\boldsymbol{y}_{j}, \boldsymbol{x}_{k}, \boldsymbol{v}_{k, j}\right) zk,j=h(yj,xk,vk,j)
这里, v k , j v_{k,j} vk,j是这次观测的噪声。
这两个方程描述了最基本的 SLAM 问题:当我们知道运动测量的读数 u,以及传感器的读数 z 时,如何求解定位问题(估计 x)和建图问题(估计 y)?这时,我们把 SLAM问题建模成了一个状态估计问题:如何通过带有噪声的测量数据,估计内部的、隐藏着的状态变量?
在机器人学中,我们习惯用一个坐标系来表达一个刚体的位姿,机器人的位置和姿态,其实就是机器人坐标系在世界坐标系的表达。我们使用齐次矩阵T来表达两个坐标系的变换关系,很显然其次矩阵T应该包括旋转和平移两个部分。
对于刚体的旋转,我们使用旋转矩阵 R \boldsymbol R R表示,她是一个行列式为1的正交矩阵。
我们可以吧旋转矩阵的的集合定义如下:
S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ( R ) = 1 } . S O(n)=\left\{R \in \mathbb{R}^{n \times n} \mid R R^{T}=I, \operatorname{det}(R)=1\right\} . SO(n)={R∈Rn×n∣RRT=I,det(R)=1}.
S O ( n ) S O(n) SO(n) 是特殊正交群 (Special Orthogonal Group) 的意思。“群” 的内容等会儿还会再说。这个集合由 n n n 维空间的旋转矩阵组成, 特别的, S O ( 3 ) S O(3) SO(3) 就是三维空间的旋转 了。通过旋转矩阵, 我们可以直接谈论两个坐标系之间的旋转变换, 而不用再从基开始谈 起了。换句话说, 旋转矩阵可以描述刚体的旋转。
对于刚体的平移,我们可以用一个平移向量 t \boldsymbol t t来表示。那么把向量 a \boldsymbol a a移动到 a ′ \boldsymbol a^{\prime} a′,可以表达为:
a ′ = R a + t a^{\prime}=R a+t a′=Ra+t
对于上面的这个式子,我们用矩阵的形式表达出来就是:
[ a ′ 1 ] = [ R t 0 T 1 ] [ a 1 ] ≜ T [ a 1 ] \left[\begin{array}{l}a^{\prime} \\ 1\end{array}\right]=\left[\begin{array}{ll}R & t \\ 0^{T} & 1\end{array}\right]\left[\begin{array}{l}a \\ 1\end{array}\right] \triangleq T\left[\begin{array}{l}a \\ 1\end{array}\right] [a′1]=[R0Tt1][a1]≜T[a1]
这里把我们向量的末尾加了一个“1”,变成了四维齐次坐标。这个T就是所谓的齐次变换矩阵,它有比较特殊的结构:左上为旋转矩阵,右侧为平移向量。左下为向量,右下为1。这种矩阵又称为特殊欧式群(Special Euclidean Group)
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } . S E(3)=\left\{T=\left[\begin{array}{cc} R & t \\ 0^{T} & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \mid R \in S O(3), t \in \mathbb{R}^{3}\right\} . SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}.
与 S O ( 3 ) S O(3) SO(3) 一样, 求解该矩阵的逆表示一个反向的变换:
T − 1 = [ R T − R T t 0 T 1 ] . T^{-1}=\left[\begin{array}{cc} R^{T} & -R^{T} t \\ 0^{T} & 1 \end{array}\right] . T−1=[RT0T−RTt1].
这里额外补充一点向量外积的知识:对于向量 a , b ∈ R 3 \boldsymbol a, \boldsymbol b \in \mathbb{R}^{3} a,b∈R3,外积可以表达为:
a × b = [ i j k a 1 a 2 a 3 b 1 b 2 b 3 ] = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b ≜ a ∧ b . a \times b=\left[\begin{array}{ccc}i & j & k \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3}\end{array}\right]=\left[\begin{array}{c}a_{2} b_{3}-a_{3} b_{2} \\ a_{3} b_{1}-a_{1} b_{3} \\ a_{1} b_{2}-a_{2} b_{1}\end{array}\right]=\left[\begin{array}{ccc}0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0\end{array}\right] b \triangleq a^{\wedge} b . a×b=⎣ ⎡ia1b1ja2b2ka3b3⎦ ⎤=⎣ ⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦ ⎤=⎣ ⎡0a3−a2−a30a1a2−a10⎦ ⎤b≜a∧b.
a × b = ∥ a ∥ ∥ b ∥ sin ( θ ) n \boldsymbol {a} \times \boldsymbol {b}=\|\boldsymbol {a}\|\|\boldsymbol {b}\| \sin (\theta) \boldsymbol {n} a×b=∥a∥∥b∥sin(θ)n
外积的方向垂直于这两个向量, 大小为 ∣ a ∣ ∣ b ∣ sin ⟨ a , b ⟩ |a||b| \sin \langle a, b\rangle ∣a∣∣b∣sin⟨a,b⟩, 是两个向量张成的四边形的 有向面积。对于外积, 我们引入了 ∧ \wedge ∧ 符号, 把 a a a 㝍成一个矩阵。事实上是一个反对称矩阵 (Skew-symmetric), 你可以将 ∧ { }^{\wedge} ∧ 记成一个反对称符号。这样就把外积 a × b a \times b a×b, 写成了矩阵 与向量的乘法 a ∧ b a^{\wedge} b a∧b, 把它变成了线件运算。这个符号将在后文经常用到, 请记住它。
外积只对三维向量存在定义, 我们还能用外积表示向量的旋转。根据右手法则,从向量a转到向量b,转动轴的方向其实就是a和b外积的方向,大小由a和b的夹角决定。
关于刚体旋转的表示,除了上面介绍的旋转矩阵,但是旋转矩阵用9个量来表示一个三自由度的旋转,显然有些冗余,其次旋转矩阵自身带有约束,必须是行列式为1的旋转矩阵,这对我们以后的估计和优化会带来非常大的麻烦。因此有必要介绍其他的表示方法。
旋转向量
之前在介绍外积的时候提到说外积可以表示向量间的旋转。其实任意的旋转都可以用一个旋转轴和一个旋转角表示。因此我们定义一个旋转向量,其方向与旋转轴一致,长度等于旋转角。这里的旋转向量其实就是下面要接杀的李代数。对于一个旋转轴为 n \boldsymbol n n, 角度为 θ \theta θ 的旋转, 显然, 它对应的旋转向 量为 θ n \theta \boldsymbol n θn 。由旋转向量到旋转矩阵的过程由罗德里格斯公式 (Rodrigues’s Formula) 表明, 由于推导过程比较复杂, 我们不作描述, 只给出转换的结果
R = cos θ I + ( 1 − cos θ ) n n T + sin θ n ∧ . \boldsymbol R=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol n \boldsymbol n^{T}+\sin \theta \boldsymbol{n}^{\wedge} . R=cosθI+(1−cosθ)nnT+sinθn∧.
这里,符号 ∧ ^{\wedge} ∧ 是向量到反对称的转换符,前面介绍向量外积的时候有提到。
当然我们也可以从旋转矩阵计算旋转向量和转角:
θ = arccos ( tr ( R ) − 1 2 ) . \theta=\arccos \left(\frac{\operatorname{tr}(\boldsymbol{R})-1}{2}\right) . θ=arccos(2tr(R)−1).
R n = n \boldsymbol R \boldsymbol n = \boldsymbol n Rn=n
转轴 n \boldsymbol n n是矩阵R特征值1对应的特征向量。求解此方程,再归一化就可得到旋转轴。
欧拉角
欧拉角当中比较常用的一种,便是用“偏航-俯仰-滚转”(yaw-pitch-roll)三个角度来描述一个旋转的。由于它等价于 ZY X 轴的旋转,我们就以 ZY X 为例。
假设一个刚体的前方(朝向我们的方向)为X 轴,右侧为 Y 轴,上方为 Z 轴,见下图 。那么,ZY X 转角相当于把任意旋转分解成以下三个轴上的转角:
此时,我们可以使用 [r, p, y]T 这样一个三维的向量描述任意旋转。这个向量十分的直观,我们可以从这个向量想象出旋转的过程。但是欧拉角的一个重大缺点是会碰到著名的万向锁问题(Gimbal Lock①):在俯仰角为±90◦ 时,第一次旋转与第三次旋转将使用同一个轴,使得系统丢失了一个自由度(由三次旋转变成了两次旋转)。这被称为奇异性问题,理论上可以证明,只要我们想用三个实数来表达三维旋转时,都会不可避免地碰到奇异性问题。由于这种原理,欧拉角不适于插值和迭代,往往只用于人机交互中。
四元数
事实上,我们找不到不带奇异性的三维向量描述方式 。这有点类似于,当我们想用两个坐标表示地球表面时(如经度和纬度),必定存在奇异性(纬度为 ±90◦ 时经度无意义)。三维旋转是一个三维流形,想要无奇异性地表达它,用三个量是不够的。
回忆我们以前学习过的复数。我们用复数集 C \mathbb C C 表示复平面上的向量,而复数的乘法则能表示复平面上的旋转:例如,乘上复数 i 相当于逆时针把一个复向量旋转 90 度。类似的,在表达三维空间旋转时,也有一种类似于复数的代数:四元数(Quaternion)。四元数是 Hamilton 找到的一种扩展的复数. 它既是紧凑的,也没有奇异性。
一个四元数 q q q 拥有一个实部和三个虚部, 像这样:
q = q 0 + q 1 i + q 2 j + q 3 k , q=q_{0}+q_{1} i+q_{2} j+q_{3} k, q=q0+q1i+q2j+q3k,
其中 i , j , k i, j, k i,j,k 为四元数的三个虚部。这三个虚部满足关系式:
{ 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 \left\{\begin{array}{l} 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 \end{array}\right. ⎩ ⎨ ⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
由于它的这种特殊表示形式,有时人们也用一个标量和一个向量来表达四元数:
q = [ s , v ] , s = q 0 ∈ R , v = [ q 1 , q 2 , q 3 ] T ∈ R 3 , q=[s, v], \quad s=q_{0} \in \mathbb{R}, v=\left[q_{1}, q_{2}, q_{3}\right]^{T} \in \mathbb{R}^{3}, q=[s,v],s=q0∈R,v=[q1,q2,q3]T∈R3,
这里, s s s 称为四元数的实部, 而 v v v 称为它的虚部。如果一个四元数虚部为 0 , 称之为实四 元数。反之, 若它的实部为 0 , 称之为虚四元数。
我们可以用四元数表达对一个点的旋转。假设一个空间三维点 p = [ x , y , z ] ∈ R 3 \boldsymbol p=[x, y, z] \in \mathbb{R}^{3} p=[x,y,z]∈R3, 以 及一个由轴角 n , θ \boldsymbol n, \theta n,θ 指定的旋转。三维点 p \boldsymbol p p 经过旋转之后变成为 p ′ \boldsymbol p^{\prime} p′ 。如果使用矩阵描述, 则为 p ′ = R p \boldsymbol p^{\prime} =\boldsymbol R \boldsymbol p p′=Rp
如果用四元数来描述,首先把三维空间点用一个虚四元数来描述:
p = [ 0 , x , y , z ] = [ 0 , v ] . \boldsymbol p=[0, x, y, z]=[0, \boldsymbol v] . p=[0,x,y,z]=[0,v].
这相当于我们把四元数的三个虚部与空间中的三个轴相对应。然后,用四 元数 q \boldsymbol q q 表示这个旋转:
q = [ cos θ 2 , n sin θ 2 ] . \boldsymbol q=\left[\cos \frac{\theta}{2}, \boldsymbol n \sin \frac{\theta}{2}\right] . q=[cos2θ,nsin2θ].
反之,我们亦可从单位四元数中计算出对应旋转轴与夹角:
{ θ = 2 arccos q 0 [ n x , n y , n z ] T = [ q 1 , q 2 , q 3 ] T / sin θ 2 \left\{\begin{array}{l} \theta=2 \arccos q_{0} \\ {\left[n_{x}, n_{y}, n_{z}\right]^{T}=\left[q_{1}, q_{2}, q_{3}\right]^{T} / \sin \frac{\theta}{2}} \end{array}\right. {θ=2arccosq0[nx,ny,nz]T=[q1,q2,q3]T/sin2θ
对上式的 θ 加上 2π,我们得到一个相同的旋转,但此时对应的四元数变成了 −q。因此,在四元数中,任意的旋转都可以由两个互为相反数的四元数表示。同理,取 θ 为 0,则得到一个没有任何旋转的实四元数。
回到正题, 旋转后的点 p ′ \boldsymbol p^{\prime} p′ 即可表示为这样的乘积:
p ′ = q p q − 1 \boldsymbol p^{\prime}=\boldsymbol q \boldsymbol p \boldsymbol q^{-1} p′=qpq−1
计算结果的实部为 0 , 故为纯虚四元数。其虚部的三个分量表 示旋转后 3 D 3 \mathrm{D} 3D 点的坐标。
设四元数 q = q 0 + q 1 i + q 2 j + q 3 k \boldsymbol q=q_{0}+q_{1} i+q_{2} j+q_{3} k q=q0+q1i+q2j+q3k, 对应的旋转矩阵 R \boldsymbol R R 为:
R = [ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 + 2 q 0 q 3 2 q 1 q 3 − 2 q 0 q 2 2 q 1 q 2 − 2 q 0 q 3 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 + 2 q 0 q 1 2 q 1 q 3 + 2 q 0 q 2 2 q 2 q 3 − 2 q 0 q 1 1 − 2 q 1 2 − 2 q 2 2 ] \boldsymbol{R}=\left[\begin{array}{lll} 1-2 q_{2}^{2}-2 q_{3}^{2} & 2 q_{1} q_{2}+2 q_{0} q_{3} & 2 q_{1} q_{3}-2 q_{0} q_{2} \\ 2 q_{1} q_{2}-2 q_{0} q_{3} & 1-2 q_{1}^{2}-2 q_{3}^{2} & 2 q_{2} q_{3}+2 q_{0} q_{1} \\ 2 q_{1} q_{3}+2 q_{0} q_{2} & 2 q_{2} q_{3}-2 q_{0} q_{1} & 1-2 q_{1}^{2}-2 q_{2}^{2} \end{array}\right] R=⎣ ⎡1−2q22−2q322q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q12−2q22⎦ ⎤
反之, 由旋转矩阵到四元数的转换如下。假设矩阵为 R = { m i j } , i , j ∈ [ 1 , 2 , 3 ] \boldsymbol R=\left\{m_{i j}\right\}, i, j \in[1,2,3] R={mij},i,j∈[1,2,3], 其对 应的四元数 q q q 由下式给出:
q 0 = tr ( R ) + 1 2 , q 1 = m 23 − m 32 4 q 0 , q 2 = m 31 − m 13 4 q 0 , q 3 = m 12 − m 21 4 q 0 . q_{0}=\frac{\sqrt{\operatorname{tr}(R)+1}}{2}, q_{1}=\frac{m_{23}-m_{32}}{4 q_{0}}, q_{2}=\frac{m_{31}-m_{13}}{4 q_{0}}, q_{3}=\frac{m_{12}-m_{21}}{4 q_{0}} . q0=2tr(R)+1,q1=4q0m23−m32,q2=4q0m31−m13,q3=4q0m12−m21.
值得一提的是, 由于 q \boldsymbol q q 和 − q -\boldsymbol q −q 表示同一个旋转, 事实上一个 R \boldsymbol R R 对应的四元数表示并 不是惟一的。同时, 除了上面给出的转换方式之外, 还存在其他几种计算方法, 而本书都 省略了。实际編程中| 当 q 0 q_{0} q0 接近 0 时, 其余三个分量会非常大, 导致解不稳定, 此时我们] 再考虑使用其他的方式进行转换。
前面介绍过,三位旋转矩阵构成了特殊正交群SO(3),而变换矩阵构成了特殊欧式群SE(3)。我们可以观察到这两个"群"对于加法不封闭,二对于乘法封闭,换句话说就是:
R 1 + R 2 ∉ S O ( 3 ) , R 1 R 2 ∈ S O ( 3 ) , R_{1}+R_{2} \notin S O(3),R_{1} R_{2} \in S O(3), R1+R2∈/SO(3),R1R2∈SO(3),
我们首先介绍群(Group)的概念:是一种集合加上一种运算的代数结构。我们把集合记作 A A A, 运算记作 . , 那么群可以记作 G = ( A , ⋅ ) G=(A, \cdot) G=(A,⋅) 。群要求这个运算满足以下几个条件:
矩阵中常见的群有:
考虑任意旋转矩阵R,有
R R T = I . R R^{T}=I . RRT=I.
在等式两边对时间求导, 得到:
R ˙ ( t ) R ( t ) T + R ( t ) R ˙ ( t ) T = 0. \dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}+\boldsymbol{R}(t) \dot{\boldsymbol{R}}(t)^{T}=0 . R˙(t)R(t)T+R(t)R˙(t)T=0.
整理得:
R ˙ ( t ) R ( t ) T = − ( R ˙ ( t ) R ( t ) T ) T . \dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}=-\left(\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}\right)^{T} . R˙(t)R(t)T=−(R˙(t)R(t)T)T.
这里我们引入符号 ∨ ^{\vee} ∨,它与符号 ∧ ^{\wedge} ∧正相反,可以将反对称矩阵变成与之对应的向量。即
a ∧ = A = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] , A ∨ = a . \boldsymbol{a}^{\wedge}=\boldsymbol{A}=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right], \quad \boldsymbol{A}^{\vee}=\boldsymbol{a} . a∧=A=⎣ ⎡0a3−a2−a30a1a2−a10⎦ ⎤,A∨=a.
于是, 由于 R ˙ ( t ) R ( t ) T \dot{R}(t) \boldsymbol{R}(t)^{T} R˙(t)R(t)T 是一个反对称矩阵, 我们可以找到一个三维向量 ϕ ( t ) ∈ R 3 \phi(t) \in \mathbb{R}^{3} ϕ(t)∈R3 与 之对应。于是有:
R ˙ ( t ) R ( t ) T = ϕ ( t ) ∧ . \dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}=\phi(t)^{\wedge} . R˙(t)R(t)T=ϕ(t)∧.
等式两边右乘 R ( t ) R(t) R(t), 由于 R R R 为正交阵, 有:
R ˙ ( t ) = ϕ ( t ) ∧ R ( t ) = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] R ( t ) . \dot{\boldsymbol{R}}(t)=\phi(t)^{\wedge} \boldsymbol{R}(t)=\left[\begin{array}{ccc} 0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0 \end{array}\right] \boldsymbol{R}(t) . R˙(t)=ϕ(t)∧R(t)=⎣ ⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦ ⎤R(t).
可以看到, 每对旋转矩阵求一次导数, 只需左乘一个 ϕ ∧ ( t ) \phi^{\wedge}(t) ϕ∧(t) 矩阵即可。为方便讨论, 我 们设 t 0 = 0 t_{0}=0 t0=0, 并设此时旋转矩阵为 R ( 0 ) = I \boldsymbol{R}(0)=\boldsymbol{I} R(0)=I 。按照导数定义, 可以把 R ( t ) \boldsymbol{R}(t) R(t) 在 0 附近进行 一阶泰勒展开:
R ( t ) ≈ R ( t 0 ) + R ˙ ( t 0 ) ( t − t 0 ) = I + ϕ ( t 0 ) ∧ ( t ) . \begin{aligned} \boldsymbol{R}(t) & \approx \boldsymbol{R}\left(t_{0}\right)+\dot{R}\left(t_{0}\right)\left(t-t_{0}\right) \\ &=\boldsymbol{I}+\phi\left(t_{0}\right)^{\wedge}(t) . \end{aligned} R(t)≈R(t0)+R˙(t0)(t−t0)=I+ϕ(t0)∧(t).
我们看到 ϕ \phi ϕ 反应了 R \boldsymbol{R} R 的导数性质, 故称它在 S O ( 3 ) S O(3) SO(3) 原点附近的正切空间 (Tangent Space) 上。同时在 t 0 t_{0} t0 附近, 设 ϕ \phi ϕ 保持为常数 ϕ ( t 0 ) = ϕ 0 \phi\left(t_{0}\right)=\phi_{0} ϕ(t0)=ϕ0 。则有
R ˙ ( t ) = ϕ ( t 0 ) ∧ R ( t ) = ϕ 0 ∧ R ( t ) . \dot{R}(t)=\phi\left(t_{0}\right)^{\wedge} \boldsymbol{R}(t)=\phi_{0}^{\wedge} \boldsymbol{R}(t) . R˙(t)=ϕ(t0)∧R(t)=ϕ0∧R(t).
上式是一个关于 R R R 的微分方程, 而且我们知道初始值 R ( 0 ) = I R(0)=I R(0)=I, 解之, 得:
R ( t ) = exp ( ϕ 0 ∧ t ) . R(t)=\exp \left(\phi_{0}^{\wedge} t\right) . R(t)=exp(ϕ0∧t).
每个李群都有与之对应的李代数。李代数描述了李群的局部性质。通用的李代数的定 义如下:
李代数由一个集合 V \mathbb{V} V, 一个数域 F \mathbb{F} F 和一个二元运算 [,] 组成。如果它们满足以下几条 性质, 称 ( V , F , [ , ] (\mathbb{V}, \mathbb{F},[,] (V,F,[,], 为一个李代数, 记作 g \mathfrak{g} g 。
其中二元运算被称为李括号。从表面上来看, 李代数所需要的性质还是拴多的。相比于群中的较为简单的二元运算, 李括号表达了两个元素的差异。它不要求结合律, 而要求 元素和自己做李括号之后为零的性质。作为例子, 三维向量 R 3 \mathbb{R}^{3} R3 上定义的叉积 × \times × 是一种李 括号, 因此 g = ( R 3 , R , × ) \mathfrak{g}=\left(\mathbb{R}^{3}, \mathbb{R}, \times\right) g=(R3,R,×) 构成了一个李代数。读者可以尝试将叉积的性质代入到上面四 条性质中。
之前提到的 ϕ \phi ϕ,事实上是一种李代数。SO(3) 对应的李代数是定义在 R3上的向量,我们记作 ϕ \phi ϕ。根据前面的推导,每个 ϕ \phi ϕ都可以生成一个反对称矩阵
Φ = ϕ ∧ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] ∈ R 3 × 3 \Phi=\phi^{\wedge}=\left[\begin{array}{ccc} 0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0 \end{array}\right] \in \mathbb{R}^{3 \times 3} Φ=ϕ∧=⎣ ⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦ ⎤∈R3×3
在此定义下, 两个向量 ϕ 1 , ϕ 2 \phi_{1}, \phi_{2} ϕ1,ϕ2 的李括号为:
[ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∨ . \left[\phi_{1}, \phi_{2}\right]=\left(\Phi_{1} \Phi_{2}-\Phi_{2} \Phi_{1}\right)^{\vee} . [ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∨.
读者可以去验证该定义下的李括号满足上面的几条性质。由于 ϕ \phi ϕ 与反对称矩阵关系很 紧密, 在不引起歧义的情况下, 就说 s o ( 3 ) \mathfrak{s o}(3) so(3)的元素是 3 维向量或者 3 维反对称矩阵, 不加 区别:
s 0 ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } . \mathfrak{s 0}(3)=\left\{\phi \in \mathbb{R}^{3}, \Phi=\phi^{\wedge} \in \mathbb{R}^{3 \times 3}\right\} . s0(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}.
至此, 我们已消楚了 s o ( 3 ) \mathfrak{s o}(3) so(3)的内容。它们是一个由三维向量组成的集合, 每个向量对 应到一个反对称矩阵, 可以表达旋转矩阵的导数。它与 S O ( 3 ) S O(3) SO(3) 的关系由指数映射给定:
R = exp ( ϕ ∧ ) . \boldsymbol{R}=\exp \left(\phi^{\wedge}\right) . R=exp(ϕ∧).
对于SE(3),他也有对应的李代数 s e ( 3 ) \mathfrak{s e}(3) se(3):
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } \mathfrak{s e}(3)=\left\{\xi=\left[\begin{array}{c} \rho \\ \phi \end{array}\right] \in \mathbb{R}^{6}, \rho \in \mathbb{R}^{3}, \phi \in \mathfrak{s o}(3), \xi^{\wedge}=\left[\begin{array}{cc} \phi^{\wedge} & \rho \\ 0^{T} & 0 \end{array}\right] \in \mathbb{R}^{4 \times 4}\right\} se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]∈R4×4}
我们把每个 s e ( 3 ) \mathfrak{s e}(3) se(3) 元素记作 ξ \xi ξ, 它是一个六维向量。前三维为平移, 记作 ρ \rho ρ : 后三维为 旋转, 记作 ϕ \phi ϕ, 实质上是 s o ( 3 ) \mathfrak{s o}(3) so(3) 元素。同时, 我们拓展了 ∧ { }^{\wedge} ∧ 符号的含义。在 s e ( 3 ) \mathfrak{s e}(3) se(3) 中, 同 样使用 ∧ \wedge ∧ 符号, 将一个六维向量转换成四维矩阵, 但这里不再表示反对称。
上文提到了指数矩阵 e x p ( ϕ ∧ ) exp(\phi^{\wedge} ) exp(ϕ∧),在李群和李代数中又称为指数映射。那它到底该如何计算呢?这里我们直接给出结果:
exp ( θ a ∧ ) = cos θ I + ( 1 − cos θ ) a a T + sin θ a ∧ . \exp \left(\theta a^{\wedge}\right)=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{a a ^ { T }}+\sin \theta \boldsymbol{a}^{\wedge} . exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧.
回忆前一讲内容, 它和罗德里格斯公式, 如出一辑。这表明, s o ( 3 ) \mathfrak{s o}(3) so(3)实际 上就是由所谓的旋转向量组成的空间, 而指数映射即罗德里格斯公式。通过它们, 我们把 s e ( 3 ) \mathfrak{s e}(3) se(3)中任意一个向量对应到了一个位于 S O ( 3 ) S O(3) SO(3) 中的旋转矩阵。反之, 如果定义对数映射, 我们也能把 S O ( 3 ) S O(3) SO(3) 中的元素对应到 s o ( 3 ) \mathfrak{s o}(3) so(3) 中:
ϕ = ln ( R ) ∨ = ( ∑ n = 0 ∞ ( − 1 ) n n + 1 ( R − I ) n + 1 ) ∨ . \phi=\ln (\boldsymbol{R})^{\vee}=\left(\sum_{n=0}^{\infty} \frac{(-1)^{n}}{n+1}(\boldsymbol{R}-\boldsymbol{I})^{n+1}\right)^{\vee} . ϕ=ln(R)∨=(n=0∑∞n+1(−1)n(R−I)n+1)∨.
不过我们通常不按照泰勒展开去计算对数映射。在介绍旋转的表示中,我们已经介绍过如何根据旋转矩阵计算对应的李代数,即利用迹的性质分别求解转角和转轴,采用那种方式更加省事一些。
同理, s e ( 3 ) \mathfrak{s e}(3) se(3) 的指数映射形式如下:
虽然我们已经清楚了 SO(3) 和 SE(3)上的李群与李代数关系,但是,当我们在 SO(3) 中完成两个矩阵乘法时,李代数中 s 0 ( 3 ) \mathfrak{s 0}(3) s0(3) 上发生了什么改变呢?反过来说,当 s 0 ( 3 ) \mathfrak{s 0}(3) s0(3) 上做两个李代数的加法时,SO(3) 上是否对应着两个矩阵的乘积?如果成立的话,相当于:
ln ( exp ( A ) exp ( B ) ) = A + B ? \ln (\exp (A) \exp (B))=A+B ? ln(exp(A)exp(B))=A+B?
遗憾的是,这个式子对于A,B为标量时成立,但A,B若是矩阵,则不成立。两个李代数指数映射乘积的完整形式,由 Baker-Campbell-Hausdorff 公式(BCH 公式)给出。由于它完整的形式较复杂,我们给出它展开式的前几项:
ln ( exp ( A ) exp ( B ) ) = A + B + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] − 1 12 [ B , [ A , B ] ] + ⋯ \ln (\exp (A) \exp (B))=A+B+\frac{1}{2}[A, B]+\frac{1}{12}[A,[A, B]]-\frac{1}{12}[B,[A, B]]+\cdots ln(exp(A)exp(B))=A+B+21[A,B]+121[A,[A,B]]−121[B,[A,B]]+⋯
其中[]为李括号。 B C H \mathrm{BCH} BCH 公式告诉我们, 当处理两个矩阵指数之积时, 它们会产生一些由 李括号组成的余项。特别地, 考虑 S O ( 3 ) S O(3) SO(3) 上的李代数 ln ( exp ( ϕ 1 ∧ ) exp ( ϕ 2 ∧ ) ) ∨ \ln \left(\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee} ln(exp(ϕ1∧)exp(ϕ2∧))∨, 当 ϕ 1 \phi_{1} ϕ1 或 ϕ 2 \phi_{2} ϕ2 为小量时, 小量二次以上的项都可以被忽略掉。此时, B C H \mathrm{BCH} BCH 拥有线性近似表达:
ln ( exp ( ϕ 1 ∧ ) exp ( ϕ 2 ∧ ) ) ∨ ≈ { J 1 ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 if ϕ 1 is small, J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 if ϕ 2 is small. \ln \left(\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee} \approx \begin{cases}J_{1}\left(\phi_{2}\right)^{-1} \phi_{1}+\phi_{2} & \text { if } \phi_{1} \text { is small, } \\ J_{r}\left(\phi_{1}\right)^{-1} \phi_{2}+\phi_{1} & \text { if } \phi_{2} \text { is small. }\end{cases} ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{J1(ϕ2)−1ϕ1+ϕ2Jr(ϕ1)−1ϕ2+ϕ1 if ϕ1 is small, if ϕ2 is small.
以第一个近似为例。该式告诉我们,当对一个旋转矩阵 R 2 R_{2} R2 (李代数为 ϕ 2 \phi_{2} ϕ2 )左乘一个 微小旋转矩脌 R 1 R_{1} R1 (李代数为 ϕ 1 \phi_{1} ϕ1 )时,可以近似地看作,在原在的李代数 ϕ 2 \phi_{2} ϕ2 上, 加上了一 项 J l ( ϕ 2 ) − 1 ϕ 1 J_{l}\left(\phi_{2}\right)^{-1} \phi_{1} Jl(ϕ2)−1ϕ1 。同理, 第二个近似描述了右乘一个微小位移的情况。于是, 李代数在 B C H \mathrm{BCH} BCH 近似下,分成了左乘近似和右乘近似两种, 在使用时我们须加注意, 使用的是左乘模型还 是右韭模型。
本书以左乘为例。左乘 B C H \mathrm{BCH} BCH 近似雅可比:
J l = J = sin θ θ I + ( 1 − sin θ θ ) a a T + 1 − cos θ θ a ∧ . J_{l}=J=\frac{\sin \theta}{\theta} I+\left(1-\frac{\sin \theta}{\theta}\right) a a^{T}+\frac{1-\cos \theta}{\theta} a^{\wedge} . Jl=J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧.
它的逆为:
J l − 1 = θ 2 cot θ 2 I + ( 1 − θ 2 cot θ 2 ) a a T − θ 2 a ∧ . J_{l}^{-1}=\frac{\theta}{2} \cot \frac{\theta}{2} I+\left(1-\frac{\theta}{2} \cot \frac{\theta}{2}\right) a a^{T}-\frac{\theta}{2} a^{\wedge} . Jl−1=2θcot2θI+(1−2θcot2θ)aaT−2θa∧.
而右乘雅可比仅需要对自变量取负号即可:
J r ( ϕ ) = J I ( − ϕ ) . J_{r}(\phi)=J_{I}(-\phi) . Jr(ϕ)=JI(−ϕ).
这样,我们就可以谈论李群乘法与李代数加法的关系了。假定对某个旋转 R,对应的李代数为 ϕ。我们给它左乘一个微小旋转,记作 ∆R,对应的李代数为 ∆ϕ。那么,在李群上,得到的结果就是 ∆R · R,而在李代数上,根据 BCH近似,为:Jl−1(ϕ)∆ϕ + ϕ。合并起来,可以简单地写成:
exp ( Δ ϕ ∧ ) exp ( ϕ ∧ ) = exp ( ( ϕ + J l − 1 ( ϕ ) Δ ϕ ) ∧ ) \exp \left(\Delta \phi^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\left(\phi+J_{l}^{-1}(\phi) \Delta \phi\right)^{\wedge}\right) exp(Δϕ∧)exp(ϕ∧)=exp((ϕ+Jl−1(ϕ)Δϕ)∧)
反之, 如果我们在李代数上进行加法, 让一个 ϕ \phi ϕ 加上 Δ ϕ \Delta \phi Δϕ, 那么可以近似为李群上带 左右雅可比的乘法:
exp ( ( ϕ + Δ ϕ ) ∧ ) = exp ( ( J l Δ ϕ ) ∧ ) exp ( ϕ ∧ ) = exp ( ϕ ∧ ) exp ( ( J r Δ ϕ ) ∧ ) \exp \left((\phi+\Delta \phi)^{\wedge}\right)=\exp \left(\left(J_{l} \Delta \phi\right)^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\phi^{\wedge}\right) \exp \left(\left(\boldsymbol{J}_{r} \Delta \phi\right)^{\wedge}\right) exp((ϕ+Δϕ)∧)=exp((JlΔϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp((JrΔϕ)∧)
这将为之后李代数上的做微积分提供了理论基础。同样的, 对于 S E ( 3 ) S E(3) SE(3), 亦有类似的 B C H \mathrm{BCH} BCH 近似公式:
exp ( Δ ξ ∧ ) exp ( ξ ∧ ) ≈ exp ( ( J l − 1 Δ ξ + ξ ) ∧ ) exp ( ξ ∧ ) exp ( Δ ξ ∧ ) ≈ exp ( ( J r − 1 Δ ξ + ξ ) ∧ ) . \begin{aligned} &\exp \left(\Delta \xi^{\wedge}\right) \exp \left(\xi^{\wedge}\right) \approx \exp \left(\left(\mathcal{J}_{l}^{-1} \Delta \xi+\xi\right)^{\wedge}\right) \\ &\exp \left(\xi^{\wedge}\right) \exp \left(\Delta \xi^{\wedge}\right) \approx \exp \left(\left(\mathcal{J}_{r}^{-1} \Delta \xi+\xi\right)^{\wedge}\right) . \end{aligned} exp(Δξ∧)exp(ξ∧)≈exp((Jl−1Δξ+ξ)∧)exp(ξ∧)exp(Δξ∧)≈exp((Jr−1Δξ+ξ)∧).
这里 J l \mathcal{J}_{l} Jl 形式比较复杂, 它是一个 6 × 6 6 \times 6 6×6 的矩阵, 读者可以参考 [6] 中式 ( 7.82 ) (7.82) (7.82) 和 (7.83) 内容。由于我们在计算中不用到该雅可比, 故这里略去它的实际形式。
在SLAM中,我们经常会构建与位姿有关的函数,然后讨论该函数关于
位姿的导数,以调整当前的估计值。所以讨论李代数的求导非常的重要,但是SO(3),SE(3)上没有良好定义的加法,他们只是群。如果我们把T当成一个普通矩阵来优化处理,那就必须对它加以约束,那就必须对它加以约束。而李代数是由向量组成的,具有良好的加法运算。因此使用李代数解决求导问题的思路分为两种:
首先,考虑SO(3)的情况,假设对空间点p进行额旋转,得到Rp。宣布在要就算旋转后的点的坐标相对于旋转的导数,我们不严谨的记为:
∂ ( R p ) ∂ R \frac{\partial(\boldsymbol{R} \boldsymbol p)}{\partial \boldsymbol{R}} ∂R∂(Rp)
由于 S O ( 3 ) S O(3) SO(3) 没有加法, 所以该导数无法按照导数的定义进行计算。设 R R R 对应的李代数为 ϕ \phi ϕ, 我们转而计算:
∂ ( exp ( ϕ ∧ ) p ) ∂ ϕ \frac{\partial\left(\exp \left(\phi^{\wedge}\right) p\right)}{\partial \phi} ∂ϕ∂(exp(ϕ∧)p)
这里同样省略推到过程给出化简后的结果:
∂ ( R p ) ∂ ϕ = ( − R p ) ∧ J l . \frac{\partial(\boldsymbol{R} \boldsymbol p)}{\partial \phi}=(-\boldsymbol{R} \boldsymbol p)^{\wedge} \boldsymbol{J}_{l} . ∂ϕ∂(Rp)=(−Rp)∧Jl.
不过,由于这里仍然含有形式比较复杂的 J l J_l Jl,我们不太希望计算它。而下面要讲的扰动模型则提供了更简单的导数计算方式。
另一种求导方式, 是对 R \boldsymbol{R} R 进行一次扰动 Δ R \Delta \boldsymbol{R} ΔR 。这个扰动可以乘在左边也可以乘在右 边, 最后结果会有一点儿微小的差异, 我们以左扰动为例。设左扰动 Δ R \Delta \boldsymbol{R} ΔR 对应的李代数为 φ \varphi φ 。然后, 对 φ \varphi φ 求导, 即:
∂ ( R p ) ∂ φ = lim φ → 0 exp ( φ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p φ . \frac{\partial(\boldsymbol{R p})}{\partial \varphi}=\lim _{\varphi \rightarrow 0} \frac{\exp \left(\varphi^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) p}{\varphi} . ∂φ∂(Rp)=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p.
该式的求导比上面更为简单:
∂ ( R p ) ∂ φ = lim φ → 0 exp ( φ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p φ ≈ lim φ → 0 ( 1 + φ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p φ = lim φ → 0 φ ∧ R p φ = lim φ → 0 − ( R p ) ∧ φ φ = − ( R p ) ∧ . \begin{aligned} \frac{\partial(R p)}{\partial \varphi} &=\lim _{\varphi \rightarrow 0} \frac{\exp \left(\varphi^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) p}{\varphi} \\ & \approx \lim _{\varphi \rightarrow 0} \frac{\left(1+\varphi^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) p}{\varphi} \\ &=\lim _{\varphi \rightarrow 0} \frac{\varphi^{\wedge} R p}{\varphi}=\lim _{\varphi \rightarrow 0} \frac{-(\boldsymbol{R} p)^{\wedge} \varphi}{\varphi}=-(\boldsymbol{R} p)^{\wedge} . \end{aligned} ∂φ∂(Rp)=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p≈φ→0limφ(1+φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφφ∧Rp=φ→0limφ−(Rp)∧φ=−(Rp)∧.
可见, 扰动模型相比于直接对李代数求导, 省去了一个雅可比 J l J_{l} Jl 的计算。这使得扰动 模型更为实用。请读者务必理解这里的求导运算, 这在位姿估计当中具有重要的意义。
最后, 我们给出 S E ( 3 ) S E(3) SE(3) 上的扰动模型, 而直接李代数上的求导就不再介绍了。假设 某空间点 p p p 经过一次变换 T T T (对应李代数为 ξ \xi ξ ), 得到 T p T p Tp。现在, 给 T T T 左乘一个扰动 Δ T = exp ( δ ξ ∧ ) \Delta \boldsymbol{T}=\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) ΔT=exp(δξ∧), 我们设扰动项的李代数为 δ ξ = [ δ ρ , δ ϕ ] T \delta \xi=[\delta \rho, \delta \phi]^{T} δξ=[δρ,δϕ]T, 那么:
∂ ( T p ) ∂ δ ξ = lim δ ξ → 0 exp ( δ ξ ∧ ) exp ( ξ ∧ ) p − exp ( ξ ∧ ) p δ ξ \frac{\partial(\boldsymbol{T} \boldsymbol{p})}{\partial \delta \boldsymbol{\xi}}=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) p-\exp \left(\boldsymbol{\xi}^{\wedge}\right) p}{\delta \boldsymbol{\xi}} ∂δξ∂(Tp)=limδξ→0δξexp(δξ∧)exp(ξ∧)p−exp(ξ∧)p
≈ lim δ ξ → 0 ( I + δ ξ ∧ ) exp ( ξ ∧ ) p − exp ( ξ ∧ ) p δ ξ \approx \lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\left(\boldsymbol{I}+\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) p-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}} ≈limδξ→0δξ(I+δξ∧)exp(ξ∧)p−exp(ξ∧)p
= lim δ ξ → 0 δ ξ ∧ exp ( ξ ∧ ) p δ ξ =\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\delta \xi^{\wedge} \exp \left(\xi^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}} =limδξ→0δξδξ∧exp(ξ∧)p
= lim δ ξ → 0 [ δ ϕ ∧ δ ρ 0 T 0 ] [ R p + t 1 ] δ ξ =\lim _{\delta \xi \rightarrow 0} \frac{\left[\begin{array}{cc}\delta \phi^{\wedge} & \delta \rho \\ \mathbf{0}^{T} & 0\end{array}\right]\left[\begin{array}{c}\boldsymbol{R p}+\boldsymbol{t} \\ 1\end{array}\right]}{\delta \xi} =limδξ→0δξ[δϕ∧0Tδρ0][Rp+t1]
= lim δ ξ → 0 [ δ ϕ ∧ ( R p + t ) + δ ρ 0 ] δ ξ = [ I − ( R p + t ) ∧ 0 T 0 T ] ≜ ( T p ) ⊙ . =\lim _{\delta \xi \rightarrow 0} \frac{\left[\begin{array}{c}\delta \phi^{\wedge}(\boldsymbol{R} p+\boldsymbol{t})+\delta \boldsymbol{\rho} \\ 0\end{array}\right]}{\delta \xi}=\left[\begin{array}{cc}\boldsymbol{I} & -(\boldsymbol{R} p+\boldsymbol{t})^{\wedge} \\ 0^{T} & 0^{T}\end{array}\right] \triangleq(\boldsymbol{T} \boldsymbol{p})^{\odot} . =limδξ→0δξ[δϕ∧(Rp+t)+δρ0]=[I0T−(Rp+t)∧0T]≜(Tp)⊙.
我们把最后的结果定义成一个算符 ⊙ \odot ⊙ 2), 它把一个齐次坐标的空间点变换成一个 4 × 6 4 \times 6 4×6 的矩阵。