RANSAC应用之拟合3D平面 by MRPT

拟合3D平面

The source for this example can be found in: https://github.com/MRPT/mrpt/tree/master/samples/math_ransac_plane3d_example

This is the resulting fit plane. Average execution time for 300 inliers and 100 outliers is 0.5 milliseconds on a Pentium M @ 2.0Ghz.

代码

使用上一篇的ransac函数来拟合3D平面

1. 主函数

//首先生成真实样本噪声样本
//然后ransac，得到结果
for (size_t iters=0;iters<TIMES;iters++)
		math::RANSAC::execute(
			data,
			ransac3Dplane_fit,
			ransac3Dplane_distance,
			ransac3Dplane_degenerate,
			DIST_THRESHOLD,
			3,  // Minimum set of points
			best_inliers,
			best_model,
			iters==0   // Verbose
			);

2. 设计拟合方程

通过选取的3个点，得到模型参数

void  ransac3Dplane_fit(
	const CMatrixDouble  &allData,
	const vector_size_t  &useIndices,
	vector< CMatrixDouble > &fitModels )
{
	ASSERT_(useIndices.size()==3);

	TPoint3D  p1( allData(0,useIndices[0]),allData(1,useIndices[0]),allData(2,useIndices[0]) );
	TPoint3D  p2( allData(0,useIndices[1]),allData(1,useIndices[1]),allData(2,useIndices[1]) );
	TPoint3D  p3( allData(0,useIndices[2]),allData(1,useIndices[2]),allData(2,useIndices[2]) );

	try
	{
		TPlane  plane( p1,p2,p3 );
		fitModels.resize(1);
		CMatrixDouble &M = fitModels[0];

		M.setSize(1,4);
		for (size_t i=0;i<4;i++)
			M(0,i)=plane.coefs[i];
	}
	catch(exception &)
	{
		fitModels.clear();
		return;
	}
}

其中，Plane是一个类

//声明
// Plane coefficients, stored as an array: A，B，C，D
double coefs[4];

TPlane(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3) throw(std::logic_error);

//定义
TPlane::TPlane(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3)  throw(std::logic_error) {
	double dx1=p2.x-p1.x;
	double dy1=p2.y-p1.y;
	double dz1=p2.z-p1.z;
	double dx2=p3.x-p1.x;
	double dy2=p3.y-p1.y;
	double dz2=p3.z-p1.z;
	coefs[0]=dy1*dz2-dy2*dz1;
	coefs[1]=dz1*dx2-dz2*dx1;
	coefs[2]=dx1*dy2-dx2*dy1;
	if (abs(coefs[0])<geometryEpsilon&&abs(coefs[1])<geometryEpsilon&&abs(coefs[2])<geometryEpsilon) throw logic_error("Points are linearly dependent");
	coefs[3]=-coefs[0]*p1.x-coefs[1]*p1.y-coefs[2]*p1.z;
}

3. 设计距离方程

根据模型计算每一个数据点到模型的距离

void ransac3Dplane_distance(
	const CMatrixDouble &allData,
	const vector< CMatrixDouble > & testModels,
	const double distanceThreshold,
	unsigned int & out_bestModelIndex,
	vector_size_t & out_inlierIndices )
{
	ASSERT_( testModels.size()==1 )
	out_bestModelIndex = 0;
	const CMatrixDouble &M = testModels[0];

	ASSERT_( size(M,1)==1 && size(M,2)==4 )

	TPlane  plane;
	plane.coefs[0] = M(0,0);
	plane.coefs[1] = M(0,1);
	plane.coefs[2] = M(0,2);
	plane.coefs[3] = M(0,3);

	const size_t N = size(allData,2);
	out_inlierIndices.clear();
	out_inlierIndices.reserve(100);
	for (size_t i=0;i<N;i++)
	{
		const double d = plane.distance( TPoint3D( allData.get_unsafe(0,i),allData.get_unsafe(1,i),allData.get_unsafe(2,i) ) );
		if (d<distanceThreshold)
			out_inlierIndices.push_back(i);
	}
}

其中，距离函数为：

double TPlane::distance(const TPoint3D &point) const	{
	return abs(evaluatePoint(point))/sqrt(squareNorm<3,double>(coefs));
}
double TPlane::evaluatePoint(const TPoint3D &point) const	{
	return dotProduct<3,double>(coefs,point)+coefs[3];
}

4. 设计退化方程

这一块没具体设计

/** Return "true" if the selected points are a degenerate (invalid) case.
  */
bool ransac3Dplane_degenerate(
	const CMatrixDouble &allData,
	const mrpt::vector_size_t &useIndices )
{
	return false;
}