ros/pcl 点云分割——分离地面

代码 

#include 
// PCL specific includes
#include 
#include 
#include 
#include 
#include 
//关于平面分割的头文件
#include    
#include    
#include   

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 



ros::Publisher pub;

void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // 将点云格式为sensor_msgs/PointCloud2 格式转为 pcl/PointCloud
 pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
 pcl::PointCloud::Ptr cloud_filtered(new 
 pcl::PointCloud);
 pcl::fromROSMsg (*input,*cloud); 


  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
 // pcl::ModelCoefficients coefficients;   //申明模型的参数
 // pcl::PointIndices inliers;             //申明存储模型的内点的索引
  // 创建一个分割方法
  pcl::SACSegmentation seg;
  // 这一句可以选择最优化参数的因子
  seg.setOptimizeCoefficients (true);

  seg.setModelType (pcl::SACMODEL_PLANE);   //平面模型
  seg.setMethodType (pcl::SAC_RANSAC);    //分割平面模型所使用的分割方法
  seg.setDistanceThreshold (0.2);        //设置最小的阀值距离

  seg.setInputCloud (cloud);   //设置输入的点云
  seg.segment (*inliers,*coefficients);  

  // 把提取出来的外点 -> ros发布出去
  pcl::ExtractIndices extract;
  extract.setInputCloud (cloud);
  extract.setIndices (inliers);
  extract.setNegative (true);
  extract.filter (*cloud_filtered);
  //再rviz上显示所以要转换回PointCloud2
  sensor_msgs::PointCloud2 output;  
  pcl::toROSMsg(*cloud_filtered,output);
  pub.publish (output);
}

int main (int argc, char** argv)
{

  ros::init (argc, argv, "yyf");
  ros::NodeHandle nh;

  ros::Subscriber sub = nh.subscribe ("rslidar_points_in", 1, cloud_cb);

  pub = nh.advertise ("output", 1);

  ros::spin ();
}

 

实验结果：

 

 

 

虚线是地面 颗粒度大的是物体