PCL圆柱拟合及圆柱中心点获取
- 问题描述
- PCL圆柱拟合
* - 获得圆柱轴心的起点跟终点
–
问题描述
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). 前三个系数表示圆柱体 轴心上的一点, 后三个系数表示沿着 轴的向量，最后一个系数表示 半径.
圆柱拟合关键代码片段
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);
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 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
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
typedef pcl::PointXYZ PointT;
boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis(pcl::PointCloud<PointT>::ConstPtr 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_COLOR, 1.0, 0.0, 0.0, "sample cloud");
viewer->addCoordinateSystem(0.1);
viewer->initCameraParameters();
return (viewer);
}
int
main ()
{
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> ());
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>);
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) << ").";
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.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);
}
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();
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;
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);
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();
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);
}
Original: https://blog.csdn.net/stanshi/article/details/124773320
Author: stanshi
Title: PCL圆柱拟合及圆柱中心点获取
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/520054/
转载文章受原作者版权保护。转载请注明原作者出处！