pcl_find_plane.cpp
Go to the documentation of this file.
1 
4 #include <iostream>
5 #include <ros/ros.h>
6 
7 #include <sensor_msgs/PointCloud2.h>
9 #include <pcl/ModelCoefficients.h>
10 #include <pcl/io/pcd_io.h>
11 #include <pcl/point_types.h>
12 #include <pcl/sample_consensus/method_types.h>
13 #include <pcl/sample_consensus/model_types.h>
14 #include <pcl/segmentation/sac_segmentation.h>
15 #include <pcl/filters/voxel_grid.h>
16 #include <pcl/filters/extract_indices.h>
17 #include <pcl/visualization/cloud_viewer.h>
18 
20 
21 void
22 cloud_cb (const pcl::PCLPointCloud2ConstPtr& cloud_blob)
23 {
24 
25 
26  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
27 
28  pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
29  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
30 
31  // Create the filtering object: downsample the dataset using a leaf size of 1cm
32  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
33  sor.setInputCloud (cloud_blob);
34  sor.setLeafSize (0.2f, 0.2f, 0.2f);
35  sor.filter (*cloud_filtered_blob);
36 
37  // Convert to the templated PointCloud
38  pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
39 
40 
41  std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
42 
43 
44  //... populate cloud
45 
46  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
47  pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
48  // Create the segmentation object
49  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
50  // Optional
51  seg.setOptimizeCoefficients (true);
52  // Mandatory
53  seg.setModelType (pcl::SACMODEL_PLANE);
54  seg.setMethodType (pcl::SAC_RANSAC);
55  seg.setMaxIterations (1000);
56  seg.setDistanceThreshold (0.2);
57  seg.setInputCloud (cloud_filtered);
58 
59  int planeFndCnt = 0, nr_points = (int) cloud_filtered->points.size ();
60  pcl::IndicesPtr remaining (new std::vector<int>);
61  remaining->resize (nr_points);
62  for (size_t i = 0; i < remaining->size (); ++i) { (*remaining)[i] = static_cast<int>(i); }
63 
64  // While 30% of the original cloud is still there
65  while (remaining->size () > 0.5 * nr_points)
66  {
67  // Segment the largest planar component from the remaining cloud
68  seg.setIndices (remaining);
69  seg.segment (*inliers, *coefficients);
70  if (inliers->indices.size () == 0) break;
71 
72  // Extract the inliers
73  std::vector<int>::iterator it = remaining->begin();
74  for (size_t i = 0; i < inliers->indices.size (); ++i)
75  {
76  int curr = inliers->indices[i];
77  // Remove it from further consideration.
78  while (it != remaining->end() && *it < curr) { ++it; }
79  if (it == remaining->end()) break;
80  if (*it == curr) it = remaining->erase(it);
81  }
82  planeFndCnt++;
83  }
84  std::cout << "Found " << planeFndCnt << " planes." << std::endl;
85 
86  // Color all the non-planar things.
87  for (std::vector<int>::iterator it = remaining->begin(); it != remaining->end(); ++it)
88  {
89  uint8_t r = 0, g = 255, b = 0;
90  uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
91  cloud_filtered->at(*it).rgb = *reinterpret_cast<float*>(&rgb);
92  }
93 
94  pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
95  viewer.showCloud (cloud_filtered);
96  while (!viewer.wasStopped ())
97  {
98  }
99  // Publish the planes we found.
100  pcl::PCLPointCloud2 outcloud;
101  pcl::toPCLPointCloud2 (*cloud_filtered, outcloud);
102  pub.publish (outcloud);
103 }
104 
105 int
106 main (int argc, char** argv)
107 {
108  // Initialize ROS
109  ros::init (argc, argv, "obstacles");
110  ros::NodeHandle nh;
111 
112  std::string inputTopic = "input";
113 
114 
115  std::string heatMapName;
116  nh.getParam("heat_map", heatMapName);
117  if (argc > 1)
118  {
119  inputTopic = argv[1];
120  }
121  // Create a ROS subscriber for the input point cloud
122  ros::Subscriber sub = nh.subscribe (inputTopic, 1, cloud_cb);
123 
124  // Create a ROS publisher for the output point cloud
125  pub = nh.advertise<pcl::PCLPointCloud2> ("obstacles", 1);
126 
127  // Spin
128  ros::spin ();
129 }
ros::Publisher
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
ros.h
pub
ros::Publisher pub
Definition: pcl_find_plane.cpp:19
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
f
f
cloud_cb
void cloud_cb(const pcl::PCLPointCloud2ConstPtr &cloud_blob)
Definition: pcl_find_plane.cpp:22
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
main
int main(int argc, char **argv)
Definition: pcl_find_plane.cpp:106
ros::spin
ROSCPP_DECL void spin()
ros::NodeHandle
ros::Subscriber
pcl_conversions.h


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Thu Sep 8 2022 02:30:19