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算法成功的检测出圆模型，但最小二乘拟合的数据偏差较大。
