ORB-SLAM 论文与RGB-D相机直接法笔记(1)

一、BA
首先是Bundle adjustment (捆绑调整),本质是一个优化模型,其目的是最小化重投影误差。

当相机成像时,三维点通过相机的R,t参数可以一一对应到相机坐标系,相机坐标系上的点(X,Y)又可以通过相机的内参矩阵P变换到像素坐标系的点(u,v)
事实上,第一次投影就是指的是相机在拍照时三维空间点投影到像素坐标的过程,我们利用一些几何信息和相应算法可以计算出来相机矩阵从而把三维坐标重新转化成像素坐标,而这个时候的计算出来的像素坐标和原来真实的像素坐标之间的差值就被称为重投影误差
而BA就是一个图优化模型
在这里插入图片描述
令点 X j X_{j} Xj在相机 P i P_{i} Pi下拍摄到的图像归一化坐标系上的坐标为 k ( u i j T , 1 ) T = K i − 1 x i j k(u_{ij}^{T},1)^{T}=K_{i}^{-1}x_{ij} k(uijT,1)T=Ki1xij,其重投影后的图像归一化坐标系下的坐标为 k ′ ( v i j T , 1 ) T = K i − 1 P i X j k^{'}(v_{ij}^{T},1)^{T}=K_{i}^{-1}P_{i}X_{j} k(vijT,1)T=Ki1PiXj,其中 K i − 1 K_{i}^{-1} Ki1是为了在计算式能不受相机内参影响, k k k k ′ k^{'} k是将齐次坐标转换为非齐次坐标的常数项,于是重投影误差为
e i j = u i j − v i j \begin{aligned} e_{ij}=u_{ij}-v_{ij} \end{aligned} eij=uijvij
BA的目的就是通过优化算法将所有点的重投影误差之和最小化
m i n R i , t i , X j ∑ i , j δ i j ∣ ∣ u i j − v i j ∣ ∣ 2 \begin{aligned} \mathop{min}\limits_{R_{i},t_{i},X{j}}\sum\limits_{i,j}\delta_{ij}||u_{ij}-v_{ij}||^{2} \end{aligned} Ri,ti,Xjmini,jδijuijvij2
其中当点 X j X_{j} Xj在相机 P i P_{i} Pi中有投影时 δ i j = 1 \delta_{ij}=1 δij=1否则 δ i j = 0 \delta_{ij}=0 δij=0
接下来利用优化算法去迭代求解最佳的相机参数就OK

BA提供了相机位姿的精准估计和稀疏的几何重建,但是这种方法的计算代价太大,不适合于实时的视觉SLAM,实时的视觉SLAM应该在重建环境的同时估计相机轨迹,为了在可以承担的计算代价下达到精准的结果,一个实时的SLAM算法必须给BA提供
1、对应于关键帧的场景特征点
2、当关键帧的数量上升复杂度提高时,应该避免冗余
3、一个可以用关键帧和特征点提供准确结果的强大网络配置,意思就是大量的关键帧观察点都有显著的视差和足够的闭环匹配
4、为非线性优化的位姿和点的位置提供一个初始的估计
5、探索中的局部地图,优化主要是为了实现可伸缩性
6、实时的运行全局优化和闭环检测的能力

第一个利用BA的实时应用是
E. Mouragnon, M. Lhuillier, M. Dhome, F. Dekeyser, and P. Sayd, “Real time localization and 3D reconstruction,” in Proc. IEEE Comput. Soc. Conf. Comput. Vision Pattern Recog., 2006, vol. 1, pp. 363–370.
然后是具有突破性的SLAM工作-PTAM
G. Klein and D. Murray, “Parallel tracking and mapping for small AR workspaces,” in Proc. IEEE ACM Int. Symp. Mixed Augmented Reality, Nara, Japan, Nov. 2007, pp. 225–234.
这个算法虽然限制在小范围运作,但是为选择关键帧,特诊匹配,点三角化,相机定位和跟踪失败之后的重定位提供了简单有效的方法。不幸的是,有几个因素严重限制了它的应用:缺少闭环检测和对遮挡的适当处理,重定位观测点的低不变性和需要人工干预的地图导航。

二、李群和李代数
特殊正交群
S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } \begin{aligned} SO(3)=\{ \textbf{R}\in\mathbb{R}^{3\times3}|\textbf{R}\textbf{R}^{T}=\textbf{I},det(\textbf{R})=1\} \end{aligned} SO(3)={RR3×3RRT=I,det(R)=1}
特殊欧式群
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 × 3 } \begin{aligned} SE(3)=\{T= \left[ \begin{matrix} \textbf{R} & \textbf{t} \\ 0^{T} & 1 \end{matrix} \right] \in\mathbb{R}^{4\times4}|\textbf{R}\in SO(3),\textbf{t}\in\mathbb{R}^{3\times3}\} \end{aligned} SE(3)={T=[R0Tt1]R4×4RSO(3),tR3×3}
这两种群关于加法不封闭,但是拥有良好定义的乘法
R 1 R 2 ∈ S O ( 3 ) , T 1 T 2 ∈ S E ( 3 ) \begin{aligned} \textbf{R}_{1}\textbf{R}_{2}\in SO(3), \textbf{T}_{1}\textbf{T}_{2}\in SE(3) \end{aligned} R1R2SO(3),T1T2SE(3)
两个旋转矩阵相乘表示做了两次旋转,对于这种只有一种运算的集合,称为
李代数引出
R R T = I \begin{aligned} \textbf{R}\textbf{R}^{T}=\textbf{I} \end{aligned} RRT=I
假设 R \textbf{R} R表示某个相机的旋转,会随时间连续变化
R ( t ) R ( t ) T = I \begin{aligned} \textbf{R}(t)\textbf{R}(t)^{T}=\textbf{I} \end{aligned} R(t)R(t)T=I
等式两边对时间求导
R ˙ ( t ) R ( t ) T + R ( t ) R ˙ ( t ) T = 0 \begin{aligned} \dot{\textbf{R}}(t)\textbf{R}(t)^{T}+\textbf{R}(t)\dot{\textbf{R}}(t)^{T}=0 \end{aligned} R˙(t)R(t)T+R(t)R˙(t)T=0
整理得到
R ˙ ( t ) R ( t ) T = − ( R ˙ ( t ) R ( t ) T ) T \begin{aligned} \dot{\textbf{R}}(t)\textbf{R}(t)^{T}=-(\dot{\textbf{R}}(t)\textbf{R}(t)^{T})^{T} \end{aligned} R˙(t)R(t)T=(R˙(t)R(t)T)T
所以 R ˙ ( t ) R ( t ) T \dot{\textbf{R}}(t)\textbf{R}(t)^{T} R˙(t)R(t)T是一个反对称矩阵,对于任意一个反对称矩阵,可以找到一个与之对应的向量,这个运算用符号 ∨ ^{\vee} 表示
a ∧ = A = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] , A ∨ = a \begin{aligned} a^{\wedge}=A=\left[\begin{matrix} 0 & -a_{3}&a_{2}\\ a_{3}&0&-a_{1}\\ -a_{2}&a_{1}&0 \end{matrix} \right], A^{\vee}=a \end{aligned} a=A=0a3a2a30a1a2a10,A=a
同理
R ˙ ( t ) R ( t ) T = ϕ ( t ) ∧ \begin{aligned} \dot{\textbf{R}}(t)\textbf{R}(t)^{T}=\phi(t)^{\wedge} \end{aligned} R˙(t)R(t)T=ϕ(t)
等式两边右乘 R ( t ) \textbf{R}(t) R(t),由于 R \textbf{R} R为正交矩阵,有:
R ˙ ( t ) = ϕ ( t ) ∧ R ( t ) = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] R ( t ) \begin{aligned} \dot{\textbf{R}}(t)=\phi(t)^{\wedge}\textbf{R}(t)=\left[\begin{matrix} 0&-\phi_{3}&\phi_{2}\\ \phi_{3}&0&-\phi_{1}\\ -\phi_{2}&\phi_{1}&0\end{matrix}\right]\textbf{R}(t) \end{aligned} R˙(t)=ϕ(t)R(t)=0ϕ3ϕ2ϕ30ϕ1ϕ2ϕ10R(t)
每次对旋转矩阵求一次导数,只需要左乘一个 ϕ ∧ ( t ) \phi^{\wedge}(t) ϕ(t)即可。设 t 0 = 0 , R ( 0 ) = I t_{0}=0,\textbf{R}(0)=\textbf{I} t0=0,R(0)=I,进行一阶泰勒展开:
R ( t ) ≈ R ( t 0 ) + R ( t 0 ) ˙ ( t − t 0 ) = I + ϕ ( t 0 ) ∧ R ( t ) \begin{aligned} \textbf{R}(t)\approx\textbf{R}(t_{0})+\dot{\textbf{R}(t_{0})}(t-t_{0})=\textbf{I}+\phi(t_{0})^{\wedge}\textbf{R}(t) \end{aligned} R(t)R(t0)+R(t0)˙(tt0)=I+ϕ(t0)R(t)
这是一个关于 R \textbf{R} R的微分方程,而且我们知道初始值 R ( 0 ) = I \textbf{R}(0)=\textbf{I} R(0)=I,解之,得到
R ( t ) = e x p ( ϕ 0 ∧ t ) \begin{aligned} \textbf{R}(t)=exp(\phi_{0}^{\wedge}t) \end{aligned} R(t)=exp(ϕ0t)
李代数so(3)
ϕ \phi ϕ实际上是一种李代数,每个 ϕ \phi ϕ都可以生成一个反对称矩阵:
Φ = ϕ ∧ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] ∈ R 3 × 3 \begin{aligned} \Phi=\phi^{\wedge}=\left[\begin{matrix} 0&-\phi_{3}&\phi_{2}\\ \phi_{3}&0&-\phi_{1}\\ -\phi_{2}&\phi_{1}&0\end{matrix}\right]\in\mathbb{R}^{3\times3} \end{aligned} Φ=ϕ=0ϕ3ϕ2ϕ30ϕ1ϕ2ϕ10R3×3
在这个定义下两个向量的李括号为
[ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∨ \begin{aligned} [\phi_{1},\phi_{2}]=(\Phi_{1}\Phi_{2}-\Phi_{2}\Phi_{1})^{\vee} \end{aligned} [ϕ1,ϕ2]=(Φ1Φ2Φ2Φ1)
不加区别的
s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } \begin{aligned} so(3)=\{\phi\in\mathbb{R}^{3},\Phi=\phi^{\wedge}\in\mathbb{R}^{3\times3}\} \end{aligned} so(3)={ϕR3,Φ=ϕR3×3}
s o ( 3 ) so(3) so(3)是由一个三维向量组成的集合,每个向量对应到一个反对称矩阵,可以表达旋转矩阵的导数,与李群SO(3)的关系由指数映射给定:
R = e x p ( ϕ ∧ ) \begin{aligned} \textbf{R}=exp(\phi^{\wedge}) \end{aligned} R=exp(ϕ)
李代数se(3)
对于SE(3),也有对应的李代数se(3)。李代数se(3)位于 R 6 \mathbb{R}^{6} R6空间中:
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } \begin{aligned} se(3)=\{\xi=\left[\begin{matrix} \rho\\ \phi \end{matrix}\right]\in\mathbb{R}^{6},\rho\in\mathbb{R}^{3},\phi\in so(3),\xi^{\wedge}=\left[\begin{matrix} \phi^{\wedge}&\rho\\ 0^{T}&0 \end{matrix}\right]\in\mathbb{R}^{4\times4}\} \end{aligned} se(3)={ξ=[ρϕ]R6,ρR3,ϕso(3),ξ=[ϕ0Tρ0]R4×4}
这里同样使用 ∧ ^{\wedge} 符号,将一个六维向量转化为四维矩阵,但这里不再表示反对称:
ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 \begin{aligned} \xi^{\wedge}=\left[\begin{matrix} \phi^{\wedge}&\rho\\ 0^{T}&0 \end{matrix}\right]\in\mathbb{R}^{4\times4} \end{aligned} ξ=[ϕ0Tρ0]R4×4
李代数se(3)有类似与so(3)的李括号:
[ ξ 1 , ξ 2 ] = ( ξ 1 ∧ ξ 2 ∧ − ξ 2 ∧ ξ 1 ∧ ) ∨ \begin{aligned} [\xi_{1},\xi_{2}]=(\xi_{1}^{\wedge}\xi_{2}^{\wedge}-\xi_{2}^{\wedge}\xi_{1}^{\wedge})^{\vee} \end{aligned} [ξ1,ξ2]=(ξ1ξ2ξ2ξ1)
在这里插入图片描述
BCH公式与近似形式
引入李代数的目的是为了做BA优化,那么在SO(3)中完成矩阵乘法时,李代数会怎样,是我们关心的问题
两个李代数指数映射乘积的完整形式,有Baker-Campbell-Hausdorff公式给出,展开式前几项如下:
l n ( e x p ( ϕ 1 ∧ ) e x p ( ϕ 2 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 i f ϕ 1 i s s m a l l J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 i f ϕ 2 i s s m a l l \begin{aligned} ln(exp(\phi_{1}^{\wedge})exp(\phi_{2}^{\wedge}))^{\vee}\approx \left\{\begin{matrix} \textbf{J}_{l}(\phi_{2})^{-1}\phi_{1}+\phi_{2} & if \quad\phi_{1} \quad is \quad small\\ \textbf{J}_{r}(\phi_{1})^{-1}\phi_{2}+\phi_{1} &if \quad \phi_{2}\quad is\quad small \end{matrix}\right. \end{aligned} ln(exp(ϕ1)exp(ϕ2)){Jl(ϕ2)1ϕ1+ϕ2Jr(ϕ1)1ϕ2+ϕ1ifϕ1issmallifϕ2issmall
这里利用左乘模型:
e x p ( △ ϕ ∧ ) e x p ( ϕ ∧ ) = e x p ( ( ϕ + J l − 1 ( ϕ ) △ ϕ ) ∧ ) \begin{aligned} exp(\triangle\phi^{\wedge})exp(\phi^{\wedge})=exp((\phi+\textbf{J}_{l}^{-1}(\phi)\triangle\phi)^{\wedge}) \end{aligned} exp(ϕ)exp(ϕ)=exp((ϕ+Jl1(ϕ)ϕ))
反之,如果我们在李代数上进行加法,让一个 ϕ \phi ϕ加上 △ ϕ \triangle\phi ϕ,那么可以近似为李群上带左右雅可比的乘法:
e x p ( ( ϕ + △ ϕ ) ∧ ) = e x p ( ( J l △ ϕ ) ∧ ) e x p ( ϕ ∧ ) = e x p ( ϕ ∧ ) e x p ( ( J r △ ϕ ) ∧ ) \begin{aligned} exp((\phi+\triangle\phi)^{\wedge})=exp((\textbf{J}_{l}\triangle\phi)^{\wedge})exp(\phi^{\wedge})=exp(\phi^{\wedge})exp((\textbf{J}_{r}\triangle\phi)^{\wedge}) \end{aligned} exp((ϕ+ϕ))=exp((Jlϕ))exp(ϕ)=exp(ϕ)exp((Jrϕ))
同样对于SE(3),也有BCH近似公式:
e x p ( △ ξ ∧ ) e x p ( ξ ∧ ) ≈ e x p ( ( J l − 1 △ ξ + ξ ) ∧ ) \begin{aligned} exp(\triangle\xi^{\wedge})exp(\xi^{\wedge})\approx exp((\mathcal{J}_{l}^{-1}\triangle\xi+\xi)^{\wedge}) \end{aligned} exp(ξ)exp(ξ)exp((Jl1ξ+ξ))
e x p ( ξ ∧ ) e x p ( △ ξ ∧ ) ≈ e x p ( ( J r − 1 △ ξ + ξ ) ∧ ) \begin{aligned} exp(\xi^{\wedge})exp(\triangle\xi^{\wedge})\approx exp((\mathcal{J}_{r}^{-1}\triangle\xi+\xi)^{\wedge}) \end{aligned} exp(ξ)exp(ξ)exp((Jr1ξ+ξ))
SO(3)上李代数的求导
不妨设某个时刻相机的位姿为 T T T,他观察到了一个世界坐标位于 p p p的点,产生了一个观测数据,并且假设观测噪声为 z z z
z = T p + w e = z − T p \begin{aligned} z=Tp+w\\ e=z-Tp \end{aligned} z=Tp+we=zTp
假设一共有 N N N个这样的观测点,对位姿 T T T的估计就相当于寻找一个 T T T使得整体误差最小化:
m i n T J ( T ) = ∑ i = 1 N ∣ ∣ z i − T p i ∣ ∣ 2 2 \begin{aligned} \mathop{min}\limits_{T} J(\textbf{T})=\sum\limits_{i=1}^{N}||z_{i}-Tp_{i}||_{2}^{2} \end{aligned} TminJ(T)=i=1NziTpi22
经常需要构造与位姿有关的函数,然后讨论该函数关于位姿的导数,以调整当前的估计值。但是SO(3),SE(3)上加法没有定义,所以转到李代数解决求导问题:
1.用李代数表示姿态,然后根据李代数加法对李代数进行求导
2.对李群左乘或者右乘微小扰动,然后对该扰动求导

1.李代数求导
考虑对一个空间点 p p p进行旋转,得到了 R p Rp Rp,由于SO(3)没有加法 ∂ ( R p ) ∂ R \frac{\partial(Rp)}{\partial R} R(Rp)无法按照导数的定义进行计算,所以利用李代数进行计算:
∂ ( e x p ( ϕ ∧ ) p ) ∂ ϕ = lim ⁡ δ ϕ → 0 e x p ( ( ϕ + δ ϕ ) ∧ ) p − e x p ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 e x p ( ( J l δ ϕ ) ∧ ) e x p ( ϕ ∧ ) p − e x p ( ϕ ∧ ) p δ ϕ ≈ lim ⁡ δ ϕ → 0 ( I + ( J l δ ϕ ) ∧ ) e x p ( ϕ ∧ ) p − e x p ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 ( J l δ ϕ ) ∧ e x p ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 − ( e x p ( ϕ ∧ ) p ) ∧ J l δ ϕ δ ϕ = − ( Rp ) ∧ J l \begin{aligned} \frac{\partial(exp(\phi^{\wedge})p)}{\partial \phi}=\lim\limits_{\delta\phi\rightarrow0} \frac{exp((\phi+\delta\phi)^{\wedge})p-exp(\phi^{\wedge})p}{\delta\phi}\\=\lim\limits_{\delta\phi\rightarrow0} \frac{exp((\textbf{J}_{l}\delta\phi)^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\delta\phi}\\\approx\lim\limits_{\delta\phi\rightarrow0}\frac{(\textbf{I}+(\textbf{J}_{l}\delta\phi)^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\delta\phi}\\=\lim\limits_{\delta\phi\rightarrow0}\frac{(\textbf{J}_{l}\delta\phi)^{\wedge}exp(\phi^{\wedge})p}{\delta\phi}=\lim\limits_{\delta\phi\rightarrow0}\frac{-(exp(\phi^{\wedge})p)^{\wedge}\textbf{J}_{l}\delta\phi}{\delta\phi}\\=-(\textbf{Rp})^{\wedge}\textbf{J}_{l} \end{aligned} ϕ(exp(ϕ)p)=δϕ0limδϕexp((ϕ+δϕ))pexp(ϕ)p=δϕ0limδϕexp((Jlδϕ))exp(ϕ)pexp(ϕ)pδϕ0limδϕ(I+(Jlδϕ))exp(ϕ)pexp(ϕ)p=δϕ0limδϕ(Jlδϕ)exp(ϕ)p=δϕ0limδϕ(exp(ϕ)p)Jlδϕ=(Rp)Jl
于是
∂ ( e x p ( ϕ ∧ ) p ) ∂ ϕ = − ( Rp ) ∧ J l \begin{aligned} \frac{\partial(exp(\phi^{\wedge})p)}{\partial \phi}=-(\textbf{Rp})^{\wedge}\textbf{J}_{l} \end{aligned} ϕ(exp(ϕ)p)=(Rp)Jl
由于含有形式较为复杂的雅可比矩阵 J l \textbf{J}_{l} Jl,一般不采用,扰动模型会更简单
SO(3)扰动模型
R R R进行一次扰动 △ R \triangle R R,以左扰动为例子,设左扰动 △ R \triangle R R对应的李代数为 ψ \psi ψ:
∂ ( R p ) ∂ ψ = lim ⁡ ψ → 0 e x p ( ψ ∧ ) e x p ( ϕ ∧ ) p − e x p ( ϕ ∧ ) p ψ ≈ lim ⁡ ψ → 0 ( 1 + ψ ∧ ) e x p ( ϕ ∧ ) p − e x p ( ϕ ∧ ) p ψ = lim ⁡ ψ → 0 ψ ∧ R p ψ = lim ⁡ ψ → 0 − ( R p ) ∧ ψ ψ = − ( R p ) ∧ \begin{aligned} \frac{\partial(Rp)}{\partial \psi}=\lim\limits_{\psi\rightarrow0}\frac{exp(\psi^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\psi}\\ \approx \lim\limits_{\psi\rightarrow0}\frac{(1+\psi^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\psi}\\ =\lim\limits_{\psi\rightarrow0}\frac{\psi^{\wedge}Rp}{\psi}=\lim\limits_{\psi\rightarrow0}\frac{-(Rp)^{\wedge}\psi}{\psi}=-(Rp)^{\wedge} \end{aligned} ψ(Rp)=ψ0limψexp(ψ)exp(ϕ)pexp(ϕ)pψ0limψ(1+ψ)exp(ϕ)pexp(ϕ)p=ψ0limψψRp=ψ0limψ(Rp)ψ=(Rp)
SE(3)扰动模型
∂ ( T p ) ∂ δ ξ = lim ⁡ δ ξ → 0 e x p ( δ ξ ∧ ) e x p ( ξ ∧ ) p − e x p ( ξ ∧ ) p δ ξ ≈ lim ⁡ δ ξ → 0 ( I + δ ξ ∧ ) e x p ( ξ ∧ ) p − e x p ( ξ ∧ ) p δ ξ = lim ⁡ δ ξ → 0 δ ξ ∧ e x p ( ξ ∧ ) p δ ξ = lim ⁡ δ ξ → 0 [ δ ϕ ∧ δ ρ 0 T 0 ] [ R p + t 1 ] δ ξ = lim ⁡ δ ξ → 0 [ δ ϕ ∧ ( R p + t ) + δ p 0 ] δ ξ = [ I − ( R p + t ) ∧ 0 T 0 T ] ≜ ( T P ) ⊙ \begin{aligned} \frac{\partial(Tp)}{\partial \delta\xi}=\lim\limits_{\delta\xi\rightarrow0}\frac{exp(\delta\xi^{\wedge})exp(\xi^{\wedge})p-exp(\xi^{\wedge})p}{\delta\xi}\\ \approx \lim\limits_{\delta\xi\rightarrow0}\frac{(\textbf{I}+\delta\xi^{\wedge})exp(\xi^{\wedge})p-exp(\xi^{\wedge})p}{\delta\xi}\\=\lim\limits_{\delta\xi\rightarrow0}\frac{\delta\xi^{\wedge}exp(\xi^{\wedge})p}{\delta\xi}=\lim\limits_{\delta\xi\rightarrow0}\frac{\left[\begin{matrix} \delta\phi^{\wedge}&\delta\rho\\ 0^{T}&0 \end{matrix}\right]\left[\begin{matrix} Rp+t\\ 1 \end{matrix}\right]}{\delta\xi}\\=\lim\limits_{\delta\xi\rightarrow0}\frac{\left[\begin{matrix} \delta\phi^{\wedge}(Rp+t)+\delta p\\ 0 \end{matrix}\right]}{\delta\xi}=\left[\begin{matrix} \textbf{I}&-(Rp+t)^{\wedge}\\ 0^{T}&0^{T} \end{matrix}\right]\triangleq (TP)^{\odot} \end{aligned} δξ(Tp)=δξ0limδξexp(δξ)exp(ξ)pexp(ξ)pδξ0limδξ(I+δξ)exp(ξ)pexp(ξ)p=δξ0limδξδξexp(ξ)p=δξ0limδξ[δϕ0Tδρ0][Rp+t1]=δξ0limδξ[δϕ(Rp+t)+δp0]=[I0T(Rp+t)0T](TP)
运算符 ⊙ ^{\odot} 把齐次坐标的空间点变换成一个 4 × 6 4\times6 4×6的矩阵

©️2020 CSDN 皮肤主题: 数字20 设计师:CSDN官方博客 返回首页