方法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 { k = (sYY - sXX + sqrt((sYY - sXX) * (sYY - sXX) + 4.0 * sXY * sXY)) / (2.0 * sXY); //斜率 b = mY - k * mX; //截距 //正则化项，使得A^2+B^2 = 1; double normFactor = 1 / sqrt(1 + k*k); A = normFactor * k; B = -normFactor; C = normFactor*b; } //返回残差 if (isIndeterminate){ return 0; } double error = A*A*sXX + 2 * A*B*sXY + B*B*sYY; error /= consensus_set.size(); return error; 解法2： //if (sXX == 0){ // A = 1; // B = 0; // C = -mX; //} //else{ // A = sXY / sXX; // B = -1; // C = mY - A*mX; // //归一化令A^2+B^2 = 1; // double normFactor = sqrt(A*A + B*B); // A /= normFactor; // B /= normFactor; // C /= normFactor; //} //double error = A*A*sXX + 2 * A*B*sXY + B*B*sYY; //error /= consensus_set.size(); //求平均误差 //return error; }};/*** @brief 运行RANSAC算法** @param[in] data 一组观测数据* @param[in] n 适用于模型的最少数据个数* @param[in] k 算法的迭代次数* @param[in] t 用于决定数据是否适应于模型的阀值* @param[in] d 判定模型是否适用于数据集的数据数目 * @param[in&out] model 自定义的待估计模型，为该函数提供Update、computeError和Estimate三个成员函数* 运行结束后，模型参数被设置为最佳的估计值* @param[out] best_consensus_set 输出一致点的索引值* @param[out] best_error 输出最小损失函数*/template<typename T, typename U>int ransac(vector &data, int n, int k, double t, int d, U &best_model, set<int> &best_consensus_set, double &best_error){ //1.初始化 int iterations = 0; //迭代次数 U maybe_model; //使用随机选点初始化求得的模型 U better_model; //根据符合条件的一致点拟合出的模型 int isFound = 0; //算法成功的标志 set<int> maybe_inliers; //初始随机选取的点(的索引值) //best_error = DBL_MAX; //初始化为最大值 best_error = 1.7976931348623158e+308; default_random_engine rng(time(NULL)); //随机数生成器 uniform_int_distribution<int> dist(0, data.size() - 1); //采用均匀分布 //2.主循环 while (iterations < k){ //3.随机选点 maybe_inliers.clear(); while (1){ int index = dist(rng); maybe_inliers.insert(index); if (maybe_inliers.size() == n){ break; } } //4.计算初始值 maybe_model.Update(data, maybe_inliers); //自定义函数，更新模型 set<int> consensus_set(maybe_inliers.begin(), maybe_inliers.end()); //选取模型后，根据误差阈值t选取的内点(的索引值) //5.根据初始模型和阈值t选择内点 for (int i = 0; i < data.size(); i++){ double error_per_item = maybe_model.computeError(data[i]); if (error_per_item < t){ consensus_set.insert(i); } } //6.根据全部的内点重新计算模型 if (consensus_set.size() > d){ double this_error = better_model.Estimate(data, consensus_set); //自定义函数，(最小二乘)更新模型，返回计算出的误差 //7.若当前模型更好，则更新输出量 if (this_error < best_error){ best_model = better_model; best_consensus_set = consensus_set; best_error = this_error; } isFound = 1; } ++iterations; } return isFound;}int main(){ //1.读入数据 //int data_size; //输入第一行表示数据大小 //cin >> data_size; //vector Points(data_size); //for (int i = 0; i < data_size; i++){ // cin >> Points[i].x >> Points[i].y; //} //测试用 clock_t start_time1 = clock(); vector Points; //{ st_point(3, 4), st_point(6, 8), st_point(9, 12), st_point(15, 20), st_point(10, -10)}; //st_point(3, 4), st_point(6, 8), st_point(9, 12), st_point(15, 20), st_point(10, -10); Points.push_back(st_point(3, 4)); Points.push_back(st_point(6, 8)); Points.push_back(st_point(9, 12)); Points.push_back(st_point(15, 20)); Points.push_back(st_point(10, -10)); int data_size = Points.size(); //2.设置输入量 int k = 50; //最大迭代次数 int n = 2; //适用于模型的最少数据个数 double t = 0.01; //用于决定数据是否适应于模型的阀值 int d = data_size*0.5; //判定模型是否适用于数据集的数据数目 //3.初始化输出量 linearModel best_model; //最佳线性模型 set<int> best_consensus_set; //记录一致点索引的set double best_error; //最小残差 //4.运行RANSAC int status = ransac(Points, n, k, t, d, best_model, best_consensus_set, best_error); //5.输出 cout <<"best_model: "<< best_model.A << " " << best_model.B << " " << best_model.C << endl; //cout << best_consensus_set << endl; cout <<"best_error: "<< best_error << endl; clock_t end_time1 = clock(); double processTime1 = static_cast<double>(end_time1 - start_time1) / CLOCKS_PER_SEC * 1000; cout << "processTime1: " << processTime1 << endl; system("pause"); return 0;}
参考文献：
https://blog.csdn.net/LaplaceSmoothing/article/details/94581854
方法2：
#include #include #include #include #include #include using namespace cv;#include // Date: 2018-01-09// Author: HSW//// RANSAC 直线拟合算法// 其他的类似//using namespace std;typedef struct st_Point{ double x; double y;}st_Point;// 基于RANSAC算法的直线拟合// pstData: 指向存储数据的指针// dataCnt: 数据点个数// lineParameterK: 直线的斜率// lineParameterB: 直线的截距// minCnt: 模型(直线)参数估计所需的数据点的个数// maxIterCnt: 最大迭代次数// maxErrorThreshold: 最大误差阈值// consensusCntThreshold: 模型一致性判断准则// modelMeanError: 模型误差// 返回值：返回0表示获取最优模型， 否则表示未获取最优模型int ransacLiner(st_Point* pstData, int dataCnt, int minCnt, double maxIterCnt, int consensusCntThreshold, double maxErrorThreshod, double& A, double& B, double& C, double& modelMeanError){ default_random_engine rng; uniform_int_distribution<unsigned> uniform(0, dataCnt - 1); rng.seed(10); // 固定随机数种子 set<unsigned int> selectIndexs; // 选择的点的索引 vector selectPoints; // 选择的点 set<unsigned int> consensusIndexs; // 满足一致性的点的索引 double line_A = 0; double line_B = 0; double line_C = 0; modelMeanError = 0; int isNonFind = 1; unsigned int bestConsensusCnt = 0; // 满足一致性估计的点的个数 int iter = 0; while (iter < maxIterCnt) { selectIndexs.clear(); selectPoints.clear(); // Step1: 随机选择minCnt个点 while (1) { unsigned int index = uniform(rng); selectIndexs.insert(index); if (selectIndexs.size() == minCnt) { break; } } // Step2: 进行模型参数估计 (y2 - y1)*x - (x2 - x1)*y + (y2 - y1)x2 - (x2 - x1)y2= 0 set<unsigned int>::iterator selectIter = selectIndexs.begin(); while (selectIter != selectIndexs.end()) { unsigned int index = *selectIter; selectPoints.push_back(pstData[index]); selectIter++; } double deltaY = (selectPoints[1]).y - (selectPoints[0]).y; double deltaX = (selectPoints[1]).x - (selectPoints[0]).x; line_A = deltaY; line_B = -deltaX; line_C = -deltaY * (selectPoints[0]).x + deltaX * (selectPoints[0]).y; // Step3: 进行模型评估: 点到直线的距离 int dataIter = 0; double meanError = 0; set<unsigned int> tmpConsensusIndexs; while (dataIter < dataCnt) { double distance = (line_A * pstData[dataIter].x + line_B * pstData[dataIter].y + line_C) / sqrt(line_A*line_A + line_B*line_B); distance = distance > 0 ? distance : -distance; if (distance < maxErrorThreshod) { tmpConsensusIndexs.insert(dataIter); } meanError += distance; dataIter++; } // Step4: 判断一致性: 满足一致性集合的最小元素个数条件 + 至少比上一次的好 if (tmpConsensusIndexs.size() >= bestConsensusCnt && tmpConsensusIndexs.size() >= consensusCntThreshold) { bestConsensusCnt = consensusIndexs.size(); // 更新一致性索引集合元素个数 modelMeanError = meanError / dataCnt; consensusIndexs.clear(); consensusIndexs = tmpConsensusIndexs; // 更新一致性索引集合 A = line_A; B = line_B; C = line_C; isNonFind = 0; } iter++; } return isNonFind;}#define MAX_LINE_CORRECT_POINT_CNT (40)#define MAX_LINE_NOISE_POINT_CNT (5)int main(){ //st_Point dataPoints[MAX_LINE_CORRECT_POINT_CNT + MAX_LINE_NOISE_POINT_CNT]; //memset(dataPoints, 0, sizeof(dataPoints)); //int iter; //for (iter = 0; iter < MAX_LINE_CORRECT_POINT_CNT; ++iter) //{ // dataPoints[iter].x = iter; // dataPoints[iter].y = iter * 2 + 5; // y = 2 * x + 5 数据 //} //int totalCnt = MAX_LINE_CORRECT_POINT_CNT + MAX_LINE_NOISE_POINT_CNT; //for (iter = MAX_LINE_CORRECT_POINT_CNT; iter < totalCnt; ++iter) //{ // dataPoints[iter].x = iter; // dataPoints[iter].y = iter * 2 + 1; // y = 2 * x + 1 噪声 //} clock_t start_time1 = clock(); st_Point dataPoints[5]; dataPoints[0].x = 3; dataPoints[0].y = 4; dataPoints[1].x = 6; dataPoints[1].y = 8; dataPoints[2].x = 9; dataPoints[2].y = 12; dataPoints[3].x = 15; dataPoints[3].y = 20; dataPoints[4].x = 10; dataPoints[4].y = -10; //Points.push_back(st_point(6, 8)); //Points.push_back(st_point(9, 12)); //Points.push_back(st_point(15, 20)); //Points.push_back(st_point(10, -10)); double A = 0; double B = 0; double C = 0; double meanError = 0; // 参数不准确 if (!ransacLiner(dataPoints, 5, 2, 10, 2.5, 0.01, A, B, C, meanError)) { cout << "A = " << A << endl; cout << "B = " << B << endl; cout << "C = " << C << endl; cout << "meanError = " << meanError << endl; } else { cout << "RANSAC Failed " << endl; } // 参数不准确 //if (!ransacLiner(dataPoints, totalCnt, 2, 30, 35, 0.1, A, B, C, meanError)) //{ // cout << "A = " << A << endl; // cout << "B = " << B << endl; // cout << "C = " << C << endl; // cout << "meanError = " << meanError << endl; //} //else //{ // cout << "RANSAC Failed " << endl; //} 参数准确 //if (!ransacLiner(dataPoints, totalCnt, 2, 50, 35, 0.1, A, B, C, meanError)) //{ // cout << "A = " << A << endl; // cout << "B = " << B << endl; // cout << "C = " << C << endl; // cout << "meanError = " << meanError << endl; //} //else //{ // cout << "RANSAC Failed " << endl; //} clock_t end_time1 = clock(); double processTime1 = static_cast<double>(end_time1 - start_time1) / CLOCKS_PER_SEC * 1000; cout << "processTime1"<< processTime1 << endl; system("pause"); return 0;}
参考文献：
https://blog.csdn.net/hit1524468/article/details/80375495
机器人实时纠偏系统(一)机器人实时纠偏(二)OpenCV+VS开发环境配置(三)结构光视觉的焊接机器人纠偏(四)结构光视觉的机器人焊接(五)结构光视觉的机器人焊接(六)机器人初始点导引(七)MATLAB标定相机参数(八)机器人的手眼标定(九)机器人坐标获取(十)机器人调试(十一)TCP/IP客户端API编程(十二)结构光传感器上位机界面多线程编程(十三)TCP&UDP(十四)C/C++ Programing(十五)机器人扫描与跟踪调试(十六)结构光传感器库函数(十七)结构光传感器编程(十八)C/C++ Programing(十九)C/C++ Programing(二十)结构光传感器编程(二十一)DX200操作要领(二十二)DX200操作要领(二十三)工装轴协调(二十四)无夹具协调(二十五)图像处理调试(二十六)STM32MODBUS_CRC编程(二十七)
在C++中调用Matlab函数(二十八)
机器人手眼标定MATLAB及C++实现
机器人位姿运算及Eigen的使用(三十)
OpenCV与Eigen矩阵运算(三十一)
VS中数据读写及OpenCV拟合(三十二)
VS2013配置OpenGL库(三十三)
曲线拟合/插值(三十四)
曲线拟合绘制滤波及机器人平移(三十五)
DX200操作要领—示教1(三十六)
直接打开与平移变换(三十七)PAM与镜像平移变换(三十八)
修改与编辑程序(三十九)
YRC1000 宏程序命令(四十)
程序编辑与试运行(四十一)
程序编辑与再现(四十二)
再现(四十三)
程序管理(四十四)
便捷功能(四十五)
便捷功能(四十六)
椭圆拟合(四十七)
RANSAC直线拟合(四十八)
读写CSV文件类(四十九)