基于M估计样本一致性算法的点云平面拟合-LMLPHP

1、算法简介

  RANSAC 是在给定模型和距离阈值 T T T的情况下,通过寻找最小代价 C C C来确定内点数据并拟合模型。如式(1)所示的代价函数,当点到模型的距离 e e e小于阈值 T T T时,该点被判定为内点且权重为0;否则为外点。由于所有内点的权重为0,所以RANSAC选取的最优模型是内点最多且权重和最小的模型。如果用于区分异常值的阈值 T T T过大,则会造成RANSAC 拟合误差增大。
ρ ( e , T ) = { 1 , T ≤ e 0 , T > e (1) \rho(e,T)= \begin{cases} 1,\quad T\leq e\\ 0, \quad T>e \end{cases} \tag{1} ρ(e,T)={1,Te0,T>e(1)

  MSAC 是RANSAC 的一种改进方法,目的是降低算法对于距离阈值 T T T选取的依赖。修改后的代价函数如式(1)所示,当点到模型的距离 e e e 小于阈值 T T T时,该点被判定为内点且权重为 e e e ;否则为外点且权重为 T T T
ρ ( e , T ) = { T , T ≤ e e , T > e (2) \rho(e,T)= \begin{cases} T,\quad T\leq e\\ e, \quad T>e \end{cases} \tag{2} ρ(e,T)={T,Tee,T>e(2)

  由于使用更加精确的距离表达代价函数,因此,MSAC 不仅可以得到更好的模型,还可以降低阈值的敏感度以及提高收敛速度。利用MSAC 进行局部基准面拟合的具体步骤如下:

  1. 从数据集中不重复地随机选择用于平面模型拟合的最小数据集 Q Q Q
  2. 利用最小二乘法和数据集 Q Q Q拟合平面模型 S S S ,得到模型参数。
  3. 计算所有点到平面模型 S S S的距离,在允许误差阈值范围内的点标记为“内点”,并计算该平面模型的代价 C i C_i Ci
  4. 比较当前模型的代价 C i C_i Ci 与之前最好模型的代价 C b C_b Cb的大小,记录较小者作为新的最好模型的代价,并记录对应的“内点”和模型参数。
  5. 重复步骤①-④,直到迭代结束,并得到“内点”数据。
  6. 利用“内点”数据,通过最小二乘法拟合平面模型,得到最佳的平面模型参数。

2、参考文献

3、实现效果

1、标准模拟数据
基于M估计样本一致性算法的点云平面拟合-LMLPHP
2、模拟数据的平面参数
基于M估计样本一致性算法的点云平面拟合-LMLPHP
3、添加高斯噪声点
基于M估计样本一致性算法的点云平面拟合-LMLPHP
4、高斯噪声点添加结果
基于M估计样本一致性算法的点云平面拟合-LMLPHP
5、MSAC拟合结果
基于M估计样本一致性算法的点云平面拟合-LMLPHP

平面模型系数coeff(a,b,c,d): 0.00987843  -0.714149        0.699925        17.943

偏差有点大???神奇!!!

4、相关代码

11-26 11:18