RANSAC之c语言实现

关于RANSAC算法已经写过两篇博文，本篇主要是基于前两篇的相关理论实现的，同时这里实现的RANSAC实现了定点化。前两篇博文请参考：matlab实现ransac，opencv和c++实现ransac。若有不当之处，望指教，谢谢！

 

typedef struct
{
	int x;
	int y;
}point_t;

typedef struct LineModel
{
	int mSlope;
	int mIntercept;
	int flag;
}LineModel;

 

 

#ifndef _RANSAC_H_
#define _RANSAC_H_

#include "least_square.h"
#include "stack_pt.h"

void ransac_computeModel(point_t* points, point_t *consensusSet, point_t *maybeInliers, LineModel *mBestModel, const int n, const int mIterations, float mThreshold);

void interpolate_point(point_t *p, point_t p1, point_t p2, int step_size, int *count);

#endif;

#include "ransac.h"

int s = 0;

//获取点集中两个随机点
void getMaybeInliers(point_t* points,point_t *maybeInliers,const int n,int* count) 
{
	int randPointIndex_0 = (int)(n * rand() / RAND_MAX - 1.0);
	int randPointindex_1 = (int)(n * rand() / RAND_MAX - 1.0);

	if (randPointIndex_0 >= 0 && randPointIndex_0 < n && randPointindex_1 >= 0 && randPointindex_1 < n)
	{
		maybeInliers[0] = points[randPointIndex_0];
		maybeInliers[1] = points[randPointindex_1];
		*count = 2;
	}
}

//检测两个点之间的距离是否小于1
int isdegenerate(point_t p1, point_t p2)
{
	int norm = sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
	if (norm < 1.0)
		return TRUE;
	return FALSE;
}

//返回直线信息：斜率和截距
LineModel getModel(const point_t p1, const point_t p2)
{
	LineModel model;/* = xmalloc(sizeof(LineModel));*/
	if (p2.x != p1.x)
	{
		model.mSlope = (((p2.y - p1.y) << 10))/ (p2.x - p1.x);
		model.mIntercept = (p1.y <<10) - model.mSlope * p1.x;
	}
	else
	{
		model.mSlope = (1<<31) - 1;
		model.mIntercept =  - ((1 << 31) - 1);
	}

	return model;
}

//检测点是否相等
int isMember(point_t p, point_t* pList)
{
	int isMember = 0;

	for (int i = 0; i < 2; i++)
	{
		if (point_equal(p, pList[i]))
		{
			isMember = 1;
		}
	}

	return isMember;
}

int64_t getDistance(point_t p, LineModel model)
{
	int64_t distance = 10 << 10;

	if (fabs(model.mSlope) < 1000.)
	{
		int64_t k = (int64_t)((model.mSlope));
		int64_t b = (int64_t)((model.mIntercept));
		distance = (k * p.x - (p.y << 10) + b)*(k * p.x - (p.y << 10) + b) / (((k*k) >> 10) + (1 << 10));
	}

	return distance;
}

//检测点是否符合模型条件
int fitsModel(point_t p,LineModel model,int mThreshold)
{
	int fits = 0;

	//点到直线的距离
	int64_t distance = getDistance(p, model);

	if (distance < (mThreshold << 10))
		fits = 1;

	return fits;
}

void ransac_push(point_t* pt, point_t p0)
{
	pt[s++] = p0;
}

void ransac_pop(point_t* pt)
{
	pt[--s];
}

int getModelRank(point_t* points, LineModel model,const int n,float mThreshold)
{
	int count = 0;
	for (unsigned int i = 0; i < n; i++)
	{
		if (fitsModel(points[i], model, mThreshold))
			count++;
	}

	return count;
}

void ransac_computeModel(point_t* points, point_t *consensusSet, point_t *maybeInliers, LineModel *mBestModel, const int n, const int mIterations, float mThreshold)
{
	int foundModel = 0;
	int iterations = 0;
	int count = 0;
	int mRequiredInliers = 2;
	s = 0;
	int mBestRank = 0;
	mBestModel->flag = FALSE;

	while (iterations < mIterations)
	{
		s = 0;
		count = 0;
		getMaybeInliers(points, maybeInliers,n, &count);
		if (count == 2)
		{
			//两点之间的距离小于1,则跳过
			if (isdegenerate(maybeInliers[0], maybeInliers[1]))
			{
				iterations++;
				continue;
			}

			ransac_push(consensusSet,maybeInliers[0]);
			ransac_push(consensusSet, maybeInliers[1]);
			LineModel model = getModel(maybeInliers[0], maybeInliers[1]);

			for (int i = 0; i < n; i++)
			{
				//排除和随机取到的点相同的点
				if (!isMember(points[i], maybeInliers))
				{
					if (fitsModel(points[i],model,mThreshold))
						ransac_push(consensusSet, points[i]);
				}
			}

			if (s >= mRequiredInliers)
			{
				model = least_square(consensusSet,s);
				int rank = getModelRank(consensusSet, model, s, mThreshold);

				if (rank > mBestRank && model.mIntercept != -1024)
				{
					mBestModel->flag = TRUE;
					mBestModel -> mSlope = model.mSlope;
					mBestModel -> mIntercept = model.mIntercept;
					mBestRank = rank;
				}
			}

			iterations++;
		}
	}
}


void interpolate_point(point_t *p, point_t p1, point_t p2, int step_size,int *count)
{
	int steps = abs(p2.x - p1.x) / step_size + 1;
	p[*count] = p1;
	(*count)++;

	float start_x, end_x, start_y, end_y;

	if (p1.x < p2.x)
	{
		start_x = p1.x;
		start_y = p1.y;
		end_x = p2.x;
		end_y = p2.y;
	}
	else
	{
		start_x = p2.x;
		start_y = p2.y;
		end_x = p1.x;
		end_y = p1.y;
	}

	if (steps < 1)
		return;

	for (int i = 1; i <= steps; i++)
	{
		int new_x = start_x + i * step_size;
		int new_y = (end_y - start_y) / (end_x - start_x) * (new_x - start_x) + start_y;

		p[*count] = point_res(new_x,new_y);
		(*count)++;
	}

}