点云拟合

mscdj 2019-12-04

展开全文

最近项目需要这方面的工作，于是开始研究这个了；

圆柱几何特征：圆柱面上的点到其轴线的距离恒等于半径

圆柱的方程：

首先是 PCL库自带的圆柱模型拟合，由于在查找最佳圆柱面的过程中会过滤很多点，因此考虑利用最小二乘的模型来拟合最接近实际点云的一个圆柱面，code如下，只是简单的调库，原理没仔细看：

#include "pch.h"
#include <iostream>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include<pcl/point_cloud.h>
#include<pcl/segmentation/sac_segmentation.h>
#include<pcl/search/search.h>
#include<pcl/search/kdtree.h>
#include<pcl/features/normal_3d.h>
#include<pcl/common/common.h>
// use ransanc to fit cylinder
int fitCylinder(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out) {
	// Create segmentation object for cylinder segmentation and set all the parameters
	pcl::ModelCoefficients::Ptr coeffients_cylinder(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);
	//  Normals
	pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ>>(new pcl::search::KdTree<pcl::PointXYZ>);
	pcl::PointCloud<pcl::Normal>::Ptr normalsFilter(new pcl::PointCloud<pcl::Normal>);
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimator;
	normalEstimator.setSearchMethod(tree);
	normalEstimator.setInputCloud(cloud_in);
	normalEstimator.setKSearch(30);
	normalEstimator.compute(*normalsFilter);
	pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
	seg.setOptimizeCoefficients(true);
	seg.setModelType(pcl::SACMODEL_CYLINDER);
	seg.setMethodType(pcl::SAC_RANSAC);
	//seg.setNormalDistanceWeight(0.1);
	seg.setNormalDistanceWeight(0.2);		// for coarse data
	seg.setMaxIterations(1000);
	seg.setDistanceThreshold(0.3);
	seg.setRadiusLimits(0, 6);				// radius is within 8 centimeters
	seg.setInputCloud(cloud_in);
	seg.setInputNormals(normalsFilter);
	// Perform segment
	seg.segment(*inliers_cylinder, *coeffients_cylinder);
	std::cerr << "Cylinder coefficient: " << *coeffients_cylinder << std::endl;
	// Ouput extracted cylinder
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(cloud_in);
	extract.setIndices(inliers_cylinder);
	extract.filter(*cloud_out);
	pcl::PCDWriter wr;
	wr.write("Extract_Cylinder.pcd", *cloud_out);
	float sum_D = 0.0;
	float sum_Ave = 0.0;
	float x0 = coeffients_cylinder->values[0];
	float y0 = coeffients_cylinder->values[1];
	float z0 = coeffients_cylinder->values[2];
	float l = coeffients_cylinder->values[3];
	float m= coeffients_cylinder->values[4];
	float n = coeffients_cylinder->values[5];
	float r0 = coeffients_cylinder->values[6];
	for (int i = 0; i < cloud_out->points.size(); i++) {
		float x = cloud_out->points[i].x;
		float y = cloud_out->points[i].y;
		float z = cloud_out->points[i].z;
		// D=part1+part2
		float part1 = pow(x - x0, 2) + pow(y - y0, 2) + pow(z - z0, 2) - pow(r0, 2);
		float part2 = -pow(l*(x - x0) + m * (y - y0) + n * (z - z0), 2) / (l*l + m * m + n * n);
		sum_D +=pow( part1 + part2,2);
		sum_Ave += fabs(part1 + part2);
	}
	std::cerr << "The average difference is " << sum_Ave / cloud_out->points.size() << std::endl;;
	std::cerr << "The Difference of average difference is " << sum_D / cloud_out->points.size() << std::endl;;
	// evaluate the cylinder quation
}

尝试一下发现，圆柱方程无法简单地采用线性最小二乘的模型，考虑采用非线性最小二乘的解法，待续...

2019年4月继续更

最近用最小二乘把点云平面拟合，球拟合，圆柱拟合一锅端了，因为太懒一直没更233，慢慢全部补上公式推导和code。其中圆柱面拟合用到的非线性迭代的方法。

首先，简化圆柱面方程：

令：

算法流程如下：

代码：

#include <Eigen/Dense>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
typedef struct _cylinder_coefficient_t {     //圆柱参数
	float x;
	float y;
	float z;
	float l;
	float m;
	float n;
	float r;
}cylinder_coefficient_t;
int Leastsquare::cylinder_fit(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, cylinder_coefficient_t *initial_params, cylinder_coefficient_t *final_params)
{
	using namespace Eigen;
	// initial params  x y z, l m n, r
	MatrixXd params(7, 1);
	params << 0, 0, 0, 0, 0, 0, 0;
	MatrixXd X(7, 1);
	X << initial_params->x, initial_params->y, initial_params->z
		+ 1, initial_params->l, initial_params->m, initial_params->n, initial_params->r;
	//X << 89, -43, 19.6,0.01, 0.01, -0.99, 4; 
	std::cout << "\n initial cylinder params=" << params + X << std::endl;
	int times = 0;
	while ((fabs(X(3, 0)) + fabs(X(4, 0)) + fabs(X(5, 0))) > 0.001 || fabs(X(6, 0)) > 0.01|| times==0) {
		// 改正
		params += X;
		std::cout << "\nparams=" << params << std::endl;
		float x0 = params(0, 0);	float y0 = params(1, 0);	float z0 = params(2, 0);
		float a0 = params(3, 0);	float b0 = params(4, 0);	float c0 = params(5, 0);
		float r0 = params(6, 0);
		float f0;
		int size = cloud_in->points.size();		//point size, M size
		MatrixXd m(size, 7), mT(7, size), B(size, 1);
		for (int i = 0; i < size; i++) {
			float x = cloud_in->points[i].x;
			float y = cloud_in->points[i].y;
			float z = cloud_in->points[i].z;
			f0 = pow(x - x0, 2) + pow(y - y0, 2) + pow(z - z0, 2) - pow((a0*(x - x0) + b0 * (y - y0) + c0 * (z - z0)), 2) - r0 * r0;
			B(i, 0) = -f0;
			m(i, 0) = (a0 * (a0*(x - x0) + b0 * (y - y0) + c0 * (z - z0)) - (x - x0)) * 2;
			m(i, 1) = (b0 * (a0*(x - x0) + b0 * (y - y0) + c0 * (z - z0)) - (y - y0)) * 2;
			m(i, 2) = (c0 * (a0*(x - x0) + b0 * (y - y0) + c0 * (z - z0)) - (z - z0)) * 2;
			m(i, 3) = -(a0*(x - x0) + b0 * (y - y0) + c0 * (z - z0))*(x - x0) * 2;
			m(i, 4) = -(a0*(x - x0) + b0 * (y - y0) + c0 * (z - z0))*(y - y0) * 2;
			m(i, 5) = -(a0*(x - x0) + b0 * (y - y0) + c0 * (z - z0))*(z - z0) * 2;
			m(i, 6) = -2*r0;
		}
		X = m.colPivHouseholderQr().solve(B);			//改正数
		std::cout << "\nX=" << X << std::endl;
		////  V=Ax-B,  x=(ATA)-1AT*B
		//mT = m.transpose();
		//MatrixXd mTm(7, 7), mTm_inv(7, 7);
		//mTm = mT * m;
		//mTm_inv = mTm.inverse();
		//X = mTm_inv * mT*B;
		//std::cout << "\nX=" << X << std::endl;
		times++;
	}
	std::cout << "After   " << times << "   iterations," << "\n final cylinder params=" << params << std::endl;
	float sum_D = 0.0;
	float sum_Ave = 0.0;
	float x0 = params(0, 0);
	float y0 = params(1, 0);
	float z0 = params(2, 0);
	float l = params(3, 0);
	float m = params(4, 0);
	float n = params(5, 0);
	float r0 = fabs(params(6, 0));		//r must be positive, int fitting process, it can be negtive
	for (int i = 0; i < cloud_in->points.size(); i++) {
		float x = cloud_in->points[i].x;
		float y = cloud_in->points[i].y;
		float z = cloud_in->points[i].z;
		// D=part1+part2
		float part1 = pow(x - x0, 2) + pow(y - y0, 2) + pow(z - z0, 2) - pow(r0, 2);
		float part2 = -pow(l*(x - x0) + m * (y - y0) + n * (z - z0), 2) / (l*l + m * m + n * n);
		sum_D += pow(part1 + part2, 2);
		sum_Ave += fabs(part1 + part2);
	}
	std::cerr << "The average difference is " << sum_Ave / cloud_in->points.size() << std::endl;;
	std::cerr << "The Difference of average difference is " << sum_D / cloud_in->points.size() << std::endl;;
	// evaluate the cylinder quation
	final_params->x = params(0, 0);
	final_params->y = params(1, 0);
	final_params->z = params(2, 0);
	final_params->l = params(3, 0);
	final_params->m = params(4, 0);
	final_params->n = params(5, 0);
	final_params->r = params(6, 0);
	return 0;
}

结果：

有几点需要注意的：圆柱拟合出来的r有时候为正，有时候为负，但数值是对的，所以我直接在迭代结束的时候取了绝对值。可能对于方程式来说，这两个值没有区别，感觉是在求这个7维方程中，有两个极值点，r的值是互为正反的。代码中未考虑可能陷入局部最优的情况。

2019年5月，继续更，评论中有朋友提到未能利用到 abc的平方和为1这一限制条件，感谢提醒，经过思考大概有一些思路，再引入一个未知数m, 把m(a*a+b*b+c*c-1)加入f中（或者简单点直接令m=1等等），暂时还没有用代码测试，感兴趣的朋友可以试试。