MATLAB点云处理(十六):多项式曲线拟合(RANSACMSAC)

MATLAB 点云处理（⼗六）：多项式曲线拟合（RANSACMSAC ）⽂章⽬录

1 多项式拟合函数 fitPolynomialRANSAC

fitPolynomialRANSAC — 使⽤RANSAC算法从点云中进⾏多项式拟合

该函数使⽤M-估计量样本⼀致性（MSAC）算法，即随机样本⼀致性（RANSAC）算法的改进算法来拟合数据。主要有 2 种重载⽅式

NO.1 给定数据点xyPoints 、拟合阶数N 、内点到模型的最⼤距离maxDistance ，返回多项式系数P P = fitPolynomialRANSAC (xyPoints ,N ,maxDistance )通过对xyPoints 中给定的个⼆维平⾯点进⾏多项式拟合，到多项式系数P（按将幂排列）。对于多项式，对应的多项式系数为其中，

xyPoints ：即[x y]，为⼆维平⾯坐标点，为m×2阶矩阵

N ：多项式拟合阶数，为⼤于 0 的整数

maxDistance ：内点到模型的最⼤距离，为正数

P ：多项式系数，作为数值向量返回。每个元素对应于N次多项式⽅程中的⼀个系数数。例如，对于⼆

次多项式，对应 NO.2 给定数据点xyPoints 、拟合阶数N 、内点到模型的最⼤距离maxDistance ，返回多项式系数P 和内点的线性索引inlierIdx

[P ,inlierIdx ] = fitPolynomialRANSAC (xyPoints ,N ,maxDistance )

2 代码实现

⽰例1：对空间种⼀组曲线点，投影到xoy平⾯进⾏RANSAC多项式拟合（直线拟合）

代码：

m p (x )=p x +1n p x +2n −1...+p x +n p n +1P =[p ,p ,...,p ,p ]12n n +1Ax +2Bx +C =0P [A ,B ,C ]

clc;

clear;

%加载点云

ptCloud =pcread('line.pcd');

figure;

pcshow(ptCloud);

title('原始点云');

xlabel('X(m)');

ylabel('Y(m)');

zlabel('Z(m)');

%提取xoy平⾯坐标点

x = ptCloud.Location(:,1);

y = ptCloud.Location(:,2);

figure;

plot(x,y,'.');

title('xoy平⾯投影点云');

xlabel('X(m)');

ylabel('Y(m)');

zlabel('Z(m)');

%设置多项式拟合阶数 N

N =1;

%设置内点到模型的最⼤距离

maxDistance =0.1;

%执⾏MSAC多项式拟合

[P, inlierIdx]=fitPolynomialRANSAC([x,y],N,maxDistance);

%输出多项式系数

%使⽤polyval计算多项式，⽤红⾊圆圈标记异常值

x1 =linspace(min(x),max(x));%在区间[min.x,max.x]内⽣成100个等间距的x，⽤与计算多项式的值yRecoveredCurve =polyval(p,x1);

figure;

plot(x1,yRecoveredCurve,'-g','LineWidth',2);

hold on;

拟合直线

plot(x(inlierIdx),y(inlierIdx),'.',x(~inlierIdx),y(~inlierIdx),'ro');

legend('多项式拟合曲线','内点','外点','Location','SouthEast');

title('拟合结果展⽰')

xlabel('X(m)');

ylabel('Y(m)');

zlabel('Z(m)');

hold off;

结果展⽰：

P =

1×2 single ⾏向量

0.6178 1.0817

⽰例2：对空间种⼀组曲线点，投影到xoy平⾯进⾏RANSAC多项式拟合（⼆次曲线拟合）代码：

clc;

clear;

%加载点云

ptCloud =pcread('test2.pcd');

%提取xoy平⾯坐标点

x = ptCloud.Location(:,1);

y = ptCloud.Location(:,2);

figure;

plot(x,y,'.');

title('xoy平⾯投影点云');

xlabel('X(m)');

ylabel('Y(m)');

zlabel('Z(m)');

%设置多项式拟合阶数 N

N =2;

%设置内点到模型的最⼤距离

maxDistance =0.1;

%执⾏MSAC多项式拟合

[P, inlierIdx]=fitPolynomialRANSAC([x,y],N,maxDistance);

%输出多项式系数

%使⽤polyval计算多项式，⽤红⾊圆圈标记异常值。

x1 =linspace(min(x),max(x));%在区间[min.x,max.x]内⽣成100个等间距的x，⽤与计算多项式的值yRecoveredCurve =polyval(P,x1);

figure;

plot(x1,yRecoveredCurve,'-g','LineWidth',2);

hold on;

plot(x(inlierIdx),y(inlierIdx),'.',x(~inlierIdx),y(~inlierIdx),'ro');

legend('多项式拟合曲线','内点','外点','Location','SouthEast');

title('拟合结果展⽰')

xlabel('X(m)');

ylabel('Y(m)');

zlabel('Z(m)');

hold off;

结果展⽰：

P =

1×3 single ⾏向量

0.2424-0.0054-1.2563