基于LO-RANSAC算法拟合圆柱-编程实验室

1、算法概述

在对地面、建筑物、低矮地物的滤除后，点云数据中只剩下了杆状地物和少量的非杆状地物。通过对杆状地物的观察，杆状地物如路灯、道路指示牌、监控杆、交通信号杆等通常具有近似圆柱体状或长方体状的几何特征，因此提出了一种LO-RANSAC算法对点云数据进行圆柱拟合，利用拟合出的圆柱体的几何参数为后续杆状地物提取工作做准备。同时在点云数据圆柱拟合后，数据中存在的异常值可以被去除，一定程度上提升了数据的鲁棒性。
对于LO-RANSAC算法，因为RANSAC算法拟合的圆柱模型依赖随机选取的样本点，容易受到采样误差的影响，为了进一步优化拟合结果，在基于RANSAC算法拟合出的圆柱模型基础上，引入LM最小二乘法，优化拟合出的圆柱模型参数，以达到对模型局部优化的效果，提高拟合模型质量。由于该算法本质是对RANSAC拟合出的圆柱进行局部优化，所以将提出的算法命名为LO-RANSAC算法。

2、RANSAC算法拟合圆柱

RANSAC算法是一种迭代算法，用于从包含噪声和异常值的数据集中估计数学模型的参数。它通过随机抽样和一致性测试，逐步筛选出最佳模型。RANSAC算法拟合模型的核心思想是先从原始数据集中随机选取最小样本集（圆柱拟合最少需要三个点），使用所选的样本集对模型进行拟合，求出模型的初始参数。接着给定一个合适的阈值，计算所有数据点到求出的拟合模型的误差，确定数据集中所有符合模型要求的数据点（内点），其余为外点。多次重复上述步骤，选择拥有最多内点的模型作为最终估计。

基于RANSAC算法拟合圆柱的过程如下：

假设原始点云数据为P i P_iPi，其中{ P i = ( x i , y i , z i ) ∣ i = 1 , 2 , 3 , ⋯ , n } \{P_i=(x_i,y_i,z_i) \mid i=1,2,3,\cdots,n\}{Pi=(xi,yi,zi)∣i=1,2,3,⋯,n}，随机选择三个点P 1 、 P 2 、 P 3 P_1、P_2、P_3P1、P2、P3用于估计圆柱的轴向量和半径：

以前两个点P 1 、 P 2 P_1、P_2P1、P2构建轴向量V = ( V x , V y , V z ) V=(V_x,V_y,V_z)V=(Vx,Vy,Vz)：
V = P 2 − P 1 ∥ P 2 − P 1 ∥ V=\frac{P_2-P_1}{\|P_2-P_1\|}V=∥P2−P1∥P2−P1
用第三个点P 3 P_3P3计算该点到轴线的垂直距离d 3 d_3d3，即为拟合圆柱的初始半径R RR：
R = d 3 = ∥ ( P 3 − P 1 ) × V ∣ ∥ V ∥ R=d_3=\frac{\|(P_3-P_1)\times V|}{\|V\|}R=d3=∥V∥∥(P3−P1)×V∣

内点/外点判定：对于点云中每个点P i P_iPi，投影到中轴线上得投影点proj ⁡ ( P i ) \operatorname{proj}(P_i)proj(Pi)，点P i P_iPi到投影点的距离为d proj d_{\text{proj}}dproj，残差：
residual ⁡ ( P i ) = ∥ P i − P proj ∥ − R \operatorname{residual}(P_i)=\|P_i-P_{\text{proj}}\|-Rresidual(Pi)=∥Pi−Pproj∥−R

设定距离阈值ε = 2 cm \varepsilon=2\text{cm}ε=2cm，若residual ⁡ ( P i ) < ε \operatorname{residual}(P_i)<\varepsilonresidual(Pi)<ε则为内点，否则为外点。迭代次数设为1000次，选择内点数量最多的圆柱模型为最终拟合结果。

💡 RANSAC拟合除噪声外，理想情况点云应与圆柱吻合，但实际拟合结果中仍有部分点云未与圆柱吻合，故引入LM最小二乘法进一步优化。

3、LM最小二乘法优化圆柱

在RANSAC拟合出的圆柱模型基础上，使用LM最小二乘法优化圆柱参数：轴上点坐标C、轴向量V、半径R。LM方法结合了梯度下降和高斯-牛顿法，通过阻尼因子λ \lambdaλ调整优化策略——λ \lambdaλ大时接近梯度下降（稳定），λ \lambdaλ小时接近高斯-牛顿（收敛快）。

优化过程：

令参数集合P = ( x c , y c , z c , v x , v y , v z , R ) P=(x_c,y_c,z_c,v_x,v_y,v_z,R)P=(xc,yc,zc,vx,vy,vz,R)，定义残差函数：
r i ( P ) = ∥ P i − P proj ∥ − R r_i(P)=\|P_i-P_{\text{proj}}\|-Rri(P)=∥Pi−Pproj∥−R

目标：使残差平方和min ⁡ P K ∑ i = 1 n r i 2 ( P K ) \min\limits_{P_K}\sum\limits_{i=1}^n r_i^2(P_K)PKmini=1∑nri2(PK)最小。

参数更新公式：
P k + 1 = P k − ( J T J + λ I ) − 1 J T r P_{k+1}=P_k-(J^TJ+\lambda I)^{-1}J^TrPk+1=Pk−(JTJ+λI)−1JTr

其中J = ∂ r i ∂ P J=\frac{\partial r_i}{\partial P}J=∂P∂ri为雅可比矩阵，r rr为残差向量。通过动态调整λ \lambdaλ使参数更新量Δ P \Delta PΔP最小，当Δ E < 0.1 cm \Delta E<0.1\text{cm}ΔE<0.1cm（Δ E \Delta EΔE为新旧残差平方和之差）时停止迭代。