pcl detecting planes in the map
I am new to pcl, i would like to detect/segment all the planar surfaces ex: tables. I tried the tutorial :http://pointclouds.org/documentation/tutorials/extract_indices.php#extract-indices
and used the extracted points to visualize in rviz but i don not see the planar surfaces clearly. I guess i am displaying wrong points ..
Could someone explain that tutorial or tell how can i detect planar surfaces in pcl2 format and further use those pcl2 ??
Update: I have attached the code:
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
// PCL specific includes
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
ros::Publisher pub;
void process_cloud (const sensor_msgs::PointCloud2ConstPtr& input)
{
// ... do data processing
sensor_msgs::PointCloud2::Ptr cloud_blob (new sensor_msgs::PointCloud2), cloud_filtered_blob (new sensor_msgs::PointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
sensor_msgs::PointCloud2::Ptr final_cloud (new sensor_msgs::PointCloud2);
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
sor.setInputCloud (input); // input is the pcl2 received from /depthcam.
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered_blob);
// Convert to the templated PointCloud
pcl::fromROSMsg (*cloud_filtered_blob, *cloud_filtered);
std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
// Write the downsampled version to disk
pcl::PCDWriter writer;
writer.write<pcl::PointXYZ> ("downsampled.pcd", *cloud_filtered, false);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
int i = 0, nr_points = (int) cloud_filtered->points.size ();
// While 30% of the original cloud is still there
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
std::stringstream ss;
ss << "new_pcl" << i << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_f);
cloud_filtered.swap (cloud_f);
i++;
}
// conbert to sensormsg type
pcl::toROSMsg (*cloud_p, *final_cloud);
//sensor_msgs::PointCloud2 output;
// Publish the data
pub.publish (final_cloud); // publish the new pcl
}
int
main (int argc, char** argv)
{
// Initialize ROS
ros::init (argc, argv, "pcl_tabletop");
ros::NodeHandle nh;
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = nh.subscribe ("depthcam", 1, process_cloud); // depthcam is the ...