ransac算法_RANSAC直线拟合（五十）

方法1：

/*************************************************Author:  SayheyheyheyDate:2019-7-4Description:根据伪代码实现通用的RANSAC模板自定义线性模型，实现两种方式的直线拟合**************************************************/#include #include #include #include #include #include #include using namespace cv;#include using namespace std;//数据点类型struct st_point{
      st_point(){};  st_point(double X, double Y) :x(X), y(Y){};  double x;  double y;};/*** @brief 线性模型** Ax+By+C = 0;*/class linearModel{
    public:  //待估计参数  double A, B, C;public:  linearModel(){};  ~linearModel(){};  //使用两个点对直线进行初始估计  void Update(vector &data, set<int> &maybe_inliers){
        assert(maybe_inliers.size() == 2);    //初始化的点不为2个，报错    //根据索引读取数据    vector<int> points(maybe_inliers.begin(), maybe_inliers.end());    st_point pts1 = data[points[0]];    st_point pts2 = data[points[1]];    //根据两个点计算直线参数(得到其中一组解，可以任意比例缩放)    double delta_x = pts2.x - pts1.x;    double delta_y = pts2.y - pts1.y;    A = delta_y;    B = -delta_x;    C = -delta_y*pts2.x + delta_x*pts2.y;  }  //返回点到直线的距离  double computeError(st_point point){
        double numerator = abs(A*point.x + B*point.y + C);    double denominator = sqrt(A*A + B*B);    return numerator / denominator;  }  //根据一致点的集合对直线进行重新估计  double Estimate(vector &data, set<int> &consensus_set){
        assert(consensus_set.size() >= 2);    //求均值 means    double mX, mY;    mX = mY = 0;    //for (auto &index: consensus_set){
        for(int index = 0; index < consensus_set.size(); index++)    {
          mX += data[index].x;      mY += data[index].y;    }    mX /= consensus_set.size();    mY /= consensus_set.size();    //求二次项的和 sum    double sXX, sYY, sXY;    sXX = sYY = sXY = 0;    //for (auto &index : consensus_set){
        for (int index = 0; index < consensus_set.size(); index++)    {
          st_point point;      point = data[index];      sXX += (point.x - mX)*(point.x - mX);      sYY += (point.y - mY)*(point.y - mY);      sXY += (point.x - mX)*(point.y - mY);    }        //解法1：求y=kx+b的最小二乘估计，然后再转换成一般形式    //参考 https://blog.csdn.net/hookie1990/article/details/91406309    bool isVertical = sXY == 0 && sXX < sYY;    bool isHorizontal = sXY == 0 && sXX > sYY;    bool isIndeterminate = sXY == 0 && sXX == sYY;    double k = 0;    double b = 0;    if (isVertical)    {
        A = 1;    B = 0;    C = mX;    }    else if (isHorizontal)    {
        A = 0;    B = 1;    C = mY;    }    else if (isIndeterminate)    {
        A = 0;    B = 0;    C = 0;    }    else    {