PCL学习笔记（三）：平面和直线提取

本节基于学习笔记（二）所学习的处理方法，在Gazebo中搭建仿真环境，提取地面和墙壁

---

仿真环境

在Gazebo中搭建一个走廊环境，移动机器人上搭载Kinect

---

地面提取

关键代码

// 获取原始点云
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr adjust_pcl_ptr (new pcl::PointCloud<pcl::PointXYZRGBA>);  
pcl::fromROSMsg(*kinectCloudMsg, *adjust_pcl_ptr);  //把msg消息转化为点云

// 平面分割
pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
seg.setInputCloud (adjust_pcl_ptr);
seg.segment (*inliers, *coefficients);

// 创建分割实例
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr ground_cloud (new pcl::PointCloud<pcl::PointXYZRGBA> ());
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr other_cloud (new pcl::PointCloud<pcl::PointXYZRGBA> ());
extract.setInputCloud (adjust_pcl_ptr);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*ground_cloud); 
extract.setNegative (true);
extract.filter (*other_cloud); 

//颜色设置
for (int i = 0; i < ground_cloud->points.size(); i++)
{
	ground_cloud->points[i].r = 0;
	ground_cloud->points[i].g = 255;
	ground_cloud->points[i].b = 0;
	ground_cloud->points[i].a = 255;
}
for (int i = 0; i < other_cloud->points.size(); i++)
{
	other_cloud->points[i].r = 255;
	other_cloud->points[i].g = 0;
	other_cloud->points[i].b = 0;
	other_cloud->points[i].a = 255;
}

// 发布点云
pcl::PointCloud<pcl::PointXYZRGBA> add_points = *ground_cloud + *other_cloud;
pcl::toROSMsg(add_points, plane_msg);  
pubCloud.publish(plane_msg);

提取结果

---

直线提取

分别直通滤波提取两侧的直线点云，通过RANSAC拟合直线参数，将两条直线参数取中点得到地面中线

关键代码

// 直通滤波获取左侧直线
pcl::PassThrough<pcl::PointXYZRGBA> passthrough;
passthrough.setInputCloud(line_cloud); 
passthrough.setFilterFieldName("x");
passthrough.setFilterLimits(-2, 0);
passthrough.setFilterLimitsNegative(false); 
passthrough.filter(*line_left_pass);   
// RANSAC拟合直线获取直线参数
pcl::ModelCoefficients::Ptr left_coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr left_inliers(new pcl::PointIndices);  //inliers表示误差能容忍的点 记录的是点云的序号
pcl::SACSegmentation<pcl::PointXYZRGBA> left_seg;    
left_seg.setOptimizeCoefficients(true);  
left_seg.setModelType(pcl::SACMODEL_LINE); 
left_seg.setMethodType(pcl::SAC_RANSAC);  
left_seg.setDistanceThreshold(0.01);         //设置误差容忍范围，也就是阈值
left_seg.setInputCloud(line_left_pass);            
left_seg.segment(*left_inliers, *left_coefficients);   //分割点云，获得平面和法向量

提取结果