PCL cylinder fitting and cylinder center point acquisition

Problem Description

PCL cylinder fitting can give a point on the axis, a vector along the axis, and a radius. But there is no way to know the length of the cylinder, as well as the starting point and the end point. In this way, the axis center of the cylinder cannot be determined.

PCL Cylindrical Fitting

PCL provides the SACMODEL_CYLINDER model , which is defined as a cylinder model, with a total of 7 parameters set. The interior points extracted from the point cloud segmentation are all located on the cylinder corresponding to the estimated parameters or within a certain distance from the cylinder. Model coefficients (point_on_axis.x, point_on_axis.y, point_on_axis.z, axis_direction.x, axis_direction.y, axis_direction.z, radius). The first three coefficients represent a point on the axis of the cylinder , and the last three coefficients represent the radius along the axis . Vector , the last coefficient represents the radius .
Cylindrical fitting key code snippet

	// Create the segmentation object for cylinder segmentation and set all the parameters
	seg.setOptimizeCoefficients(true);
	//设置分割模型类别
	seg.setModelType(pcl::SACMODEL_CYLINDER);
	//设置使用那个随机参数估计方法为随机样本共识
	seg.setMethodType(pcl::SAC_RANSAC);
	//设置表面法线权重系数
	seg.setNormalDistanceWeight(0.1);
	//设置最大迭代数
	seg.setMaxIterations(10000);
	//设置是否为模型内点的距离阈值
	seg.setDistanceThreshold(0.005);
	//设置估计出的圆柱模型的半径的范围
	seg.setRadiusLimits(0.01, 0.1);
	seg.setInputCloud(cloud_filtered);
	seg.setInputNormals(cloud_normals);
	//最终获取内点以及模型系数
	seg.segment(*inliers_cylinder, *coefficients_cylinder);

PCL可视化圆柱体
viewer_ptr->addCylinder(*coefficients_cylinder);

It can be seen from the verification that the cylinder model cannot obtain the start and end points of the cylinder axis.

Get the start and end points of the axis of the cylinder

Method ideas:

1. Analyze the cylindrical model through PCA principal component analysis to obtain the maximum and minimum axial values ​​of the cylinder.

2. Back-project the two largest and smallest points obtained after PCA analysis to the original point cloud.

3. Map the obtained maximum and minimum points in the original point cloud to the axis, so that the starting point and the ending point of the cylinder are obtained

The final result, the red line on the cylinder axis represents the result of the projection, and the black arrow straight line represents the line drawn according to the first three values ​​of the model coefficient as the starting point, which verifies that the first three coefficients of the cylindrical model coefficient are not the starting point or the end point.
Calculated From the starting point to the end point, the pivot point can be calculated.

The projection point is mapped to the straight line by solving the vector. The cylindrical model coefficients are known and the vector is known. The straight line of the axis can be expressed, and then the projection point can be obtained. For details, please refer to the key code of the calculation of the projection point

	//将点云上的极值点投影到轴心上
	Eigen::Vector3f ori_projstart_vec(reconstruct_points->points[0].x - pStart.x, reconstruct_points->points[0].y - pStart.y, reconstruct_points->points[0].z - pStart.z);
	//求参数t=（d(xp-a)+e(yp-b)+f(zp-c)）/( dd+ee+ff )
	double start_t = axis_direct_vec[0] * ori_projstart_vec[0] + axis_direct_vec[1] * ori_projstart_vec[1] + axis_direct_vec[1] * ori_projstart_vec[1] / axis_direct_vec.norm();
	//xm = dt + a ym = et + b zm = ft + c
	Eigen::Vector3f start_proj_point(axis_direct_vec[0] * start_t + axis_start_point[0], axis_direct_vec[1] * start_t + axis_start_point[1], axis_direct_vec[2] * start_t + axis_start_point[2]);
	

full code

#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <time.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <pcl/common/pca.h>
#include <pcl/visualization/pcl_visualizer.h>

typedef pcl::PointXYZ PointT;

boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis(pcl::PointCloud<PointT>::ConstPtr cloud)
{
    
    
	// --------------------------------------------
	// -----Open 3D viewer and add point cloud-----
	// --------------------------------------------
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addPointCloud<PointT>(cloud, "sample cloud");
	//viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "sample cloud");
	viewer->addCoordinateSystem(0.1);
	viewer->initCameraParameters();
	return (viewer);
}

int
main ()
{
    
    
	// All the objects needed
	pcl::PCDReader reader;
	pcl::PassThrough<PointT> pass;
	pcl::NormalEstimation<PointT, pcl::Normal> ne;
	pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; 
	pcl::PCDWriter writer;
	pcl::ExtractIndices<PointT> extract;
	pcl::ExtractIndices<pcl::Normal> extract_normals;
	pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());

	// Datasets
	pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
	pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
	pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);
	pcl::PointCloud<PointT>::Ptr cloud_cylinder(new pcl::PointCloud<PointT>);

	// Read in the cloud data
	reader.read ("cylinder_002.pcd", *cloud);
	std::cerr << "PointCloud has: " << cloud->size () << " data points." << std::endl;

	//创建滤波器
	pcl::VoxelGrid<PointT> sor;
	sor.setInputCloud(cloud);
	sor.setLeafSize(0.001f, 0.001f, 0.001f);
	sor.filter(*cloud_filtered);
	std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << "data points (" << pcl::getFieldsList(*cloud_filtered) << ").";

	// Estimate point normals
	ne.setSearchMethod (tree);
	ne.setInputCloud (cloud_filtered);
	ne.setKSearch (50);
	ne.compute (*cloud_normals);

	seg.setOptimizeCoefficients(true);
	//设置分割模型类别
	seg.setModelType(pcl::SACMODEL_CYLINDER);
	//设置使用那个随机参数估计方法为随机样本共识
	seg.setMethodType(pcl::SAC_RANSAC);
	//设置表面法线权重系数
	seg.setNormalDistanceWeight(0.1);
	//设置最大迭代数
	seg.setMaxIterations(10000);
	//设置是否为模型内点的距离阈值
	seg.setDistanceThreshold(0.005);
	//设置估计出的圆柱模型的半径的范围
	seg.setRadiusLimits(0.01, 0.1);
	seg.setInputCloud(cloud_filtered);
	seg.setInputNormals(cloud_normals);
	//最终获取内点以及模型系数
	seg.segment(*inliers_cylinder, *coefficients_cylinder);

	std::cout << "coefficients_cylinder" << *coefficients_cylinder << std::endl;

	//Extract the cylinder inliers from the input cloud
	extract.setInputCloud(cloud_filtered);
	extract.setIndices(inliers_cylinder);
	extract.setNegative(false);
	extract.filter(*cloud_cylinder);
	std::cout << "PointCloud representing the planar component: " << cloud_cylinder-> size() << "data points." << std::endl;
	writer.write("cylinder_piece.pcd", *cloud_cylinder, false);

	//显示, 实际点云及拟合点云区别
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_ptr;
	viewer_ptr = simpleVis(cloud_cylinder);
	viewer_ptr->addCylinder(*coefficients_cylinder);
	while (!viewer_ptr->wasStopped())
	{
    
    
		viewer_ptr->spinOnce(100);
	}

	//-----------------------------------------------------------------------------------------
	//通过PCA分析圆柱表面信息将圆柱极值点投影到圆柱体轴心上
	Eigen::Vector3d axis_start_point(coefficients_cylinder->values[0], coefficients_cylinder->values[1], coefficients_cylinder->values[2]);
	Eigen::Vector3f axis_direct_vec(coefficients_cylinder->values[3], coefficients_cylinder->values[4], coefficients_cylinder->values[5]);
	pcl::PCA<PointT> pca;
	pca.setInputCloud(cloud_cylinder);

	//修改第一成分方向向量(提高分量准确度，及分量沿着圆柱方向)
	Eigen::Vector3f direct_vec(coefficients_cylinder->values[3], coefficients_cylinder->values[4], coefficients_cylinder->values[5]);
	pca.getEigenVectors().block<3, 1>(0, 0) = direct_vec;

	//获取圆柱点云在各个成分向量上的分量
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPCAprojection(new pcl::PointCloud<pcl::PointXYZ>);
	pca.project(*cloud_cylinder, *cloudPCAprojection);
	//或得质心
	Eigen::Vector4f pcaCentroid = pca.getMean();
	std::cout << "特征值va(3x1):\n" << pca.getEigenValues() << std::endl;
	std::cout << "特征向量ve(3x3):\n" << pca.getEigenVectors() << std::endl;
	std::cout << "质心点(4x1):\n" << pcaCentroid << std::endl;

	Eigen::Matrix4f tm = Eigen::Matrix4f::Identity();
	Eigen::Matrix4f tm_inv = Eigen::Matrix4f::Identity();
	//矩阵的转置
	tm.block<3, 3>(0, 0) = pca.getEigenVectors().transpose(); //R
	tm.block<3, 1>(0, 3) = -1.0f * (pca.getEigenVectors().transpose()) * (pcaCentroid.head<3>());

	tm_inv = tm.inverse();

	std::cout << "变换矩阵tm(4x4):\n" << tm << std::endl;
	std::cout << "逆变矩阵tm'(4x4):\n" << tm_inv << std::endl;

	pcl::PointCloud<PointT>::Ptr transformedCloud(new pcl::PointCloud<PointT>);
	pcl::transformPointCloud(*cloud, *transformedCloud, tm);

	PointT min_p1, max_p1;
	Eigen::Vector3f c1, c;
	pcl::getMinMax3D(*transformedCloud, min_p1, max_p1);

	PointT start, end; 
	pcl::getMinMax3D(*transformedCloud, start, end);
	//保留第一主成分
	start.y = start.z = 0; 
	end.y = end.z = 0;

	//重投影点在点云表面
	pcl::PointCloud<PointT>::Ptr reconstruct_points(new pcl::PointCloud<PointT>);
	reconstruct_points->points.push_back(start);
	reconstruct_points->points.push_back(end);
	pca.reconstruct(*reconstruct_points, *reconstruct_points);
	std::cout << "start" << reconstruct_points->points[0] << ",end" << reconstruct_points->points[1] << std::endl;
	Eigen::Vector3d recons_start_point(reconstruct_points->points[0].x, reconstruct_points->points[0].y, reconstruct_points->points[0].z);
	Eigen::Vector3d recons_end_point(reconstruct_points->points[1].x, reconstruct_points->points[1].y, reconstruct_points->points[1].z);
	Eigen::Vector3d recons_end_start_vec(reconstruct_points->points[1].x- reconstruct_points->points[0].x, reconstruct_points->points[1].y- reconstruct_points->points[0].y, reconstruct_points->points[1].z- reconstruct_points->points[0].z);
	Eigen::Vector3d axis_end_point(axis_start_point[0] + recons_end_start_vec[0], axis_start_point[1] + recons_end_start_vec[1], axis_start_point[2] + recons_end_start_vec[2]);

	
	std::cout << "min_p1" << min_p1 << ",max_p2" << max_p1 << std::endl;
	c1 = 0.5f * (min_p1.getVector3fMap() + max_p1.getVector3fMap());
	std::cout << "型心c1(3x1):\n" << c1 << std::endl;

	//反射变换矩阵
	Eigen::Affine3f tm_inv_aff(tm_inv);
	pcl::transformPoint(c1, c, tm_inv_aff);

	Eigen::Vector3f whd, whd1;
	whd1 = max_p1.getVector3fMap() - min_p1.getVector3fMap();
	whd = whd1;
	float sc1 = (whd1(0) + whd1(1) + whd1(2)) / 3;  //点云平均尺度，用于设置主方向箭头大小
	std::cout << "width1=" << whd1(0) << std::endl;
	std::cout << "heght1=" << whd1(1) << std::endl;
	std::cout << "depth1=" << whd1(2) << std::endl;
	std::cout << "scale1=" << sc1 << std::endl;

	//四元素
	const Eigen::Quaternionf bboxQ1(Eigen::Quaternionf::Identity());
	const Eigen::Vector3f    bboxT1(c1);
	const Eigen::Quaternionf bboxQ(tm_inv.block<3, 3>(0, 0));
	const Eigen::Vector3f    bboxT(c);

	PointT op;
	op.x = 0.0;
	op.y = 0.0; 
	op.z = 0.0; 
	Eigen::Vector3f px, py, pz;
	Eigen::Affine3f tm_aff(tm);

	Eigen::Vector3f e1 = pca.getEigenVectors().col(0);
	Eigen::Vector3f e2 = pca.getEigenVectors().col(1);
	Eigen::Vector3f e3 = pca.getEigenVectors().col(2);

	std::cout << "第大特征值" << e1 << std::endl;
	pcl::transformVector(e1, px, tm_aff);
	pcl::transformVector(e2, py, tm_aff);
	pcl::transformVector(e3, pz, tm_aff);

	PointT pcaX;
	pcaX.x = sc1 * px(0);
	pcaX.y = sc1 * px(1);
	pcaX.z = sc1 * px(2);

	PointT pcaY;
	pcaY.x = sc1 * py(0);
	pcaY.y = sc1 * py(1);
	pcaY.z = sc1 * py(2);

	PointT pcaZ;
	pcaZ.x = sc1 * pz(0);
	pcaZ.y = sc1 * pz(1);
	pcaZ.z = sc1 * pz(2);

	//初始点云的主方向
	PointT cp;
	cp.x = pcaCentroid(0);
	cp.y = pcaCentroid(1);
	cp.z = pcaCentroid(2);
	PointT pcX;
	pcX.x = sc1 * pca.getEigenVectors()(0, 0) + cp.x;
	pcX.y = sc1 * pca.getEigenVectors()(1, 0) + cp.y;
	pcX.z = sc1 * pca.getEigenVectors()(2, 0) + cp.z;
	PointT pcY;
	pcY.x = sc1 * pca.getEigenVectors()(0, 1) + cp.x;
	pcY.y = sc1 * pca.getEigenVectors()(1, 1) + cp.y;
	pcY.z = sc1 * pca.getEigenVectors()(2, 1) + cp.z;
	PointT pcZ;
	pcZ.x = sc1 * pca.getEigenVectors()(0, 2) + cp.x;
	pcZ.y = sc1 * pca.getEigenVectors()(1, 2) + cp.y;
	pcZ.z = sc1 * pca.getEigenVectors()(2, 2) + cp.z;

	//visualization
	pcl::visualization::PCLVisualizer viewer;

	pcl::visualization::PointCloudColorHandlerCustom<PointT> tc_handler(transformedCloud, 70, 70, 70); //转换到原点的点云相关
	viewer.addPointCloud(transformedCloud, tc_handler, "transformCloud");
	viewer.addCube(bboxT1, bboxQ1, whd1(0), whd1(1), whd1(2), "bbox1");
	viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "bbox1");
	viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "bbox1");

	viewer.addArrow(pcaX, op, 1.0, 0.0, 0.0, false, "arrow_X");
	viewer.addArrow(pcaY, op, 0.0, 1.0, 0.0, false, "arrow_Y");
	viewer.addArrow(pcaZ, op, 0.0, 0.0, 1.0, false, "arrow_Z");

	pcl::visualization::PointCloudColorHandlerCustom<PointT> color_handler(cloud, 200, 200, 200);  //输入的初始点云相关
	viewer.addPointCloud(cloud, color_handler, "cloud");
	viewer.addCube(bboxT, bboxQ, whd(0), whd(1), whd(2), "bbox");
	viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "bbox");
	viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "bbox");

	viewer.addArrow(pcX, cp, 1.0, 0.0, 0.0, false, "arrow_x");
	viewer.addArrow(pcY, cp, 0.0, 1.0, 0.0, false, "arrow_y");
	viewer.addArrow(pcZ, cp, 0.0, 0.0, 1.0, false, "arrow_z");
	viewer.addArrow(reconstruct_points->points[0], reconstruct_points->points[1], 0.0, 0.0, 0.0, true, "pca_reproject_direction");
	PointT pStart(axis_start_point[0], axis_start_point[1], axis_start_point[2]), pEnd(axis_end_point[0], axis_end_point[1], axis_end_point[2]);

	//将点云上的极值点投影到轴心上
	Eigen::Vector3f ori_projstart_vec(reconstruct_points->points[0].x - pStart.x, reconstruct_points->points[0].y - pStart.y, reconstruct_points->points[0].z - pStart.z);
	//求参数t=（d(xp-a)+e(yp-b)+f(zp-c)）/( dd+ee+ff )
	double start_t = axis_direct_vec[0] * ori_projstart_vec[0] + axis_direct_vec[1] * ori_projstart_vec[1] + axis_direct_vec[1] * ori_projstart_vec[1] / axis_direct_vec.norm();
	
	Eigen::Vector3f ori_proend_vec(reconstruct_points->points[1].x - pStart.x, reconstruct_points->points[1].y - pStart.y, reconstruct_points->points[1].z - pStart.z);
	double end_t = axis_direct_vec[0] * ori_proend_vec[0] + axis_direct_vec[1] * ori_proend_vec[1] + axis_direct_vec[1] * ori_proend_vec[1] / axis_direct_vec.norm();

	//xm = dt + a ym = et + b zm = ft + c
	Eigen::Vector3f start_proj_point(axis_direct_vec[0] * start_t + axis_start_point[0], axis_direct_vec[1] * start_t + axis_start_point[1], axis_direct_vec[2] * start_t + axis_start_point[2]);
	Eigen::Vector3f end_proj_point(axis_direct_vec[0] * end_t + axis_start_point[0], axis_direct_vec[1] * end_t + axis_start_point[1], axis_direct_vec[2] * end_t + axis_start_point[2]);
	PointT project_start(start_proj_point[0], start_proj_point[1], start_proj_point[2]), project_end(end_proj_point[0], end_proj_point[1], end_proj_point[2]);

	viewer.addArrow(pStart, pEnd, 0.0, 0.0, 0.0, true, "axis");
	viewer.addArrow(project_start, project_end, 1, 0.0, 0.0, true, "true_axis");

	viewer.addCoordinateSystem(0.5f * sc1);
	viewer.setBackgroundColor(1.0, 1.0, 1.0);
	while (!viewer.wasStopped())
	{
    
    
		viewer.spinOnce(100);
	}
	return 0;
  return (0);
}

Point cloud download link