【转】PCL提取圆柱系数
网上看了很多教程,没看到圆柱提取后的系数解释。
源码如下:
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.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/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/filters/radius_outlier_removal.h>
typedef pcl::PointXYZ PointT;
// 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::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
// Datasets
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients), coefficients_cylinder(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices), inliers_cylinder(new pcl::PointIndices);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("viewer"));
pcl::PointCloud<pcl::PointXYZ>::Ptr clicked_points_3d(new pcl::PointCloud<pcl::PointXYZ>);
int num = 0;
void pp_callback(const pcl::visualization::AreaPickingEvent& event, void* args)
{
clicked_points_3d->points.clear();
pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);
std::vector< int > indices;
if (event.getPointsIndices(indices) == -1)
return;
for (int i = 0; i < indices.size(); ++i)
{
clicked_points_3d->points.push_back(cloud->points.at(indices[i]));
}
//clicked_points_3d->width = 1;
//clicked_points_3d->height = clicked_points_3d->size();
//if (!clicked_points_3d->points.empty())
//{
// writer.write("Selected.pcd", *clicked_points_3d, false);
//}
// Estimate point normals
ne.setSearchMethod(tree);
ne.setInputCloud(clicked_points_3d);
ne.setKSearch(50);
ne.compute(*cloud_normals);
// 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);
double threshold;
std::cout << "threshold: ";
std::cin >> threshold;
seg.setDistanceThreshold(threshold); //单位米
double radius;
std::cout << "radius: ";
std::cin >> radius;
seg.setRadiusLimits(0, radius); //单位米
seg.setInputCloud(clicked_points_3d);
seg.setInputNormals(cloud_normals);
// Obtain the cylinder inliers and coefficients
seg.segment(*inliers_cylinder, *coefficients_cylinder);
std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
// Write the cylinder inliers to disk
extract.setInputCloud(clicked_points_3d);
extract.setIndices(inliers_cylinder);
extract.setNegative(false);
pcl::PointCloud<PointT>::Ptr cloud_cylinder(new pcl::PointCloud<PointT>());
extract.filter(*cloud_cylinder);
if (cloud_cylinder->points.empty())
std::cerr << "Can't find the cylindrical component." << std::endl;
else
{
std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size() << " data points." << std::endl;
cloud_cylinder->width = 1;
cloud_cylinder->height = cloud_cylinder->size();
writer.write("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
}
system("pause");
std::stringstream ss;
std::string cloudName;
ss << num++;
ss >> cloudName;
cloudName += "_cloudName";
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red(clicked_points_3d, 255, 0, 0);
viewer->addPointCloud(clicked_points_3d, red, cloudName);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, cloudName);
//std::stringstream ss;
//std::string cloudName;
ss << num++;
ss >> cloudName;
cloudName += "_cloudName";
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue(cloud_cylinder, 0, 0, 255);
viewer->addPointCloud(cloud_cylinder, blue, cloudName);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, cloudName);
}
int main (int argc, char** argv)
{
//std::string location;
//std::getline(std::cin, location);
//if (location[0] == '"')
//{
// location = location.substr(1, location.length() - 2);
//}
//if (pcl::io::loadPCDFile<pcl::PointXYZ>(location, *cloud) == -1)
//{
// PCL_ERROR("Couldn't read file \n");
// system("pause");
//}
//std::cout << "Reading Success" << std::endl;
//生成圆柱点云
for (float z(-10); z <= 10; z += 0.5)
{
for (float angle(0.0); angle <= 360.0; angle += 5.0)
{
pcl::PointXYZ basic_point;
basic_point.x = 10+3.5*cos(angle / 180 * M_PI);
basic_point.y = 200+3.5*sin(angle / 180 * M_PI);
basic_point.z = z;
cloud->points.push_back(basic_point);
}
}
Read in the cloud data
//reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
//std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;
// Build a passthrough filter to remove spurious NaNs
//pass.setInputCloud (cloud);
//pass.setFilterFieldName ("z");
//pass.setFilterLimits (0, 1.5);
//pass.filter (*#);
//std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
viewer->addPointCloud(cloud, "bunny");
viewer->setCameraPosition(0, 0, -2, 0, -1, 0, 0);
viewer->registerAreaPickingCallback(pp_callback, (void*)&cloud);
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return (0);
}
程序运行后看不见点云按R键
接着按下X键选中点云,再按下X键
设置偏差阈值为1
圆柱的半径大于3.5
就可以得到如下结果
系数0、1、2代表圆柱轴线上的原点,3、4、5代表这条轴线的方向向量,系数6就是圆柱的半径。
向原作者表示感谢!!