Added possibilit to get the table point cloud

trunk/src/examples/tos_supervoxels_test.cpp

@@ -122,18 +122,22 @@ main (int argc, char ** argv)
   viewer->setBackgroundColor (0, 0, 0);
   viewer->registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer);  
   
+  seg.show_table_plane(viewer); // we add the table plane  
+  std::cout << "Press 'n' to show the segmented objects\n";
+  while (!viewer->wasStopped () && !pressed) // the pressed variable is just usfull only for this first while (bad programming)
+      viewer->spinOnce (100);
+
   // show super voxels with normals and adiacency map 
   bool show_adjacency_map = true;
   bool show_super_voxel_normals = false;
   seg.show_super_voxels(viewer,show_adjacency_map,show_super_voxel_normals);  
   
-  std::cout << "Press 'n' to show the segmented objects\n";
   while (!viewer->wasStopped () && !pressed) // the pressed variable is just usfull only for this first while (bad programming)
       viewer->spinOnce (100);
   seg.clean_viewer(viewer);
 
   // show the segmented objects, the result of the segmentation
-  std::cout << "\nClose the visualizer to go to the next step\n";
+  std::cout << "\nClose the visualizer to go to the next step: retrieving the objects point in the RGB image\n";
   seg.show_segmented_objects(viewer);
   while (!viewer->wasStopped ()) // the pressed variable is just usfull only for this first while (bad programming)
       viewer->spinOnce (100);

trunk/src/tos_supervoxels.cpp

@@ -166,6 +166,7 @@ void tos_supervoxels::detectObjectsOnTable(pcl::PointCloud<pcl::PointXYZRGBA>::P
   pcl::PointIndices::Ptr planeIndices(new pcl::PointIndices);
   segmentation.segment(*planeIndices, this->plane_coefficients);
 
+
   if (planeIndices->indices.size() == 0)
     std::cout << "Could not find a plane in the scene." << std::endl;
   else
@@ -174,11 +175,13 @@ void tos_supervoxels::detectObjectsOnTable(pcl::PointCloud<pcl::PointXYZRGBA>::P
     pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
     extract.setInputCloud(cloud);
     extract.setIndices(planeIndices);
+    //extract.filter(*(this->table_plane_cloud));
     extract.filter(*plane);
  
     // Retrieve the convex hull.
     pcl::ConvexHull<pcl::PointXYZRGBA> hull;
     hull.setInputCloud(plane);
+    //hull.setInputCloud(this->table_plane_cloud);
     // Make sure that the resulting hull is bidimensional.
     hull.setDimension(2); //2dimension -> planar convex hull
     hull.reconstruct(*convexHull);
@@ -207,6 +210,8 @@ void tos_supervoxels::detectObjectsOnTable(pcl::PointCloud<pcl::PointXYZRGBA>::P
 
     }
     else std::cout << "The chosen hull is not planar." << std::endl;
+
+    this->table_plane_cloud = plane;
   }
 }
 
@@ -472,12 +477,18 @@ void tos_supervoxels::show_segmented_objects(boost::shared_ptr<pcl::visualizatio
   return;
 }
 
+void tos_supervoxels::show_table_plane(boost::shared_ptr<pcl::visualization::PCLVisualizer> & viewer)
+{
+  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> rgb(this->table_plane_cloud);
+  viewer->addPointCloud<pcl::PointXYZRGBA> (this->table_plane_cloud, rgb, "table_plane_cloud");
+}
+
 void tos_supervoxels::clean_viewer(boost::shared_ptr<pcl::visualization::PCLVisualizer> & viewer)
 {
   viewer->removePointCloud("supervoxel_cloud");
   viewer->removePointCloud("supervoxel_normals");
   viewer->removePointCloud("segmented_object_cloud");
-
+  viewer->removePointCloud("table_plane_cloud");
 
   std::multimap<uint32_t,uint32_t>::iterator label_itr = (this->supervoxel_adjacency).begin ();
   for ( ; label_itr != (this->supervoxel_adjacency).end(); )
@@ -535,7 +546,10 @@ pcl::PointCloud<pcl::PointXYZRGBA>::Ptr tos_supervoxels::get_input_cloud()
 {
   return this->cloud;
 }
-
+pcl::PointCloud<pcl::PointXYZRGBA>::Ptr tos_supervoxels::get_plane_cloud()
+{
+  return this->table_plane_cloud;
+}
 pcl::PointCloud<pcl::PointXYZL> tos_supervoxels::get_labeled_voxel_cloud()
 {
   return *(this->labeled_voxel_cloud);

trunk/src/tos_supervoxels.h

@@ -148,6 +148,9 @@ class tos_supervoxels
 
     std::vector<Object> detected_objects;
     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud;
+
+    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr table_plane_cloud;
+
     pcl::PointCloud<pcl::PointXYZL>::Ptr lccp_labeled_cloud;
     std::multimap<uint32_t, uint32_t> supervoxel_adjacency;
     pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_voxel_cloud;
@@ -266,6 +269,12 @@ class tos_supervoxels
     */
   	void show_segmented_objects(boost::shared_ptr<pcl::visualization::PCLVisualizer> & viewer); 
 
+    /*! \brief show the detect table plane
+    *
+    * \param viewer viewer in which shows the objects
+    */
+    void show_table_plane(boost::shared_ptr<pcl::visualization::PCLVisualizer> & viewer); 
+
     /*! \brief clean all the pointclouds or shapes introduced by the class in the viewer
     *
     * \param viewer viewer to clean up
@@ -289,6 +298,10 @@ class tos_supervoxels
     */
     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr get_input_cloud();
 
+    /*! \brief returns the table plane point cloud
+    */
+    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr get_plane_cloud();
+
     /*! \brief returns labeld voxel cloud
     */
     pcl::PointCloud<pcl::PointXYZL> get_labeled_voxel_cloud();