RANSAC算法(一)

这个算法主要是一种思想,并没有具体的代码,下面的代码是根据这种思想,进行的一维直线的拟合,也有与最小二乘法进行对比的结果

/*--------------------------------------------------------------------------------------------------
    *  @Copyright (c) , All rights reserved.
    *  @file:       myRANSAC.cpp
    *  @version:    ver 1.0
    *  @author:     闹闹
    *  @brief:  
    *  @change:
    *  @email: 	1319144981@qq.com
    *  Date             Version    Changed By      Changes 
    *  2021/5/17 15:11:30    1.0       闹闹            create
	写一句自己最喜欢的话吧。
	为天地立心,为生民立命,为往圣继绝学,为万世开太平。
--------------------------------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------
* modify_author:			闹闹
* modify_time:      2021/5/17 15:15:16
* modify_content:	 此版本主要用于线拟合
* modify_reference:
https://blog.csdn.net/xrwang/article/details/6232327
https://blog.csdn.net/u010551600/article/details/79013453?utm_medium=distribute.pc_relevant.none-task-blog-baidujs_title-0&spm=1001.2101.3001.4242
https://blog.csdn.net/robinhjwy/article/details/79174914?utm_medium=distribute.pc_relevant.none-task-blog-2~default~BlogCommendFromMachineLearnPai2~default-2.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2~default~BlogCommendFromMachineLearnPai2~default-2.control
https://blog.csdn.net/luoshixian099/article/details/50217655
https://blog.csdn.net/pamxy/article/details/9206485
* modify_other:
* modify_version:  1.0.0.2
--------------------------------------------------------------------------------------------------*/

#ifndef __MYRANSAC_H__
#define __MYRANSAC_H__
#include <iostream>
#include <vector>
#include <math.h>
#include <set>
#include <stdlib.h>
#include <time.h>
class Point2D {
public:
	Point2D(double px, double py) : x(px), y(py) {}
	double x;
	double y;
};
/*--------------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------*/
inline std::ostream &operator<<(std::ostream &output, const Point2D &pnt){
	output << pnt.x << ' ' << pnt.y;
	return(output);
}

/*
这个类定义了参数估计器的接口。
从它继承的类可以被RanSac类用于执行鲁棒参数估计。
*/
template<class T, class S>
class ParameterEsitmator {
public:
	/*--------------------------------------------------------------------------------------------------
	* @FuncName:  
	* @Author:    闹闹
	* @Brief:    参数的精确估计。使用最小数据量估计参数(精确估计)。
	* @Version:   1.0.0.1
	* @Date:	  2021/5/17 15:22
	* @InputParameter: data,用于估算的数据。parameters,被清除,用计算的结果填充
	* @OutputParameter: 
	* @Returns:   
	* @Others:    
	--------------------------------------------------------------------------------------------------*/
	virtual void estimate(std::vector<T *> &data, std::vector<S> &parameters) = 0;

	/*--------------------------------------------------------------------------------------------------
	* @FuncName:  
	* @Author:    闹闹
	* @Brief:   参数的最小二乘估计。使用不止最小量的数据来估计参数,从而使估计最小化最小平方误差标准。
	* @Version:   1.0.0.1
	* @Date:	  2021/5/17 15:19
	* @InputParameter: data,用于估算的数据。parameters,被清除,用计算的结果填充
	* @OutputParameter: 
	* @Returns:   
	* @Others:    
	--------------------------------------------------------------------------------------------------*/
	virtual void leastSquaresEstimate(std::vector<T *> &data, std::vector<S> &parameters) = 0;

	/*--------------------------------------------------------------------------------------------------
	* @FuncName:  
	* @Author:    闹闹
	* @Brief:     该方法测试给定的数据是否与给定的模型参数一致。给定的数据是否与模型参数一致。
	* @Version:   1.0.0.1
	* @Date:	  2021/5/17 15:18
	* @InputParameter: 
	* @OutputParameter: 
	* @Returns:   
	* @Others:    
	--------------------------------------------------------------------------------------------------*/
	virtual bool agree(std::vector<S> &parameters, T &data) = 0;
};


/*
此类估计2D线的参数。
2D线表示为:(*)dot(n,p-a)= 0 。其中n是线法线(| n | = 1),而'a'是线上的点。
满足方程式(*)的所有点'p'都在线上。
选择此行参数化的原因是它可以表示所有的直线,包括垂直和水平线。与斜率截距(y = ax + b)参数化不同。
虚函数在类内实现
*/
class LineParamEstimator : public ParameterEsitmator<Point2D, double> {
public:
	LineParamEstimator(double delta) : m_deltaSquared(delta*delta) {};
	/*--------------------------------------------------------------------------------------------------
	* @FuncName:  
	* @Author:    闹闹
	* @Brief:    计算由给定数据点定义的线。
	* @Version:   1.0.0.1
	* @Date:	  2021/5/17 15:53
	* @InputParameter: data,包含两个2D点的向量。parameters,清除此向量,然后填充计算出的参数。
	* @OutputParameter: 
	* @Returns:   
	* @Others:    
	通过这些点[n_x,n_y,a_x,a_y]的线的参数,其中||(n_x,ny)|| = 1。如果向量包含少于两个点,则结果参数向量为空(大小= 0)。
	Compute the line parameters  [n_x,n_y,a_x,a_y]
	通过输入的两点来确定所在直线,采用法线向量的方式来表示,以兼容平行或垂直的情况
	其中n_x,n_y为归一化后,与原点构成的法线向量,a_x,a_y为直线上任意一点
	--------------------------------------------------------------------------------------------------*/
	virtual void estimate(std::vector<Point2D *> &data, std::vector<double> &parameters) {
		parameters.clear();
		if (data.size() < 2) return;
		double nx = data[1]->y - data[0]->y;
		double ny = data[0]->x - data[1]->x;  // 原始直线的斜率为K,则法线的斜率为-1/k , ny / nx 是法向量。
		double norm = sqrt(nx*nx + ny*ny);
		parameters.push_back(nx / norm);
		parameters.push_back(ny / norm);
		parameters.push_back(data[0]->x);
		parameters.push_back(data[0]->y);
	};

	/*--------------------------------------------------------------------------------------------------
	* @FuncName:  
	* @Author:    闹闹
	* @Brief:    计算由给定点定义的直线的最小二乘估计。该实现具有正交最小二乘误差。
	* @Version:   1.0.0.1
	* @Date:	  2021/5/17 16:16
	* @InputParameter: data,该线应使这些点的最小二乘误差最小。parameters,清除此向量,然后填充计算出的参数。
	* @OutputParameter: 
	* @Returns:   
	* @Others:    
	使用计算的线参数[n_x,n_y,a_x,a_y]填充此向量,其中||(n_x,ny)|| = 1。如果向量包含少于两点,则结果参数向量为空(大小= 0)。
	Compute the line parameters  [n_x,n_y,a_x,a_y]
	使用最小二乘法,从输入点中拟合出确定直线模型的所需参量
	--------------------------------------------------------------------------------------------------*/
	virtual void leastSquaresEstimate(std::vector<Point2D *> &data, std::vector<double> &parameters) {
		parameters.clear();
		if (data.size() < 2) return;
		double meanX, meanY, nx, ny, norm;
		double covMat11, covMat12, covMat21, covMat22;															// 对称协方差矩阵的项
		meanX = meanY = 0.0;
		covMat11 = covMat12 = covMat21 = covMat22 = 0;
		int i, dataSize = static_cast<int>(data.size());

		for (i = 0; i < dataSize; i++) {
			meanX += data[i]->x;
			meanY += data[i]->y;
			covMat11 += data[i]->x * data[i]->x;
			covMat12 += data[i]->x * data[i]->y;
			covMat22 += data[i]->y * data[i]->y;
		}
		meanX /= dataSize;																						//均值
		meanY /= dataSize;
		covMat11 -= dataSize*meanX*meanX;
		covMat12 -= dataSize*meanX*meanY;
		covMat22 -= dataSize*meanY*meanY;
		covMat21 = covMat12;
		if (covMat11 < 1e-12) {
			nx = 1.0;
			ny = 0.0;
		}
		else {	    // lamda1是协方差矩阵的最大特征值,并用于计算与最小特征值相对应的特征向量,该特征值未明确计算。
			double lamda1 = (covMat11 + covMat22 + sqrt((covMat11 - covMat22)*(covMat11 - covMat22) + 4 * covMat12*covMat12)) / 2.0;
			nx = -covMat12;
			ny = lamda1 - covMat22;
			norm = sqrt(nx*nx + ny*ny);
			nx /= norm;
			ny /= norm;
		}
		parameters.push_back(nx);
		parameters.push_back(ny);
		parameters.push_back(meanX);
		parameters.push_back(meanY);
	};

	/*--------------------------------------------------------------------------------------------------
	* @FuncName:  
	* @Author:    闹闹
	* @Brief:     如果参数定义的线与给定点之间的距离小于'delta'(请参见构造函数),则返回true。
	* @Version:   1.0.0.1
	* @Date:	  2021/5/17 16:19
	* @InputParameter: parameters, 线参数[n_x,n_y,a_x,a_y]。 data,检查该点与直线之间的距离是否小于“ delta”。
	* @OutputParameter: 
	* @Returns:   
	* @Others:   
	Given the line parameters  [n_x,n_y,a_x,a_y] check if [n_x, n_y] dot [data.x-a_x, data.y-a_y] < m_delta
	通过与已知法线的点乘结果,确定待测点与已知直线的匹配程度;结果越小则越符合,为零则表明点在直线上
	--------------------------------------------------------------------------------------------------*/
	virtual bool agree(std::vector<double> &parameters, Point2D &data) {
		double signedDistance = parameters[0] * (data.x - parameters[2]) + parameters[1] * (data.y - parameters[3]);
		return ((signedDistance*signedDistance) < m_deltaSquared);
	};

	/*--------------------------------------------------------------------------------------------------
	* @FuncName:  
	* @Author:    闹闹
	* @Brief:     测试类方法。
	* @Version:   1.0.0.1
	* @Date:	  2021/5/17 16:24
	* @InputParameter: 
	* @OutputParameter: 
	* @Returns:   
	* @Others:    
	--------------------------------------------------------------------------------------------------*/
	static void debugTest(std::ostream &out) {
		std::vector<double> lineParameters;
		LineParamEstimator lpEstimator(0.5);
		std::vector<Point2D *> pointData;

		pointData.push_back(new Point2D(7, 7));
		pointData.push_back(new Point2D(-1, -1));
		lpEstimator.estimate(pointData, lineParameters);
		out << "[n_x,n_y,a_x,a_y] [ " << lineParameters[0] << ", " << lineParameters[1] << ", ";
		out << lineParameters[2] << ", " << lineParameters[3] << " ]" << std::endl;

		lpEstimator.leastSquaresEstimate(pointData, lineParameters);
		out << "[n_x,n_y,a_x,a_y] [ " << lineParameters[0] << ", " << lineParameters[1] << ", ";
		out << lineParameters[2] << ", " << lineParameters[3] << " ]" << std::endl;
		delete pointData[0];
		delete pointData[1];
		pointData.clear();

		pointData.push_back(new Point2D(6, 12));
		pointData.push_back(new Point2D(6, 6));
		lpEstimator.estimate(pointData, lineParameters);
		out << "[n_x,n_y,a_x,a_y] [ " << lineParameters[0] << ", " << lineParameters[1] << ", ";
		out << lineParameters[2] << ", " << lineParameters[3] << " ]" << std::endl;

		lpEstimator.leastSquaresEstimate(pointData, lineParameters);
		out << "[n_x,n_y,a_x,a_y] [ " << lineParameters[0] << ", " << lineParameters[1] << ", ";
		out << lineParameters[2] << ", " << lineParameters[3] << " ]" << std::endl;
		delete pointData[0];
		delete pointData[1];
		pointData.clear();

		pointData.push_back(new Point2D(7, 9));
		pointData.push_back(new Point2D(10, 9));
		lpEstimator.estimate(pointData, lineParameters);
		out << "[n_x,n_y,a_x,a_y] [ " << lineParameters[0] << ", " << lineParameters[1] << ", ";
		out << lineParameters[2] << ", " << lineParameters[3] << " ]" << std::endl;
		lpEstimator.leastSquaresEstimate(pointData, lineParameters);
		out << "[n_x,n_y,a_x,a_y] [ " << lineParameters[0] << ", " << lineParameters[1] << ", ";
		out << lineParameters[2] << ", " << lineParameters[3] << " ]" << std::endl;
		delete pointData[0];
		delete pointData[1];
		pointData.clear();
	};

private:
	double m_deltaSquared; //给定线L和点P,如果dist(L,P)^ 2 <m_delta ^ 2,则该点在线上
};

/*
此类实现了随机样本一致性(Ransac)框架,该框架用于可靠的参数估计。给定包含异常值的数据,我们使用原始数据的子集来估计模型参数。
1.从数据中选择最小子集,以计算精确的模型参数。
2.查看多少输入数据与计算出的参数一致。
3.转到步骤1。最多可以执行(m选择N)次,其中m是精确估计所需的数据对象数,而N是数据对象总数。
4.取得同意参数的最大对象子集,并使用它们计算最小二乘拟合。
类模板参数T 是用于参数估计的T对象(例如Point2D)。 S-参数类型(例如double)。
*/
template<class T, class S>
class Ransac {
public:
	/*--------------------------------------------------------------------------------------------------
	* @FuncName:  
	* @Author:    闹闹
	* @Brief:    使用Ransac框架估算模型参数。
	* @Version:   1.0.0.1
	* @Date:	  2021/5/17 17:24
	* @InputParameter:
	   parameters, 一个将包含估计参数的向量。 如果输入中有错误,则此向量将为空。 错误如下:1.数据对象比完全适合的数据对象少。
	   paramEstimator,可以使用精确拟合或最小二乘拟合来估计所需参数的对象。
	   data, 估计参数所依据的输入。
	   numForEstimate,完全适合所需的数据对象数。
	   desiredProbabilityForNoOutliers,至少一个选定子集不包含异常值的概率。
	   maximalOutlierPercentage,离群值的最大预期百分比。
	* @OutputParameter: 
	* @Returns:   返回最小二乘估计中使用的数据百分比。
	* @Others:    
	--------------------------------------------------------------------------------------------------*/
	static double compute(std::vector<S> &parameters,ParameterEsitmator<T, S> *paramEstimator,std::vector<T> &data,int numForEstimate,double desiredProbabilityForNoOutliers,double maximalOutlierPercentage);

	/*--------------------------------------------------------------------------------------------------
	* @FuncName:  
	* @Author:    闹闹
	* @Brief:    通过遍历所有可能的子集(暴力法),使用最大共识集估计模型参数。
	* @Version:   1.0.0.1
	* @Date:	  2021/5/17 17:30
	* @InputParameter: 
	  parameters,一个包含估计参数的向量,如果输入中有错误,则该向量将为空。 错误如下:1.数据对象比完全适合的要少。
	  paramEstimator,可以使用精确拟合或最小二乘法拟合估计所需参数的对象。
	  data,估计参数所依据的输入。
	  numForEstimate,完全适合所需的数据对象数。
	* @OutputParameter: 
	* @Returns:   返回最小二乘估计中使用的数据百分比。
	* @Others:   
	Given: n -  data.size()
	       k - numForEstimate
	We go over all n choose k subsets       n!
	*                                     ------------
	*                                      (n-k)! * k!
	仅当n选择k较小(即k或(n-k)大约等于n)时才应使用此方法
	--------------------------------------------------------------------------------------------------*/
	static double compute(std::vector<S> &parameters,ParameterEsitmator<T, S> *paramEstimator,std::vector<T> &data,int numForEstimate);

private:
	static void computeAllChoices(ParameterEsitmator<T, S> *paramEstimator, std::vector<T> &data, int numForEstimate,
								short *bestVotes, short *curVotes, int &numVotesForBest, int startIndex, 
								int n, int k, int arrIndex, int *arr);

	static void estimate(ParameterEsitmator<T, S> *paramEstimator, std::vector<T> &data, int numForEstimate,
						short *bestVotes, short *curVotes, int &numVotesForBest, int *arr);

	class SubSetIndexComparator { //子集序列比较器
	private:
		int m_length;
	public:
		SubSetIndexComparator(int arrayLength) : m_length(arrayLength) {}
		bool operator()(const int *arr1, const int *arr2) const {
			for (int i = 0; i < m_length; i++) {
				if (arr1[i] < arr2[i]) {
					return true;
				}
			}
			return false;
		}
	};

};

/*--------------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------*/
template<class T, class S>
double Ransac<T, S>::compute(std::vector<S> &parameters,ParameterEsitmator<T, S> *paramEstimator,std::vector<T> &data,int numForEstimate,double desiredProbabilityForNoOutliers,double maximalOutlierPercentage){
	int numDataObjects = data.size();
	if (numDataObjects < numForEstimate || maximalOutlierPercentage >= 1.0) return 0;   //数据对象的数量少于完全拟合所需的最小值,还是所有数据都是离群值?

	std::vector<T *> exactEstimateData;
	std::vector<T *> leastSquaresEstimateData;
	std::vector<S> exactEstimateParameters;
	int i, j, k, l, numVotesForBest, numVotesForCur, maxIndex, numTries;
	short *bestVotes = new short[numDataObjects];										//如果data [i]符合最佳模型,则为1,否则为0
	short *curVotes  = new short[numDataObjects];										//如果data [i]与当前模型一致,则为1,否则为0
	short *notChosen = new short[numDataObjects];										//如果未选择data [i]来计算精确拟合,则不为零;否则为零
	SubSetIndexComparator subSetIndexComparator(numForEstimate);
	std::set<int *, SubSetIndexComparator > chosenSubSets(subSetIndexComparator);
	int *curSubSetIndexes;
	double outlierPercentage = maximalOutlierPercentage;
	double numerator = log(1.0 - desiredProbabilityForNoOutliers);
	double denominator = log(1 - pow(1 - maximalOutlierPercentage, numForEstimate));
	parameters.clear();

	numVotesForBest = -1;																//用-1初始化,以便将第一个计算设置为最佳
	srand((unsigned)time(NULL));														//种子随机数生成器
	numTries = (int)(numerator / denominator + 0.5);
	
	for (i = 0; i < numTries; i++) {
		memset(notChosen, '1', numDataObjects * sizeof(short));							//随机选择用于精确模型拟合的数据(“ numForEstimate”对象)。
		curSubSetIndexes = new int[numForEstimate];
		exactEstimateData.clear();
		maxIndex = numDataObjects - 1;
		for (l = 0; l < numForEstimate; l++) {
			int selectedIndex = (int)(((float)rand() / (float)RAND_MAX)*maxIndex + 0.5); //selectedIndex在[0,maxIndex]中
			for (j = -1, k = 0; k < numDataObjects && j < selectedIndex; k++) {
				if (notChosen[k])
					j++;
			}
			k--;
			exactEstimateData.push_back(&(data[k]));
			notChosen[k] = 0;
			maxIndex--;
		}
		for (l = 0, j = 0; j < numDataObjects; j++) {									//获取所选对象的索引,以便我们可以检查是否尚未选择此子集
			if (!notChosen[j]) {
				curSubSetIndexes[l] = j + 1;
				l++;
			}
		}
		std::pair< std::set<int *, SubSetIndexComparator >::iterator, bool > res = chosenSubSets.insert(curSubSetIndexes);//检查刚刚选择的子集是否唯一
		if (res.second == true) {														//第一次我们选择了这个子集				 
			paramEstimator->estimate(exactEstimateData, exactEstimateParameters);		//使用选定的数据进行精确的模型参数拟合
			numVotesForCur = 0;															//看看有多少人同意这个估计
			memset(curVotes, '\0', numDataObjects * sizeof(short));
			for (j = 0; j < numDataObjects; j++) {
				if (paramEstimator->agree(exactEstimateParameters, data[j])) {
					curVotes[j] = 1;
					numVotesForCur++;
				}
			}
			if (numVotesForCur > numVotesForBest) {
				numVotesForBest = numVotesForCur;
				memcpy(bestVotes, curVotes, numDataObjects * sizeof(short));
			}
			outlierPercentage = 1 - (double)numVotesForCur / (double)numDataObjects;   //更新离群值的估计和我们需要的迭代次数
			if (outlierPercentage < maximalOutlierPercentage) {
				maximalOutlierPercentage = outlierPercentage;
				denominator = log(1 - pow(1 - maximalOutlierPercentage, numForEstimate));
				numTries = (int)(numerator / denominator + 0.5);
			}
		}
		else {																			//该子集已经出现,不计此迭代
			delete[] curSubSetIndexes;
			i--;
		}
	}
	std::set<int *, SubSetIndexComparator >::iterator it = chosenSubSets.begin();		//释放内存
	std::set<int *, SubSetIndexComparator >::iterator chosenSubSetsEnd = chosenSubSets.end();
	while (it != chosenSubSetsEnd) {
		delete[](*it);
		it++;
	}
	chosenSubSets.clear();
	for (j = 0; j < numDataObjects; j++) {												//使用最大子集计算最小二乘估计
		if (bestVotes[j])
			leastSquaresEstimateData.push_back(&(data[j]));
	}
	paramEstimator->leastSquaresEstimate(leastSquaresEstimateData, parameters);
	delete[] bestVotes;
	delete[] curVotes;
	delete[] notChosen;
	return (double)numVotesForBest / (double)numDataObjects;
}

/*--------------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------*/
template<class T, class S>
double Ransac<T, S>::compute(std::vector<S> &parameters,ParameterEsitmator<T, S> *paramEstimator,std::vector<T> &data,int numForEstimate){
	std::vector<T *> leastSquaresEstimateData;
	int numDataObjects = data.size();
	int numVotesForBest = -1;
	int *arr = new int[numForEstimate];													// numForEstimate表示拟合模型所需要的最少点数,对本例的直线来说,该值为2  
	short *curVotes = new short[numDataObjects];										//如果data [i]与当前模型一致,则为1,否则为0
	short *bestVotes = new short[numDataObjects];										//如果data [i]符合最佳模型,则为1,否则为0						  
	if (numDataObjects < numForEstimate) return 0;										//数据对象的数量少于完全拟合所需的最小值
	
	// 计算所有可能的直线,寻找其中误差最小的解。对于100点的直线拟合来说,大约需要100*99*0.5=4950次运算,复杂度无疑是庞大的。一般采用随机选取子集的方式。
	computeAllChoices(paramEstimator, data, numForEstimate,bestVotes, curVotes, numVotesForBest, 0, data.size(), numForEstimate, 0, arr);
	for (int j = 0; j < numDataObjects; j++) {											//使用最大子集计算最小二乘估计
		if (bestVotes[j])
			leastSquaresEstimateData.push_back(&(data[j]));
	}
	paramEstimator->leastSquaresEstimate(leastSquaresEstimateData, parameters);          // 对局内点再次用最小二乘法拟合出模型 
	delete[] arr;
	delete[] bestVotes;
	delete[] curVotes;
	return (double)leastSquaresEstimateData.size() / (double)numDataObjects;
}

/*--------------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------*/
template<class T, class S>
void Ransac<T, S>::computeAllChoices(ParameterEsitmator<T, S> *paramEstimator, std::vector<T> &data, int numForEstimate,short *bestVotes, short *curVotes, int &numVotesForBest, int startIndex, int n, int k, int arrIndex, int *arr){
	if (k == 0) {																		//我们有新的索引选择
		estimate(paramEstimator, data, numForEstimate, bestVotes, curVotes, numVotesForBest, arr);
		return;
	}
	int endIndex = n - k;																//继续递归地生成索引的选择
	for (int i = startIndex; i <= endIndex; i++) {
		arr[arrIndex] = i;
		computeAllChoices(paramEstimator, data, numForEstimate, bestVotes, curVotes, numVotesForBest,i + 1, n, k - 1, arrIndex + 1, arr);
	}
}

/*--------------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------*/
template<class T, class S>
void Ransac<T, S>::estimate(ParameterEsitmator<T, S> *paramEstimator, std::vector<T> &data, int numForEstimate,short *bestVotes, short *curVotes, int &numVotesForBest, int *arr){
	std::vector<T *> exactEstimateData;
	std::vector<S> exactEstimateParameters;
	int numDataObjects;
	int numVotesForCur;																	 //以-1初始化,以便将第一个计算设置为最佳
	int j;
	numDataObjects = data.size();
	memset(curVotes, '\0', numDataObjects * sizeof(short));
	numVotesForCur = 0;

	for (j = 0; j < numForEstimate; j++)
		exactEstimateData.push_back(&(data[arr[j]]));
	paramEstimator->estimate(exactEstimateData, exactEstimateParameters);
	for (j = 0; j < numDataObjects; j++) {
		if (paramEstimator->agree(exactEstimateParameters, data[j])) {
			curVotes[j] = 1;
			numVotesForCur++;
		}
	}
	if (numVotesForCur > numVotesForBest) {
		numVotesForBest = numVotesForCur;
		memcpy(bestVotes, curVotes, numDataObjects * sizeof(short));
	}
}
#endif  //__MYRANSAC_H__
/*----------------------------------------------------------------------------- (C) COPYRIGHT LEI *****END OF FILE------------------------------------------------------------------------------*/

example.cpp

#include "myRANSAC.hpp"


int  main(int argc, char* argv[])
{
	
	std::vector<double> lineParameters;
	LineParamEstimator lpEstimator(0.5); //要使点在直线上,它必须与直线之间的距离小于0.5个单位
	
	std::vector<Point2D> pointData;
	std::vector<Point2D *> pointDataPtr;
	
	int numForEstimate = 2;
	int numSamples = 40;                                    //样本点
	int numOutliers = 60;                                   //噪点
	double desiredProbabilityForNoOutliers = 0.999;
	double maximalOutlierPercentage = 0.1 + (double)numOutliers / (double)(numSamples + numOutliers);
	double noiseSpreadRadius = 0.4;                              //噪声传播半径
	double outlierSpreadRadius = 10;                             //离群点传播半径
	int i;
	double newX, newY, dx, dy, norm;

	//1.与异常值一起创建数据
	//随机选择一个方向[dx,dy]并为在该线上采样的每个点创建一条穿过原点的线,这会添加随机噪声,最后在线法线方向上添加偏外点。
	srand((unsigned)time(NULL)); //种子随机数生成器
	//得到随机方向
	dx = rand();
	dy = rand();
	norm = sqrt(dx*dx + dy*dy);
	dx /= norm;
	dy /= norm;
	dx *= (rand() > RAND_MAX / 2 ? 1 : -1);
	dy *= (rand() > RAND_MAX / 2 ? 1 : -1);

	//添加'numSamples'点
	for (i = 0; i < numSamples; i++) {
		newX = i*dx + noiseSpreadRadius*(double)rand() / (double)RAND_MAX * (rand() > RAND_MAX / 2 ? 1 : -1);
		newY = i*dy + noiseSpreadRadius*(double)rand() / (double)RAND_MAX * (rand() > RAND_MAX / 2 ? 1 : -1);
		pointDataPtr.push_back(new Point2D(newX, newY));
		pointData.push_back(*(pointDataPtr[i]));
	}

	//'numOutliers'点
	double centerX = -dy * 100;
	double centerY = dx * 100;
	for (i = 0; i < numOutliers; i++) {
		newX = centerX + outlierSpreadRadius * (double)rand() / (double)RAND_MAX * (rand() > RAND_MAX / 2 ? 1 : -1);
		newY = centerY + outlierSpreadRadius * (double)rand() / (double)RAND_MAX * (rand() > RAND_MAX / 2 ? 1 : -1);
		pointDataPtr.push_back(new Point2D(newX, newY));
		pointData.push_back(*(pointDataPtr[pointDataPtr.size() - 1]));
	}

	double dotProd;

	//2. Compare least squares approach to Ransac

	std::cout << "总的点数(包含噪点): " << pointData.size() << std::endl;
	std::cout << "噪点数: " << numOutliers << std::endl;
	//
	std::cout << "真实的线的参数 [nx,ny,ax,ay]\n\t [ " << -dy << ", " << dx << ", 0, 0 ]" << std::endl;

	//最小二乘法估计的线参数
	lpEstimator.leastSquaresEstimate(pointDataPtr, lineParameters);
	std::cout << "最小二乘法估计的线参数: [n_x,n_y,a_x,a_y]\n\t [ " << lineParameters[0] << ", " << lineParameters[1] << ", ";
	std::cout << lineParameters[2] << ", " << lineParameters[3] << " ]" << std::endl;
	
	dotProd = lineParameters[0] * (-dy) + lineParameters[1] * dx;
	std::cout << "\t 实线和计算线法线的点积 [+-1=correct]: " << dotProd << std::endl;
	dotProd = (-dy)*(lineParameters[2]) + dx*lineParameters[3];
	std::cout << "\t 检查计算点是否在实线上 [0=correct]: " << dotProd << std::endl;

	//A RANSAC estimate of the line parameters

	double usedData = Ransac<Point2D, double>::compute(lineParameters,&lpEstimator,pointData,numForEstimate);


	std::cout << "RANSAC 线参数 [n_x,n_y,a_x,a_y]\n\t [ " << lineParameters[0] << ", " << lineParameters[1] << ", ";
	std::cout << lineParameters[2] << ", " << lineParameters[3] << " ]" << std::endl;
	dotProd = lineParameters[0] * (-dy) + lineParameters[1] * dx;
	std::cout << "\t  实线和计算线法线的点积[+-1=correct]: " << dotProd << std::endl;
	dotProd = (-dy)*(lineParameters[2]) + dx*lineParameters[3];
	std::cout << "\t检查计算点是否在实线上  [0=correct]: " << dotProd << std::endl;
	std::cout << "\t 用于最终估算的点数百分比: " << usedData << std::endl;
	system("pause");
	return 0;
}
上一篇:httprunner 3.x学习10 - parameters 参数化


下一篇:【Laravel3.0.0源码阅读分析】url类url.php