方法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檔案類(四十九)