问题描述
PCL圆柱拟合能给到轴的一点, 沿着轴的向量，以及半径. 但是没法知道圆柱的长度，以及起点，终点. 这样就不能确圆柱的轴心.
PCL圆柱拟合
PCL提供了SACMODEL_CYLINDER模型，定义为圆柱体模型，共设置7个参数，从点云中分割提取的内点都处在估计参数对应的圆柱体上或距离圆柱体的距离在一定范围内. 模型系数(point_on_axis.x, point_on_axis.y, point_on_axis.z, axis_direction.x, axis_direction.y, axis_direction.z,radius). 前三个系数表示圆柱体轴心上的一点, 后三个系数表示沿着轴的向量，最后一个系数表示半径.
圆柱拟合关键代码片段
// 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);
验证可知圆柱模型无法得到圆柱轴心的起点与终点.
获得圆柱轴心的起点跟终点
方法思路:
1. 将圆柱模型通过PCA主成分分析, 获得圆柱体轴向最大最小值.
2. 将通过PCA分析以后得到的最大，最小的两个点在反投影到原点云.
3. 将得到的在原点云的最大最小的点在映射到轴线上, 这样就得到了圆柱的起点跟终点
最终结果, 圆柱轴上红色线代表投影的结果, 黑色箭头直线代表按照模型系数前三个值作为起始点的画出的线, 验证了圆柱模型系数前三个系数不是起始点或者终点.
计算出起始点，终点，就能计算出轴心点.
投影点映射到直线是通过向量求解获取, 圆柱模型系数已知点跟向量, 轴的直线就能表示出来, 然后就能获取投影点, 具体可参考投影点计算
关键代码
//将点云上的极值点投影到轴心上
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]);
完整代码
#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);
}
文章出处登录后可见！
已经登录？立即刷新
