RANSAC算法的理解---直线检测和圆检测的小例子

算法流程：

贴一下关键函数（下面链接是数据和源码的工程文件）：
其他函数可以下载一下：
写了个父类和直线拟合的子类，如果想拟合其他模型，可以新建子类，将纯虚函数重写一下即可

链接：https://pan.baidu.com/s/1ehiKFKs7mszh1dBd986yNg
提取码：9cz2
复制这段内容后打开百度网盘手机App，操作更方便哦

//RANSAC.h
#ifndef  RANSAC_H
#define RANSAC_H

#include 
#include 
#include 
#include 

#include "read_txt.h"
#include "Matrix.h"
using namespace std;
using namespace piratfMatrixH;

class MyRANSAC
{
public:
	/*判断子集是否合格--------------
	input:
	data_:观测值
	output：
	bool：返回抽取的子集数据是否合格
	*/
	virtual bool isQualifiedSub(const vector>&data_, const vectorindex_) = 0;
	/*判断是否是内点（小于距离阈值即为内点）--------------
	input:
		x:观测值
		y:观测值
		model_：模型系数
	output：
		ThresholdDis：判断是否为内点的阈值
	*/
	virtual bool isInlier(const double x, const double y, const vector&model_, const double ThresholdDis) = 0;
	/*初始子集拟合目标模型--------------
	input:
		subSample:抽取的子观测集
	output：
		model_：模型系数
	*/
	virtual void ModelFitting(const vector>&subSample, vector&model_) = 0;
	/*传入样本数据和内点索引，提取内点的数据
	input:
		Sample:观测数据
		InliersIndex:内点索引
	output：
		X0：观测值X
		Y0：观测值Y
	*/
	//virtual void LeastSquareFittingDataReady(const vector>&Sample, vectorInliersIndex, Matrix&X0, Matrix&Y0) = 0;
	/**内点的数据最小二乘数据拟合
	input:
		X0:观测数据X
		Y0:观测数据Y
	output：
		finalmodel_：最小二乘算法拟合的模型系数
	*/
	virtual void LeastSquareFitting(const vector>&Sample, vectorInliersIndex, vector&finalmodel_) = 0;
	/*
	RANSAC算法实现-----------------------------------
	input:
		Sample:观测数据
		ThresholdDis：距离阈值
		ThresholdNumberInlier：最小内点数量阈值
		ThresholdNumberIter：最大迭代次数阈值
		NumberOrginalSub：初始拟合模型的抽样个数
	output：
		belief：拟合的置信度
		FinalModel:输出的模型系数
	*/
	void RANSAC_(const vector>&Sample, double ThresholdDis, int ThresholdNumberInlier, int ThresholdNumberIter, int NumberOrginalSub, double &belief, vector&FinalModel);

private:
	/*产生某个范围内的随机数--------------
	input:
		LowNumber:最低
		HeightNumber：最高
		Number：缺省参数，默认产生一个随机数
	output：
		vector：返回随机数（整型数）
	*/
	vector GetRandNumber(const int LowNumber, const int HeightNumber, int Number = 1);
	
	
};

//Ransac直线拟合----------------------------------------------------------------------------------------------------------------------------------------------
//继承于MyRANSAC父类，重写一些关键的纯虚函数........
class lineRansac : public MyRANSAC
{
public:
	 bool isQualifiedSub(const vector>&data_, const vectorindex_);
	 bool isInlier(const double x, const double y, const vector&model_, const double ThresholdDis);
	 void ModelFitting(const vector>&subSample, vector&model_);
	 //void LeastSquareFittingDataReady(const vector>&Sample, vectorInliersIndex, Matrix&X0, Matrix&Y0);
	 void LeastSquareFitting(const vector>&Sample, vectorInliersIndex, vector&finalmodel_);
private:


};

//Ransac圆检测----------------------------------------------------------------------------------------------------------------------------------------------
//继承于MyRANSAC父类，重写一些关键的纯虚函数........
class circleRansac : public MyRANSAC
{
public:
	bool isQualifiedSub(const vector>&data_, const vectorindex_);
	bool isInlier(const double x, const double y, const vector&model_, const double ThresholdDis);
	void ModelFitting(const vector>&subSample, vector&model_);
	void LeastSquareFitting(const vector>&Sample, vectorInliersIndex, vector&finalmodel_);
private:
};

#endif //  RANSAC_H

//RANSAC.cpp
#include "RANSAC.h"
vector MyRANSAC::GetRandNumber(const int LowNumber, const int HeightNumber, int Number){
	//生成Number个整型随机数
	static bool first = true;//只有第一次调用这个函数的时候才会刷新时间种子，否则多次调用会出现相同的随机数
	if (first)
	{
		srand(time(0));
		first = false;
	}
	vectortemp;
	temp.resize(Number);
	for (int i = 0; i < Number; i++)
	{
		temp[i] = int(1.0 *rand() / RAND_MAX * (HeightNumber - LowNumber + 1) + LowNumber);
	}

	return temp;
}
void MyRANSAC::RANSAC_(const vector>&Sample, double ThresholdDis, int ThresholdNumberInlier, int ThresholdNumberIter, int NumberOrginalSub, double &belief, vector&FinalModel){
	int NumberSample = Sample.size();
	vectorrandnumber;
	vectorInliersIndexTemp;
	/*存储内点索引的容器*/
	vectorInliersIndex;

	for (int iter_ = 0; iter_ < ThresholdNumberIter; iter_++)
	{
		while (true)
		{
			//随机生成两个不重复的索引序号
			randnumber = GetRandNumber(0, NumberSample - 1, NumberOrginalSub);
			//判断选择的子集是否合格
			if (isQualifiedSub(Sample, randnumber))
			{
				break;
			}
			randnumber.clear();
		}
		//模型拟合-----------------------------
		vector>subSample;
		vectortempsub, model_;
		for (int i_ = 0; i_ < randnumber.size(); i_++)
		{
			tempsub.push_back(Sample[randnumber[i_]][0]);
			tempsub.push_back(Sample[randnumber[i_]][1]);
			subSample.push_back(tempsub);
			tempsub.clear();
		}
		ModelFitting(subSample, model_);
		subSample.clear();
		//依次判断所有数据中哪些是属于内点的
		bool isInlier_ = false;
		for (int i = 0; i < NumberSample; i++)
		{
			isInlier_ = isInlier(Sample[i][0], Sample[i][1], model_, ThresholdDis);
			if (isInlier_)
			{
				InliersIndexTemp.push_back(i);
			}
		}
		//将每次迭代中内点数量多的那一个放到存储的容器中
		if (iter_ == 0)
		{
			InliersIndex = InliersIndexTemp;
		}
		else
		{
			if (InliersIndexTemp.size() > InliersIndex.size())
			{
				InliersIndex = InliersIndexTemp;
			}
		}
		//每次迭代军判断内点数量是否大于给定的阈值，....
		if (InliersIndex.size() >= ThresholdNumberInlier)//如果内点数量大于阈值给的数量，不进行后续的迭代，直接输出
		{
			break;
		}
		InliersIndexTemp.clear();
	}
	InliersIndexTemp.clear();

	//利用内点和最小二乘重新求一遍模型
	int NumberInlier = InliersIndex.size();
	LeastSquareFitting(Sample, InliersIndex, FinalModel);
	belief = 1.0*NumberInlier / NumberSample;
}

//-----------------------------------------------------------------------------------------------------------------
bool lineRansac::isQualifiedSub(const vector>&data_, const vectorindex_){

	if (index_[0] != index_[1])
	{
		return true;
	}
	return false;
}
bool lineRansac::isInlier(const double x, const double y, const vector&model_, const double ThresholdDis){

	double k = model_[0];
	double b = model_[1];
	bool isInlier_ = false;
	double tempdis = fabs(k*x + b - y);
	if (tempdis < ThresholdDis)
	{
		isInlier_ = true;
	}
	return isInlier_;
}
void lineRansac::ModelFitting(const vector>&subSample, vector&model_){
	//直线模型---------------------
	double x0 = subSample[0][0];
	double y0 = subSample[0][1];
	double x1 = subSample[1][0];
	double y1 = subSample[1][1];
	double k = (y0 - y1) / (x0 - x1);
	double b = y0 - k*x0;
	model_.clear();
	model_.push_back(k);
	model_.push_back(b);
}
void lineRansac::LeastSquareFitting(const vector>&Sample, vectorInliersIndex, vector&finalmodel_){
	MatrixX0, Y0;
	int nowInlierNumber = InliersIndex.size();
	X0.resize(nowInlierNumber, 2);
	Y0.resize(nowInlierNumber, 1);
	for (int i = 0; i < nowInlierNumber; i++){
		X0[i][0] = Sample[InliersIndex[i]][0];
		X0[i][1] = 1;
		Y0[i][0] = Sample[InliersIndex[i]][1];
	}

	Matrixmodel_;
	//Model = ((X0T*X0).INV)*X0T*Y0
	model_ = (X0.getTransposition().mul(X0)).inv().mul(X0.getTransposition()).mul(Y0);

	int rows_ = model_.row();

	finalmodel_.resize(rows_);
	for (int i = 0; i < rows_; i++)
	{
		finalmodel_[i] = model_[i][0];
	}
}
//-----------------------------------------------------------------------------------------------------------------
bool circleRansac::isQualifiedSub(const vector>&data_, const vectorindex_){

	double X[3], Y[3];
	for (int i = 0; i < 3; i++){
		X[i] = data_[index_[i]][0];
		Y[i] = data_[index_[i]][1];
	}
	double k1 = (Y[0] - Y[1]) / (X[0] - X[1]);
	double k2 = (Y[0] - Y[2]) / (X[0] - X[2]);

	double kThreshold = tan(1.0 / 180 * 3.1415926);//如果斜率差超过一度，则判定他们三点不共线
	if (fabs(k1 - k2)>kThreshold)
	{
		return true;
	}
	return false;
}
bool circleRansac::isInlier(const double x, const double y, const vector&model_, const double ThresholdDis){

	double xcenter = model_[0];
	double ycenter = model_[1];
	double r = model_[2];
	bool isInlier_ = false;

	double tempdis = fabs((x - xcenter)*(x - xcenter) + (y - ycenter)*(y - ycenter)-r*r);

	if (tempdis < ThresholdDis)
	{
		isInlier_ = true;
	}
	return isInlier_;
}
void circleRansac::ModelFitting(const vector>&subSample, vector&model_){
	//圆模型拟合---------------------
	double x0 = subSample[0][0];
	double y0 = subSample[0][1];
	double x1 = subSample[1][0];
	double y1 = subSample[1][1];
	double x2 = subSample[2][0];
	double y2 = subSample[2][1];

	double a = x0 - x1;
	double b = y0 - y1;
	double c = x0 - x2;
	double d = y0 - y2;
	double e = ((x0*x0 - x1*x1) - (y1*y1 - y0*y0)) / 2;
	double f = ((x0*x0 - x2*x2) - (y2*y2 - y0*y0)) / 2;

	double fm = b*c - a*d;

	double xcenter = -(d*e - b*f) / fm;
	double ycenter = -(a*f - c*e) / fm;
	double r = sqrt((x0 - xcenter)*(x0 - xcenter) + (y0 - ycenter)*(y0 - ycenter));

	model_.clear();
	model_.push_back(xcenter);
	model_.push_back(ycenter);
	model_.push_back(r);
}

void circleRansac::LeastSquareFitting(const vector>&Sample, vectorInliersIndex, vector&finalmodel_){
	vectorX0, Y0;
	int num = InliersIndex.size();
	X0.resize(num, 0);
	Y0.resize(num, 0);
	for (int i = 0; i < num; i++){
		X0[i] = Sample[InliersIndex[i]][0];
		Y0[i] = Sample[InliersIndex[i]][1];
	}
	double ALL_XX(0), ALL_X(0), ALL_Y(0), ALL_YY(0), ALL_XY(0), ALL_XYY(0), ALL_XXX(0), ALL_YYY(0), ALL_XXY(0), ALL_XXandYY(0);
	double D, E, F, H, I;
	for (int i = 0; i < num;i++)
	{
		ALL_XX = ALL_XX+(X0[i] * X0[i]);
		ALL_X = ALL_X + X0[i];
		ALL_Y = ALL_Y+Y0[i];
		ALL_YY = ALL_YY+(Y0[i] * Y0[i]);
		ALL_XY = ALL_XY+(X0[i] * Y0[i]);
		ALL_XYY = ALL_XYY+(X0[i] * Y0[i] * Y0[i]);
		ALL_XXX = ALL_XXX+(X0[i] * X0[i] * X0[i]);
		ALL_YYY = ALL_YYY+(Y0[i] * Y0[i] * Y0[i]);
		ALL_XXY = ALL_XXY+(X0[i] * X0[i] * Y0[i]);
		ALL_XXandYY = ALL_XXandYY+(X0[i] * X0[i] + Y0[i] * Y0[i]);
	}
	D = num*ALL_XX - ALL_X*ALL_X;
	E = num*ALL_XY - ALL_X*ALL_Y;
	F = num*ALL_XXX + num*ALL_XYY - ALL_XXandYY*ALL_X;
	H = num*ALL_YY - ALL_Y*ALL_Y;
	I = num*ALL_XXY + num*ALL_YYY - ALL_XXandYY*ALL_Y;

	double a, b, c;
	a = (E*I - F*H) / (D*H - E*E);
	b = (D*I - E*F) / (E*E - D*H);
	c = -(ALL_XXandYY + a*ALL_X + b*ALL_Y) / num;

	double x0 = -a / 2;
	double y0 = -b / 2;
	double r = sqrt(a*a + b*b - 4 * c)/2;
	finalmodel_.clear();
	finalmodel_.push_back(x0);
	finalmodel_.push_back(y0);
	finalmodel_.push_back(r);
}

//main.cpp
#include "RANSAC.h"
int main()
{
	/*
	//直线检测--------------------------------------------------------------------
	vector>Sample;
	string filename_ = "C:/Users/Administrator/Desktop/project.txt";
	read_txt(&filename_, Sample);
	double ThresholdDis = 0.3;
	int ThresholdNumberInlier = 300;
	int ThresholdNumberIter = 1000;
	int NumberOrginalSub = 2;
	double belief;
	vectorFinalModel;
	MyRANSAC *ptrline = new lineRansac;
	ptrline->RANSAC_(Sample, ThresholdDis, ThresholdNumberInlier, ThresholdNumberIter, NumberOrginalSub, belief, FinalModel);
	delete ptrline;*/
	
	
	//圆检测-----------------------------------------------------------------------
	//测试数据.............
	vector>Sample;
	vectortemp;
	double pi = 3.141592653;
	double r = 2.0;
	for (double i = 0; i < 2 * pi;i+=pi/10)
	{
		temp.push_back(r*cos(i));
		temp.push_back(r*sin(i));
		Sample.push_back(temp);
		temp.clear();
	}
	double noisex[21] = { -1, -0.5, 0, 0, 1, 2, - 2, 3, 0.5, - 3, - 1, - 1, 1, 2, - 2.5, 3, 2.5, 2.5, 0, 1, 0.5 };
	double noisey[21] = { -1, -1, -1, 0, 0.5, -2, 2, 3, 1.5, 1, 1, 0, -0.5, 3, 0, 0, 2, 0.5, 3, 2.5, 1 };
	for (int i = 0; i < 21;i++)
	{
		temp.push_back(noisex[i]);
		temp.push_back(noisey[i]);
		Sample.push_back(temp);
		temp.clear();
	}
	//开始检测....
	double ThresholdDis = 0.3;
	int ThresholdNumberInlier = 15;
	int ThresholdNumberIter = 1000;
	int NumberOrginalSub = 3;
	double belief;
	vectorFinalModel;
	MyRANSAC *ptrline = new circleRansac;
	ptrline->RANSAC_(Sample, ThresholdDis, ThresholdNumberInlier, ThresholdNumberIter, NumberOrginalSub, belief, FinalModel);
	delete ptrline;



	return 0;
}

示例数据的投影值xy平面的直线检测结果：

**

圆检测

**
算法在上面给出了，但最小二乘拟合的公式推导看下图或参考链接：https://blog.csdn.net/jacky_ponder/article/details/70314919?ops_request_misc=&request_id=&biz_id=102&utm_source=distribute.pc_search_result.none-task-blog-SOBAIDUWEB-0

用上面的算法求出的圆心和半径为（0,0）和2：

绘制测试数据的散点和检测出的圆

对比RANSAC圆检测算法和直接最小二乘拟合圆两种策略的区别：

给出的数据集有21个点满足圆的表达式，加入了噪声点21个，两种方法的结果如下表示，Ransac算法成功的检测出圆模型，但最小二乘拟合的数据偏差较大。