ransac

#include<iostream>
#include<opencv2/opencv.hpp>
#include<vector>
#include<stdlib.h>
#include <time.h>


using namespace std;
using namespace cv;

void BasicLineParam(vector<Point> & data, vector<double> & param)
{
	Point p1 = (0, 0), p2 = (0, 0);
	//srand((unsigned)time(NULL));			  //不能这样选取随机数，因为要迭代，速度太快了，时间基本没有改变，每次选出来的随机数是一样的。
	while (p1 == p2)                         //保证选取的是两个不同的点
	{
		p1 = data[rand() % data.size()];	
		p2 = data[rand() % data.size()];		
	}
	cout <<"选取的两个点" <<endl << p1<< endl;
	cout << p2 << endl;

	double nx = p1.y - p2.y;
	double ny = p2.x - p1.x;// 原始直线的斜率为K，则法线的斜率为-1/k 
	double norm = sqrt(nx*nx + ny*ny);
	param.push_back(nx / norm);
	param.push_back(ny / norm);
	param.push_back(p1.x);
	param.push_back(p1.y);
	param.push_back(p2.x);
	param.push_back(p2.y);
	cout << "直线的法线向量 （" << param[0] << ", " << param[1] << endl;
}
int  ComputeInnerPoint(Point basic_point, double nx, double ny, vector<Point> & data, /*int maxInner,*/ double delta, vector<int> & l);

int main()
{
	//生成一个矩形(  假设矩形的一条边的方程是y = 0.1x + 100;  y = -10x + 4140; y = -10x + 10200)
	Mat image(1600, 1600, CV_8UC3, Scalar(0, 0, 0));
	Point p1(400, 140);                //算出来的4个定点
	Point p2(1000, 200);
	Point p3(950, 700);
	Point p4(350, 640);

	circle(image, p1, 20, Scalar(0, 255, 0));
	//circle(image, p2, 2, Scalar(0, 255, 0));
	//circle(image, p3, 2, Scalar(0, 255, 0));
	//circle(image, p4, 2, Scalar(0, 255, 0));
	//line(image, p1, p2, Scalar(0, 0, 255));
	//line(image, p3, p2, Scalar(0, 0, 255));
	//line(image, p4, p3, Scalar(0, 0, 255));
	//line(image, p1, p4, Scalar(0, 0, 255));

	//生成测试数据
	vector<Point> l1, l2, l3, l4;
	for (int i = 0; i < 20; i++)
	{
		Point temp;
		//增加直线l1上的噪声
		temp.x = rand() % (p2.x - p1.x) + p1.x;
		temp.y = 0.1 * temp.x + 100;
		temp.y += -5 + rand() % 11;
		l1.push_back(temp);
		circle(image, temp, 2, Scalar(255, 0, 0));
		


		temp.x = rand() % 1600;                             //增加l1随机噪声
		temp.y = rand() % 1600;
		l1.push_back(temp);
		circle(image, temp, 20, Scalar(0, 100, 100));


		增加直线l2上的噪声
		//temp.x = rand() % (p2.x - p3.x) + p3.x;
		//temp.y = -10 * temp.x + 10200;
		//temp.x += -5 + rand() % 11;
		//l2.push_back(temp);
		//circle(image, temp, 2, Scalar(255, 0, 0));

		//temp.x = rand() % 1600;                             //增加l2随机噪声
		//temp.y = rand() % 1600;
		//l2.push_back(temp);
		//circle(image, temp, 2, Scalar(255, 0, 0));

		增加直线l3上的噪声
		//temp.x = rand() % (p3.x - p4.x) + p4.x;
		//temp.y = 0.1 * temp.x + 605;
		//temp.y += -5 + rand() % 11;
		//l3.push_back(temp);
		//circle(image, temp, 2, Scalar(255, 0, 0));

		//temp.x = rand() % 1600;                             //增加l3随机噪声
		//temp.y = rand() % 1600;
		//l3.push_back(temp);
		//circle(image, temp, 2, Scalar(255, 0, 0));

		增加直线l4上的噪声
		//temp.x = rand() % (p1.x - p4.x) + p4.x;
		//temp.y = -10 * temp.x + 4140;
		//temp.x += -5 + rand() % 11;
		//l4.push_back(temp);
		//circle(image, temp, 2, Scalar(255, 0, 0));

		//temp.x = rand() % 1600;                             //增加l4随机噪声
		//temp.y = rand() % 1600;
		//l4.push_back(temp);
		//circle(image, temp, 2, Scalar(255, 0, 0));
	}
	
	//l1的斜率
	int iter = 0;
	vector<int> l(40);
	vector<int> inner(40);
	int maxInner = 2;
	while (iter < 50)
	{
		iter++;
		
		vector<double> param;
		BasicLineParam(l1, param);	
		Point basicPoint = Point(param[2], param[3]);             //将随机选出的一个点作为基础点
		int curInner = ComputeInnerPoint(basicPoint, param[0], param[1], l1, 5, l);				//以基础点和法向量，来判定直线的内点
		if (maxInner < curInner)
		{
			
			maxInner = curInner;
			
			for (int i = 0; i < 40; i++)
				inner[i] = l[i];
		}
	}
	cout << "max!!!!!!!!!!!!!!!!!!!  " << maxInner << endl;

	for (int i = 0; i < 40; i++)
	{
		if (inner[i] == 1)
			circle(image, l1[i], 2, Scalar(255, 255, 255));
	}
		namedWindow("sdf", WINDOW_NORMAL);
		imshow("sdf", image);
		waitKey();
	

}



//计算内点,basic_point是直线上一点， nx,ny是上面算出来的法线向量，data是所有的点集，maxinner用来算出最好模型的内点数，
//delta是确定为内点的最短的点到直线的距离
int ComputeInnerPoint(Point basic_point, double nx, double ny, vector<Point> & data,/* int maxInner, */double delta, vector<int> & l)
{
	int curInn = 0;   //内点计数	
	for (int i = 0; i < data.size(); i++)
	{		
		double tempX = data[i].x - basic_point.x;
		double tempY = data[i].y - basic_point.y;
		double length = fabs(tempX * nx + tempY * ny);
		if (length <= delta)
		{
			l[i] = 1;
			curInn++;
		}
		else
			l[i] = 0;
	}
	cout << "cur:  " << curInn << endl;
	
	return curInn;
}