1、OpenCV实现

使用OpenCV实现二维直线的拟合：

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
using namespace cv;
using namespace std;int main( )
{const char* filename = "1.bmp";Mat src_image = imread(filename,1);if( src_image.empty() ){cout << "Couldn't open image!" << filename;return 0;}int img_width = src_image.cols;int img_height = src_image.rows;Mat gray_image,bool_image;cvtColor(src_image,gray_image,CV_RGB2GRAY);threshold(gray_image,bool_image,0,255,CV_THRESH_OTSU);imshow("二值图", bool_image);//获取二维点集vector<Point> point_set;Point point_temp;for( int i = 0; i < img_height; ++i){for( int j = 0; j < img_width; ++j ){if (bool_image.at<unsigned char>(i,j) < 255){point_temp.x = j;point_temp.y = i;point_set.push_back(point_temp);	}}}//直线拟合 	//拟合结果为一个四元素的容器，比如Vec4f - (vx, vy, x0, y0)//其中(vx, vy) 是直线的方向向量//(x0, y0) 是直线上的一个点Vec4f fitline;//拟合方法采用最小二乘法fitLine(point_set,fitline,CV_DIST_L2,0,0.01,0.01);//求出直线上的两个点double k_line = fitline[1]/fitline[0];Point p1(0,k_line*(0 - fitline[2]) + fitline[3]);Point p2(img_width - 1,k_line*(img_width - 1 - fitline[2]) + fitline[3]);//显示拟合出的直线方程:y=k_line*(x-fitline[2])+fitline[3]char text_equation[1024];sprintf(text_equation,"y-%.2f=%.2f(x-%.2f)",fitline[3],k_line,fitline[2]);putText(src_image,text_equation,Point(30,50),CV_FONT_HERSHEY_COMPLEX,0.5,Scalar(0,0,255),1,8);//显示拟合出的直线line(src_image,p1,p2,Scalar(0,0,255),2); imshow("原图+拟合结果", src_image);waitKey();return 0;
}

使用这种方式，只能在数据基本正确时，才能得到较好的拟合结果，当数据中存在噪声数据时，使用RANSAC的方式进行拟合是较好的拟合方式。

2、RANSAC二维直线拟合实现

二维直线的一般方程为：Ax+By+C=0。
若：已知直线上的两点P1(X1,Y1)和P2(X2,Y2)， P1和P2两点不重合；
则：

该式子对所有的直线方程均满足。
C++实现主要函数：

//mTrajectoryParams.MaxModelFitIterations=2000
int FitLineRansac(std::vector<cv::Point>& points, cv::Vec3f& line)
{int PointsNum = 2;std::vector<std::vector<size_t>> mvSets = std::vector<std::vector<size_t>>(mTrajectoryParams.MaxModelFitIterations, std::vector<size_t>(PointsNum, 0));//点的对数const int N = points.size();//新建一个容器vAllIndices存储点云索引，并预分配空间std::vector<size_t> vAllIndices;vAllIndices.reserve(N);//在RANSAC的某次迭代中，还可以被抽取来作为数据样本的索引，所以这里起的名字叫做可用的索引std::vector<size_t> vAvailableIndices;//初始化所有特征点对的索引，索引值0到N-1for (int i = 0; i < N; i++)vAllIndices.push_back(i);//用于进行随机数据样本采样，设置随机数种子SeedRandOnce(0);//开始每一次的迭代 for (int it = 0; it < mTrajectoryParams.MaxModelFitIterations; it++){//迭代开始的时候，所有的点都是可用的vAvailableIndices = vAllIndices;//选择最小的数据样本集for (size_t j = 0; j < PointsNum; j++){// 随机产生一对点的id,范围从0到N-1int randi = RandomInt(0, vAvailableIndices.size() - 1);// idx表示哪一个索引对应的点对被选中int idx = vAvailableIndices[randi];//将本次迭代这个选中的第j个特征点对的索引添加到mvSets中mvSets[it][j] = idx;// 由于这对点在本次迭代中已经被使用了,所以我们为了避免再次抽到这个点,就在"点的可选列表"中,// 将这个点原来所在的位置用vector最后一个元素的信息覆盖,并且删除尾部的元素// 这样就相当于将这个点的信息从"点的可用列表"中直接删除了vAvailableIndices[randi] = vAvailableIndices.back();vAvailableIndices.pop_back();}//依次提取出6个点}//迭代mMaxIterations次，选取各自迭代时需要用到的最小数据集int BestMatch = 0;std::multiset<float> verror;for (int it = 0; it < mTrajectoryParams.MaxModelFitIterations; it++){std::vector<cv::Point> pts;//选择6个点对进行迭代for (size_t j = 0; j < PointsNum; j++){//从mvSets中获取当前次迭代的某个特征点对的索引信息int idx = mvSets[it][j];cv::Point pt = points[idx];pts.push_back(pt);}//Ax+By+C=0float A = pts[1].y - pts[0].y;float B = pts[0].x - pts[1].x;float C = pts[1].x*pts[0].y - pts[0].x*pts[1].y;int Match = 0;verror.clear();for (int i = 0; i < points.size(); i++){float error = (A * points[i].x + B * points[i].y + C) / sqrt(A * A + B * B);if (abs(error) < 1)Match++;verror.insert(error);}if (Match > BestMatch){BestMatch = Match;line = cv::Vec3f(A, B, C);}}return 1;
}

随机处理函数：

//随机处理
static bool m_already_seeded = false;
inline void SeedRand(int seed)
{srand(seed);
}
inline void SeedRandOnce(int seed)
{//if (!m_already_seeded)//{SeedRand(seed);m_already_seeded = true;//}
}
inline int RandomInt(int min, int max)
{int d = max - min + 1;return int(((double)rand() / ((double)RAND_MAX + 1.0)) * d) + min;
}

绘制直线的例子：

cv::Vec3f Line;
FitLineRansac(points, Line);
cv::Mat imdraw = im.clone();
cv::cvtColor(imdraw, imdraw, cv::COLOR_GRAY2BGR);
cv::Point pt0, pt1;
if (abs(Line[0] / Line[1]) < 0.067)
{pt0 = cv::Point(0, -Line[2] / Line[1]);pt1 = cv::Point(imdraw.cols - 1, -Line[2] / Line[1]);
}
else if (abs(Line[0] / Line[1]) > 15)
{pt0 = cv::Point(-Line[2] / Line[0], 0);pt1 = cv::Point(-Line[2] / Line[0], imdraw.rows - 1);
}
else
{pt0 = cv::Point(0, -Line[2] / Line[1]);pt1 = cv::Point(-Line[2] / Line[0], 0);
}
cv::line(imdraw, pt0, pt1, cv::Scalar(0, 255, 0), 1, 8);