文章目录
雅克比矩阵推导
loam中有两种误差,即点到线的距离
和点到面的距离
。这两者都可以用目标外一点到目标上的投影点的距离表示。即误差 f ( X ) = D ( p w , p t ) f(X)=D\left(p^{w}, p^{t}\right) f(X)=D(pw,pt)
定义:
D ( p w , p t ) D\left(p^{w}, p^{t}\right) D(pw,pt)为两点间距离, p l p^l pl 为当前帧的点在局部坐标系下坐标, p w p^w pw为当前帧的点在世界坐标系下坐标, p t p^{t} pt 为对应线/面上目标点在世界坐标系的坐标, X X X为当前系统的状态 { R , T } \{R,T\} {
R,T}。开始计算偏导:
p w = G ( p l , X ) = R p l + T ∂ f ∂ X = ∂ f ∂ G ( p l , X ) ∗ ∂ G ( p l , X ) ∂ X \begin{aligned} &p^{w}=G\left(p^{l}, X\right)=R p^{l}+T \\ & \frac{\partial f}{\partial X}=\frac{\partial f}{\partial G\left(p^{l}, X\right)} * \frac{\partial G\left(p^{l}, X\right)}{\partial X} \end{aligned} pw=G(pl,X)=