PCL使用RANSAC拟合三位平面

1、使用PCL工具

1 1 //创建一个模型参数对象，用于记录结果
2 2 pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
3 3 //inliers表示误差能容忍的点，记录点云序号
4 4 pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
5 5 //创建一个分割器
6 6 pcl::SACSegmentation<pcl::PointXYZ> seg;
7 7 //Optional,设置结果平面展示的点是分割掉的点还是分割剩下的点
8 8 seg.setOptimizeCoefficients(true);
9 9 //Mandatory-设置目标几何形状
1010 seg.setModelType(pcl::SACMODEL_PLANE);
1111 //分割方法：随机采样法
1212 seg.setMethodType(pcl::SAC_RANSAC);
1313 //设置误差容忍范围，也就是阈值
1414 seg.setDistanceThreshold(0.01);
1515 //输入点云
1616 seg.setInputCloud (cloud);
1717 //分割点云
1818 seg.segment (*inliers, *coefficients);

2、RANSAC拟合平面代码

1while ((iterNum < iter_maxNum) && inPlaneNum_max <= RSample_pointsNum)
2{
3    inPlaneNum_t = 3;//当前拟合平面中点个数
4
5    //随机抽3个点，验证不在一条直线上
6    /*A(x1,y1)、B(x2,y2)、C(x3,y3)
7    AB斜率：kAB=(y2-y1)/(x2-x1)
8    BC斜率：kBC=(y3-y2)/(x3-x2)
9    计算结果可得：kAB=kBC
10    因为kAB=kBC，且共点B
11    所以直线AB与直线BC共线。*/
12    do {
13        rand_i_1 = real(gen);
14        rand_i_2 = real(gen);
15        if (rand_i_1 == rand_i_2)continue;
16        rand_i_3 = real(gen);
17        if (rand_i_1 == rand_i_3 || rand_i_2 == rand_i_3)continue;
18
19        x1 = r_sample[rand_i_1].x; x2 = r_sample[rand_i_2].x; x3 = r_sample[rand_i_3].x;
20        y1 = r_sample[rand_i_1].y; y2 = r_sample[rand_i_2].y; y3 = r_sample[rand_i_3].y;
21
22    } while (((y2 - y1)*(x3 - x2)) == ((y3 - y2)*(x2 - x1)));
23
24    //x1 = r_sample[rand_i_1].x; x2 = r_sample[rand_i_2].x; x3 = r_sample[rand_i_3].x;
25    //y1 = r_sample[rand_i_1].y; y2 = r_sample[rand_i_2].y; y3 = r_sample[rand_i_3].y;
26    z1 = r_sample[rand_i_1].z; z2 = r_sample[rand_i_2].z; z3 = r_sample[rand_i_3].z;
27    //求平面方程
28    A_t = (y2 - y1)*(z3 - z1) - (z2 - z1)*(y3 - y1);
29    B_t = (x3 - x1)*(z2 - z1) - (x2 - x1)*(z3 - z1);
30    C_t = (x2 - x1)*(y3 - y1) - (x3 - x1)*(y2 - y1);
31    D_t = -(A_t * x1 + B_t * y1 + C_t * z1);
32
33    //求在平面内的点的个数
34    temp = sqrt(A_t*A_t + B_t*B_t + C_t*C_t);//点到平面距离参数
35
36    for (int i = 0; i < RSample_pointsNum; i++)
37    {
38        temp_D = abs(A_t*r_sample[i].x + B_t*r_sample[i].y + C_t*r_sample[i].z + D_t) / temp;//点到平面距离
39        if (temp_D < maxD)
40        {
41            inPlaneNum_t++;
42        }
43    }
44    //与最优（最大）个数比较，保留最优个数的平面公式
45    if (inPlaneNum_t > inPlaneNum_max)
46    {
47        A_best = A_t;
48        B_best = B_t;
49        C_best = C_t;
50        D_best = D_t;
51        inPlaneNum_max = inPlaneNum_t;
52    }
53    iterNum++;//迭代次数+1
54}

 3、多点情况迭代次数的计算（转载于https://www.cnblogs.com/littlepear/p/10129861.html）

4、关于RANSAC算法https://blog.csdn.net/weixin_43795395/article/details/90751650讲得很好，可以参考

5、自适应阈值https://blog.csdn.net/hanshuobest/article/details/73718440