使用RANSAC拟合二维直线

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-1
	for (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-1
			int 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=0
		float 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);