RANSAC拟合平面(C++)（再差的代码自己也得写）

参考资料：

[1]https://www.jianshu.com/p/d2c215f686c7
[2]https://www.jianshu.com/p/f6af5615f6a9

自己的解答：

//程序：用ransac实现平面拟合
#include <iostream>
#include<ctime>
#include<vector>
#include<set>
#include<math.h>
using namespace std;

void PlaneFittingRansac(const vector<double> x_data,const vector<double> y_data,\
                        const vector<double> z_data,vector<double>& bestplane)
{
    if(x_data.size() ==0)
        return;

    cout<<"data number:"<<x_data.size()<<endl;

    int select_num[3]={0};
    int maxinliers=0;
    srand((unsigned)time(0));
    //迭代10次
    for(int i=0;i<10;i++)
    {
        //步骤1：从数据中随机选择3组数据
        //
        set<int> s;
        while(1)
        {
            int r = rand()%(x_data.size());
            s.insert(r);
            if(s.size() ==3)
                break;
        }
        int j=0;
        int select_num[3]={0};
        for(auto iElement = s.cbegin();iElement != s.end();iElement++)
        {
            //cout<<*iElement<<" ";
            select_num[j++]=*iElement;
        }
        cout<<select_num[0]<<","<<select_num[1]<<","<<select_num[2]<<endl;
        cout<<"data1:"<<x_data[select_num[0]]<<","<<y_data[select_num[0]]<<","<<z_data[select_num[0]]<<endl;
        cout<<"data2:"<<x_data[select_num[1]]<<","<<y_data[select_num[1]]<<","<<z_data[select_num[1]]<<endl;
        cout<<"data3:"<<x_data[select_num[2]]<<","<<y_data[select_num[2]]<<","<<z_data[select_num[2]]<<endl;
        //步骤2：通过获得的数据获得模型参数
        double a=((z_data[select_num[0]]-z_data[select_num[2]])*(y_data[select_num[1]]-y_data[select_num[2]])-(z_data[select_num[1]]-z_data[select_num[2]])*(y_data[select_num[0]]-y_data[select_num[2]]))/\
                ((x_data[select_num[0]]-x_data[select_num[2]])*(y_data[select_num[1]]-y_data[select_num[2]])-(x_data[select_num[1]]-x_data[select_num[2]])*(y_data[select_num[0]]-y_data[select_num[2]]));
        double b=((z_data[select_num[1]]-z_data[select_num[2]])-a*(x_data[select_num[1]]-x_data[select_num[2]]))/\
                (y_data[select_num[1]]-y_data[select_num[2]]);
        double c=z_data[select_num[0]]-a*x_data[select_num[0]]-b*y_data[select_num[0]];
        cout<<"a:"<<a<<",b:"<<b<<",c:"<<c<<endl;
        //步骤3：统计内点个数
        int inliers=0;
        int sigma= 1.0;//阈值,可以自己调整
        for(int k=0;k<x_data.size();k++)
        {

            if(fabs(a*x_data[k]+b*y_data[k]-z_data[k]+c)<sigma)
                inliers++;
        }
        cout<<"inliers"<<inliers<<endl;
        //步骤4：选出内点个数最大的参数
        if(inliers>maxinliers)
        {
            maxinliers=inliers;
            bestplane.clear();
            bestplane.push_back(a);
            bestplane.push_back(b);
            bestplane.push_back(c);
        }
    }
}


int main()
{
    //z=ax+bx+c
    vector<double> x_data,y_data,z_data;
    //可以自己设定几个数，设定10组数据
    //z=x+y+1
    //怎么合理给数据，数据必须给的好，否则我都以为自己解方程解错了！！！数据不要给成1条线
    //1
    x_data.push_back(1.5);
    y_data.push_back(2.0);
    z_data.push_back(4.5);
    //2
    x_data.push_back(2.2);
    y_data.push_back(3.0);
    z_data.push_back(6.2);
    //3
    x_data.push_back(3.4);
    y_data.push_back(4.0);
    z_data.push_back(8.4);
    //4
    x_data.push_back(4.7);
    y_data.push_back(5.6);
    z_data.push_back(11.3);
    //5
    x_data.push_back(5.2);
    y_data.push_back(6.5);
    z_data.push_back(12.7);
    //6
    x_data.push_back(6.3);
    y_data.push_back(7.8);
    z_data.push_back(15.1);
    //7
    x_data.push_back(7.4);
    y_data.push_back(8.8);
    z_data.push_back(17.2);
    //8,离群点
    x_data.push_back(8.7);
    y_data.push_back(9.4);
    z_data.push_back(20.1);
    //9,离群点
    x_data.push_back(13.4);
    y_data.push_back(11.1);
    z_data.push_back(40.5);
    //10,离群点
    x_data.push_back(9.7);
    y_data.push_back(10.1);
    z_data.push_back(45.0);

    //z=ax+by+c
    //a,b,c3个参数
    vector<double> bestplane(3,0);

    PlaneFittingRansac(x_data,y_data,z_data,bestplane);
    cout<<"z=ax+by+c,estimate parameters:a="<<bestplane[0]<<",b="<<bestplane[1]<<",c="<<bestplane[2]<<endl;


}