多传感器融合定位 第二讲 3D激光里程计1
一、理论
1.1 ICP 算法
ICP全称 Iterative Closest Point， 中文翻译为迭代最近点。核心思想：寻找一个最佳的旋转R和平移t使得从源点云经过这个旋转和平移后得到的点云与目标点云最接近。根据这一思想，可以表达成以下的数学公式：
m i n f ( R , t ) = ∑ i = 1 N ( x i − R y i − t ) 2 min f(R,t) =\sum\limits_{i=1}^N (x_i-Ry_i-t)^2 minf(R,t)=i=1∑N(xi−Ryi−t)2其中，目标点云和源点云分别为 X = { x 1 , x 2 , x 3 , . . . . , x n } X=\{x_1,x_2,x_3,....,x_n\} X={
x1,x2,x3,....,xn}， Y = { y 1 , y 2 , y 3 , . . . . , y n } Y=\{y_1,y_2,y_3,....,y_n\} Y={
y1,y2,y3,....,yn}。该问题的解可以通过SVD分解的方式或优化的方式解的。
参考文献：A method for registration of 3-D shapes
1.2 NDT 算法
NDT全称 Normal Distributions Transform， 中文翻译为正态分布变换。核心思想：将目标点云分割成若干个栅格，并假定每个栅格里面点的分布服从正态分布。寻找一个最佳的旋转R和平移t使得源点云经过这个旋转和平移后得到的点云与目标栅格里面点云的联合概率最大。根据这一思想，可以表达成以下的数学公式：
m a x f ( R , t ) = ∑ i = 1 N 1 2 π Σ e x p ( − ( R y i + t − μ j ) T Σ j − 1 ( R y i + t − μ j ) 2 ) max f(R,t)=\sum\limits_{i=1}^N \frac{1}{\sqrt{2\pi}\sqrt{\varSigma}}exp(-\frac{(Ry_i+t-\mu_j )^T \varSigma^{-1}_j (Ry_i+t - \mu_j)}{2}) maxf(R,t)=i=1∑N2πΣ1exp(−2(Ryi+t−μj)TΣj−1(Ryi+t−μj))等价于
m i n f ( R , t ) = ∑ i = 1 N ( R y i + t − μ j ) T Σ j − 1 ( R y i + t − μ j ) min f(R,t)= \sum\limits_{i=1}^N (Ry_i+t-\mu_j )^T \varSigma^{-1}_j (Ry_i+t - \mu_j) minf(R,t)=i=1∑N(Ryi+t−μj)TΣj−1(Ryi+t−μ
最低0.47元/天 解锁文章
扫一扫
710
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
