Ask Your Question
1

ROS pcl run-time error segmenting planes

asked 2012-07-25 13:24:49 -0500

MartinW gravatar image

Hello all,

I am trying to write a program that subscribes to the openni_launch's pointcloud topic, downsamples the data, and publishes an extracted pointcloud plane. So far the code compiles fine but upon running it produces this error:

/usr/include/boost/smart_ptr/shared_ptr.hpp:412: boost::shared_ptr<t>::reference boost::shared_ptr<t>::operator*() const [with T = sensor_msgs::PointCloud2_<std::allocator<void> >, boost::shared_ptr<t>::reference = sensor_msgs::PointCloud2_<std::allocator<void> >&]: Assertion `px != 0' failed. Aborted (core dumped)

I am unsure of where the error is coming from and would appreciate any insight. Here is the code I have written thus far:

   // Includes and whatnot

    ros::Publisher pub;
    sensor_msgs::PointCloud2::Ptr downsampled,output;

    pcl::PointCloud<pcl::PointXYZ>::Ptr output_p, downsampled_XYZ;


    void callback(const sensor_msgs::PointCloud2ConstPtr& input)
    {
      // Do some downsampling to the point cloud
      pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
      sor.setInputCloud (input);
      sor.setLeafSize (0.01f, 0.01f, 0.01f);
      sor.filter (*downsampled);

      // Change from type sensor_msgs::PointCloud2 to pcl::PointXYZ
      pcl::fromROSMsg (*downsampled, *downsampled_XYZ);


      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;

      // Segment the largest planar component from the cloud
      seg.setInputCloud (downsampled_XYZ);
      seg.segment (*inliers, *coefficients);
      if (inliers->indices.size () == 0)
          {
              std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
              }

      // Extract the inliers
      extract.setInputCloud (downsampled_XYZ);
      extract.setIndices (inliers);
      extract.setNegative (false);
      extract.filter (*output_p);
      std::cerr << "PointCloud representing the planar component: " << output_p->width * output_p->height << " data points." << std::endl;

      // Create the filtering object
      // extract.setNegative (true);
      // extract.filter (*cloud_f);
      // cloud_filtered.swap (cloud_f);


      pcl::toROSMsg (*output_p, *output);

      //Publish the results
      pub.publish(output);
    }




    int
    main (int argc, char** argv)
    {
     // INITIALIZE ROS
      ros::init (argc, argv, "table");
      ros::NodeHandle nh;
      ros::Subscriber sub = nh.subscribe("/camera/depth_registered/points", 1, callback);

      pub = nh.advertise<sensor_msgs::PointCloud2> ("table", 1);

      ros::spin();


      return (0);
    }

Thank you in advance!

Cheers, Martin

edit retag flag offensive close merge delete

Comments

2 Answers

Sort by ยป oldest newest most voted
3

answered 2012-07-25 15:00:32 -0500

I am guessing that you should allocate the memory for output_p and downsampled_XYZ when you enter the callback.

Also, one more advice, after

pcl::toROSMsg (*output_p, *output);

You should set the frame_id of output to the same value as input

output.header.frame_id = input->header.frame_id;

otherwise if you try to visualize the point cloud with rviz, it will complain because it would not know the frame of reference for output.

edit flag offensive delete link more

Comments

Thanks! I got my program running and I've extracted the pointcloud plane of a table in front of my Kinect! I tried to add your line of code "output.header.frame_id = input->header.frame_id;" But when I did it said sensor_msgs::pointcloud2 has no member header. But I could see it in Rviz anyway!

MartinW gravatar imageMartinW ( 2012-07-26 09:26:43 -0500 )edit
1

answered 2013-12-01 16:16:30 -0500

sharky gravatar image

pcl::PointCloud<pcl::pointxyz>::Ptr output_p(new pcl::PointCloud<pcl::pointxyz>);

That did it for me :)

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2012-07-25 13:24:49 -0500

Seen: 1,590 times

Last updated: Dec 01 '13