obj_rec_ransac_orr_octree_zprojection.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  *
00038  */
00039 
00040 /*
00041  * obj_rec_ransac_orr_octree_zprojection.cpp
00042  *
00043  *  Created on: Jan 15, 2013
00044  *      Author: papazov
00045  *
00046  *  Visualizes the specialized octree class used for the ransac-based object
00047  *  recognition. Use the left/right arrows to select a full octree leaf which
00048  *  will be used to place a sphere at it and to cut the sphere against the
00049  *  other full octree leaves which are visualized in yellow.
00050  */
00051 
00052 #include <pcl/point_cloud.h>
00053 #include <pcl/point_types.h>
00054 #include <pcl/io/pcd_io.h>
00055 #include <pcl/recognition/ransac_based/orr_octree_zprojection.h>
00056 #include <pcl/visualization/pcl_visualizer.h>
00057 #include <vtkRenderWindow.h>
00058 #include <vtkPolyData.h>
00059 #include <vtkAppendPolyData.h>
00060 #include <vtkPolyDataReader.h>
00061 #include <vtkCubeSource.h>
00062 #include <vtkPointData.h>
00063 #include <vector>
00064 #include <list>
00065 #include <cstdlib>
00066 #include <cstring>
00067 #include <cstdio>
00068 
00069 using namespace pcl;
00070 using namespace pcl::visualization;
00071 using namespace pcl::recognition;
00072 using namespace pcl::io;
00073 
00074 void run (const char *file_name, float voxel_size);
00075 bool vtk_to_pointcloud (const char* file_name, PointCloud<PointXYZ>& points);
00076 void show_octree (ORROctree* octree, PCLVisualizer& viz);
00077 void show_octree_zproj (ORROctreeZProjection* zproj, PCLVisualizer& viz);
00078 void node_to_cube (ORROctree::Node* node, vtkAppendPolyData* additive_octree);
00079 void rectangle_to_vtk (float x1, float x2, float y1, float y2, float z, vtkAppendPolyData* additive_rectangle);
00080 
00081 //#define _SHOW_POINTS_
00082 
00083 int main (int argc, char ** argv)
00084 {
00085   if ( argc != 3 )
00086   {
00087     fprintf(stderr, "\nERROR: Syntax is ./pcl_obj_rec_ransac_orr_octree_zprojection <vtk file> <leaf_size>\n"
00088                     "EXAMPLE: ./pcl_obj_rec_ransac_orr_octree_zprojection ../../test/tum_table_scene.vtk 6\n\n");
00089     return -1;
00090   }
00091 
00092   // Get the voxel size
00093   float voxel_size = static_cast<float> (atof (argv[2]));
00094   if ( voxel_size <= 0.0 )
00095   {
00096     fprintf(stderr, "ERROR: leaf_size has to be positive and not %lf\n", voxel_size);
00097     return -1;
00098   }
00099 
00100   run(argv[1], voxel_size);
00101 }
00102 
00103 //===============================================================================================================================
00104 
00105 void run (const char* file_name, float voxel_size)
00106 {
00107   PointCloud<PointXYZ>::Ptr points_in (new PointCloud<PointXYZ> ());
00108   PointCloud<PointXYZ>::Ptr points_out (new PointCloud<PointXYZ> ());
00109 
00110   // Get the points and normals from the input vtk file
00111   if ( !vtk_to_pointcloud (file_name, *points_in) )
00112     return;
00113 
00114   // Build the octree with the desired resolution
00115   ORROctree octree;
00116   octree.build (*points_in, voxel_size);
00117 
00118   // Now build the octree z-projection
00119   ORROctreeZProjection zproj;
00120   zproj.build (octree, 0.15f*voxel_size, 0.15f*voxel_size);
00121 
00122   // The visualizer
00123   PCLVisualizer viz;
00124 
00125   show_octree(&octree, viz);
00126   show_octree_zproj(&zproj, viz);
00127 
00128 #ifdef _SHOW_POINTS_
00129   // Get the point of every full octree leaf
00130   octree.getFullLeafPoints (*points_out);
00131 
00132   // Add the point clouds
00133   viz.addPointCloud (points_in, "cloud in");
00134   viz.addPointCloud (points_out, "cloud out");
00135 
00136   // Change the appearance
00137   viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud in");
00138   viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloud out");
00139   viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "cloud out");
00140 #endif
00141 
00142   // Enter the main loop
00143   while (!viz.wasStopped ())
00144   {
00145     //main loop of the visualizer
00146     viz.spinOnce (100);
00147     boost::this_thread::sleep (boost::posix_time::microseconds (100000));
00148   }
00149 }
00150 
00151 //===============================================================================================================================
00152 
00153 bool vtk_to_pointcloud (const char* file_name, PointCloud<PointXYZ>& pcl_points)
00154 {
00155   size_t len = strlen (file_name);
00156   if ( file_name[len-3] != 'v' || file_name[len-2] != 't' || file_name[len-1] != 'k' )
00157   {
00158     fprintf (stderr, "ERROR: we need a .vtk object!\n");
00159     return false;
00160   }
00161 
00162   // Load the model
00163   vtkSmartPointer<vtkPolyDataReader> reader = vtkSmartPointer<vtkPolyDataReader>::New ();
00164   reader->SetFileName (file_name);
00165   reader->Update ();
00166 
00167   // Get the points
00168   vtkPolyData *vtk_poly = reader->GetOutput ();
00169   vtkPoints *vtk_points = vtk_poly->GetPoints ();
00170   vtkIdType num_points = vtk_points->GetNumberOfPoints ();
00171   double p[3];
00172 
00173   pcl_points.resize (num_points);
00174 
00175   // Copy the points
00176   for ( vtkIdType i = 0 ; i < num_points ; ++i )
00177   {
00178     vtk_points->GetPoint (i, p);
00179     pcl_points[i].x = static_cast<float> (p[0]);
00180     pcl_points[i].y = static_cast<float> (p[1]);
00181     pcl_points[i].z = static_cast<float> (p[2]);
00182   }
00183 
00184   return true;
00185 }
00186 
00187 //===============================================================================================================================
00188 
00189 void show_octree (ORROctree* octree, PCLVisualizer& viz)
00190 {
00191   vtkSmartPointer<vtkPolyData> vtk_octree = vtkSmartPointer<vtkPolyData>::New ();
00192   vtkSmartPointer<vtkAppendPolyData> append = vtkSmartPointer<vtkAppendPolyData>::New ();
00193 
00194   cout << "There are " << octree->getFullLeaves ().size () << " full leaves.\n";
00195 
00196   std::vector<ORROctree::Node*>& full_leaves = octree->getFullLeaves ();
00197   for ( std::vector<ORROctree::Node*>::iterator it = full_leaves.begin () ; it != full_leaves.end () ; ++it )
00198     // Add it to the other cubes
00199     node_to_cube (*it, append);
00200 
00201   // Save the result
00202   append->Update();
00203   vtk_octree->DeepCopy (append->GetOutput ());
00204 
00205   // Add to the visualizer
00206   vtkRenderer *renderer = viz.getRenderWindow ()->GetRenderers ()->GetFirstRenderer ();
00207   vtkSmartPointer<vtkActor> octree_actor = vtkSmartPointer<vtkActor>::New();
00208   vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
00209   mapper->SetInput(vtk_octree);
00210   octree_actor->SetMapper(mapper);
00211 
00212   // Set the appearance & add to the renderer
00213   octree_actor->GetProperty ()->SetColor (1.0, 1.0, 1.0);
00214   renderer->AddActor(octree_actor);
00215 }
00216 
00217 //===============================================================================================================================
00218 
00219 void show_octree_zproj (ORROctreeZProjection* zproj, PCLVisualizer& viz)
00220 {
00221   cout << "There is (are) " << zproj->getFullPixels ().size () << " full pixel(s).\n";
00222 
00223   vtkSmartPointer<vtkAppendPolyData> upper_bound = vtkSmartPointer<vtkAppendPolyData>::New (), lower_bound = vtkSmartPointer<vtkAppendPolyData>::New ();
00224   const ORROctreeZProjection::Pixel *pixel;
00225   const float *b = zproj->getBounds ();
00226   float x, y, psize = zproj->getPixelSize ();
00227   int i, j, width, height;
00228 
00229   zproj->getNumberOfPixels (width, height);
00230 
00231   for ( i = 0, x = b[0] ; i < width ; ++i, x += psize )
00232   {
00233     for ( j = 0, y = b[2] ; j < height ; ++j, y += psize )
00234     {
00235       pixel = zproj->getPixel (i, j);
00236 
00237       if ( !pixel )
00238         continue;
00239 
00240       rectangle_to_vtk (x, x + psize, y, y + psize, pixel->z1 (), lower_bound);
00241       rectangle_to_vtk (x, x + psize, y, y + psize, pixel->z2 (), upper_bound);
00242     }
00243   }
00244 
00245   // Save the result
00246   upper_bound->Update();
00247   lower_bound->Update();
00248 
00249   // Add to the visualizer
00250   vtkRenderer *renderer = viz.getRenderWindow ()->GetRenderers ()->GetFirstRenderer ();
00251   vtkSmartPointer<vtkActor> upper_actor = vtkSmartPointer<vtkActor>::New(), lower_actor = vtkSmartPointer<vtkActor>::New();
00252   vtkSmartPointer<vtkDataSetMapper> upper_mapper = vtkSmartPointer<vtkDataSetMapper>::New (), lower_mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
00253   upper_mapper->SetInput(upper_bound->GetOutput ());
00254   upper_actor->SetMapper(upper_mapper);
00255   lower_mapper->SetInput(lower_bound->GetOutput ());
00256   lower_actor->SetMapper(lower_mapper);
00257 
00258   // Set the appearance & add to the renderer
00259   upper_actor->GetProperty ()->SetColor (1.0, 0.0, 0.0);
00260   renderer->AddActor(upper_actor);
00261   lower_actor->GetProperty ()->SetColor (1.0, 1.0, 0.0);
00262   renderer->AddActor(lower_actor);
00263 }
00264 
00265 //===============================================================================================================================
00266 
00267 void node_to_cube (ORROctree::Node* node, vtkAppendPolyData* additive_octree)
00268 {
00269   // Define the cube representing the leaf
00270   const float *b = node->getBounds ();
00271   vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
00272   cube->SetBounds (b[0], b[1], b[2], b[3], b[4], b[5]);
00273   cube->Update ();
00274 
00275   additive_octree->AddInput (cube->GetOutput ());
00276 }
00277 
00278 //===============================================================================================================================
00279 
00280 void rectangle_to_vtk (float x1, float x2, float y1, float y2, float z, vtkAppendPolyData* additive_rectangle)
00281 {
00282   // Define the cube representing the leaf
00283   vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
00284   cube->SetBounds (x1, x2, y1, y2, z, z);
00285   cube->Update ();
00286 
00287   additive_rectangle->AddInput (cube->GetOutput ());
00288 }
00289 
00290 //===============================================================================================================================


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:26:12