<div class="._5ce-wx-style" style="font-size:16px;">
<div class="rich_media_content" id="js_content">
<p><img alt="9a5aab284dead063ee410acbe7cc402b.png" src="https://beijingoptbbs.oss-cn-beijing.aliyuncs.com/cs/5606289-e9f77df708668f0bed2c74990b561079.png"></p>
<p>方法1：</p>
<pre class="blockcode"><code>/*************************************************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 it |
|
转播
