文章目录
雅克比矩阵推导
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)
最低0.47元/天 解锁文章
5772
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
