ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
2

pcl detecting planes in the map

asked 2013-07-11 06:01:26 -0600

kk gravatar image

updated 2013-07-12 12:06:24 -0600

Philip gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2013-07-13 12:26:17 -0600

kk gravatar image

I think the problem is in publishing... when i tried to publish with buffer size 1000(earlier it was 1) .. i saw output put in rviz... but this did not happen always adding more confusion ;-p The pcl is not published continuously... whenever i close the pcl visualiser window(it restarts again) the pcl was published( rostopic echo showing the data in this situation and after this no message published ..again have to close pcl visualiser)...

so the problem is pcl is not published continuously ... so any idea how to publish continuously (not to forget that i am already publishing inside the callback function of a subscriber making it continuous or am i wrong )...

edit flag offensive delete link more

Comments

If you use the code you posted on dropbox, this is the correct behaviour. What happens is the following: You enter the callback, pcl visualizer spins until the viewer is closed. Afterwards, the publisher is invoked. Replace 'while (!viewer->wasStopped ()) {...}' by a simple 'viewer->spinOnce (100)'.

Philip gravatar image Philip  ( 2013-07-14 06:04:20 -0600 )edit

Hi, Yeah you are right, i did not notice that pcl visualiser is running continuously ;-p Further i don not use it so removed :) And major issue was with the buffer size of the publisher... Now its all working fine thanks a ton :)

kk gravatar image kk  ( 2013-07-17 07:22:07 -0600 )edit

any idea on this : http://answers.ros.org/question/67492/marker-array-not-visible-in-rviz-but-is-getting-published/ ??

kk gravatar image kk  ( 2013-07-17 07:22:40 -0600 )edit
2

answered 2013-07-11 10:03:41 -0600

Philip gravatar image

updated 2013-07-12 05:47:38 -0600

I guess this question would be more suited to the pcl user mailinglist as it is not really related to ROS.

Did you have a look at the plane segmentation tutorial of PCL?


Based on the information so far, it could be lots of reasons (wrong computation, error in publishing the result, ...). Can you be a little more specific about what doesn't work? What exactly do you see in RViz (no points / wrong points / "almost right points")? Can you verify that PCL segments the correct points, e.g. by displaying the result in PCL visualizer? Adding the code for publishing to your question might also help.

edit flag offensive delete link more

Comments

Hi, thank you for the reply. Even though its about pcl i am not able to visualise in rviz. The tutorial is working fine but i have modified to read pcl from a ros topic and and further publish the extracted points as a ros topic. When i visualize in rviz i do not see proper planes detected

kk gravatar image kk  ( 2013-07-12 03:35:31 -0600 )edit

i updated the question with code.

kk gravatar image kk  ( 2013-07-12 09:50:30 -0600 )edit
1

I reformatted your code for easier readability. Can you give information about what doesn't work (see my question)?

Philip gravatar image Philip  ( 2013-07-12 12:08:52 -0600 )edit

Thanks, just now i tried similar example using ransac and visulaized in pcl visualizer as you said.I will update the question.

kk gravatar image kk  ( 2013-07-12 12:11:45 -0600 )edit

I am unable to upload snapshots saying insufficient badges can i send it by email ?? please give ur id thanks

kk gravatar image kk  ( 2013-07-12 12:19:49 -0600 )edit
1

Please use an image/file sharing service to do this until you earn more karma.

tfoote gravatar image tfoote  ( 2013-07-12 13:14:28 -0600 )edit

Thanks , here is the drop box link : http://db.tt/lPzmAU1f

kk gravatar image kk  ( 2013-07-12 13:45:12 -0600 )edit

Strange! I fear that I have no immediate idea... can you verify that the final_cloud which you send to RViz is created correctly? E.g. display the number of points by something like printf(final_cloud.fields.count)

Philip gravatar image Philip  ( 2013-07-12 22:52:03 -0600 )edit

Question Tools

Stats

Asked: 2013-07-11 06:01:26 -0600

Seen: 6,582 times

Last updated: Jul 13 '13