octree_viewer.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  *
00035  *  \author Raphael Favier
00036  * */
00037 
00038 #include <boost/thread/thread.hpp>
00039 
00040 #include <pcl/io/pcd_io.h>
00041 
00042 #include <pcl/visualization/pcl_visualizer.h>
00043 #include <pcl/visualization/point_cloud_handlers.h>
00044 #include <pcl/visualization/common/common.h>
00045 
00046 #include <pcl/octree/octree.h>
00047 #include <pcl/octree/octree_impl.h>
00048 
00049 #include <pcl/filters/filter.h>
00050 
00051 #include <omp.h>
00052 
00053 //=============================
00054 // Displaying cubes is very long!
00055 // so we limit their numbers.
00056  const int MAX_DISPLAYED_CUBES(15000);
00057 //=============================
00058 
00059 class OctreeViewer
00060 {
00061 public:
00062   OctreeViewer (std::string &filename, double resolution) :
00063     viz ("Octree visualizator"), cloud (new pcl::PointCloud<pcl::PointXYZ>()),
00064         displayCloud (new pcl::PointCloud<pcl::PointXYZ>()), octree (resolution), displayCubes(false),
00065         showPointsWithCubes (false), wireframe (true)
00066   {
00067 
00068     //try to load the cloud
00069     if (!loadCloud(filename))
00070       return;
00071 
00072     //register keyboard callbacks
00073     viz.registerKeyboardCallback(&OctreeViewer::keyboardEventOccurred, *this, 0);
00074 
00075     //key legends
00076     viz.addText("Keys:", 0, 170, 0.0, 1.0, 0.0, "keys_t");
00077     viz.addText("a -> Increment displayed depth", 10, 155, 0.0, 1.0, 0.0, "key_a_t");
00078     viz.addText("z -> Decrement displayed depth", 10, 140, 0.0, 1.0, 0.0, "key_z_t");
00079     viz.addText("d -> Toggle Point/Cube representation", 10, 125, 0.0, 1.0, 0.0, "key_d_t");
00080     viz.addText("x -> Show/Hide original cloud", 10, 110, 0.0, 1.0, 0.0, "key_x_t");
00081     viz.addText("s/w -> Surface/Wireframe representation", 10, 95, 0.0, 1.0, 0.0, "key_sw_t");
00082 
00083     //set current level to half the maximum one
00084     displayedDepth = static_cast<int> (floor (octree.getTreeDepth() / 2.0));
00085     if (displayedDepth == 0)
00086       displayedDepth = 1;
00087 
00088     //show octree at default depth
00089     extractPointsAtLevel(displayedDepth);
00090 
00091     //reset camera
00092     viz.resetCameraViewpoint("cloud");
00093 
00094     //run main loop
00095     run();
00096 
00097   }
00098 
00099 private:
00100   //========================================================
00101   // PRIVATE ATTRIBUTES
00102   //========================================================
00103   //visualizer
00104   pcl::PointCloud<pcl::PointXYZ>::Ptr xyz;
00105   pcl::PointCloud<pcl::PointXYZRGB>::Ptr xyz_rgb;
00106 
00107   pcl::visualization::PCLVisualizer viz;
00108   //original cloud
00109   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
00110   //displayed_cloud
00111   pcl::PointCloud<pcl::PointXYZ>::Ptr displayCloud;
00112   //octree
00113   pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ> octree;
00114   //level
00115   int displayedDepth;
00116   //bool to decide if we display points or cubes
00117   bool displayCubes, showPointsWithCubes, wireframe;
00118   //========================================================
00119 
00120   /* \brief Callback to interact with the keyboard
00121    *
00122    */
00123   void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *)
00124   {
00125 
00126     if (event.getKeySym() == "a" && event.keyDown())
00127     {
00128       IncrementLevel();
00129     }
00130     else if (event.getKeySym() == "z" && event.keyDown())
00131     {
00132       DecrementLevel();
00133     }
00134     else if (event.getKeySym() == "d" && event.keyDown())
00135     {
00136       displayCubes = !displayCubes;
00137       update();
00138     }
00139     else if (event.getKeySym() == "x" && event.keyDown())
00140     {
00141       showPointsWithCubes = !showPointsWithCubes;
00142       update();
00143     }
00144     else if (event.getKeySym() == "w" && event.keyDown())
00145     {
00146       if(!wireframe)
00147         wireframe=true;
00148       update();
00149     }
00150     else if (event.getKeySym() == "s" && event.keyDown())
00151     {
00152       if(wireframe)
00153         wireframe=false;
00154       update();
00155     }
00156   }
00157 
00158   /* \brief Graphic loop for the viewer
00159    *
00160    */
00161   void run()
00162   {
00163     while (!viz.wasStopped())
00164     {
00165       //main loop of the visualizer
00166       viz.spinOnce(100);
00167       boost::this_thread::sleep(boost::posix_time::microseconds(100000));
00168     }
00169   }
00170 
00171   /* \brief Helper function that read a pointcloud file (returns false if pbl)
00172    *  Also initialize the octree
00173    *
00174    */
00175   bool loadCloud(std::string &filename)
00176   {
00177     std::cout << "Loading file " << filename.c_str() << std::endl;
00178     //read cloud
00179     if (pcl::io::loadPCDFile(filename, *cloud))
00180     {
00181       std::cerr << "ERROR: Cannot open file " << filename << "! Aborting..." << std::endl;
00182       return false;
00183     }
00184 
00185     //remove NaN Points
00186     std::vector<int> nanIndexes;
00187     pcl::removeNaNFromPointCloud(*cloud, *cloud, nanIndexes);
00188     std::cout << "Loaded " << cloud->points.size() << " points" << std::endl;
00189 
00190     //create octree structure
00191     octree.setInputCloud(cloud);
00192     //update bounding box automatically
00193     octree.defineBoundingBox();
00194     //add points in the tree
00195     octree.addPointsFromInputCloud();
00196     return true;
00197   }
00198 
00199   /* \brief Helper function that draw info for the user on the viewer
00200    *
00201    */
00202   void showLegend(bool showCubes)
00203   {
00204     char dataDisplay[256];
00205     sprintf(dataDisplay, "Displaying data as %s", (showCubes) ? ("CUBES") : ("POINTS"));
00206     viz.removeShape("disp_t");
00207     viz.addText(dataDisplay, 0, 60, 1.0, 0.0, 0.0, "disp_t");
00208 
00209     char level[256];
00210     sprintf(level, "Displayed depth is %d on %d", displayedDepth, octree.getTreeDepth());
00211     viz.removeShape("level_t1");
00212     viz.addText(level, 0, 45, 1.0, 0.0, 0.0, "level_t1");
00213 
00214     viz.removeShape("level_t2");
00215     sprintf(level, "Voxel size: %.4fm [%zu voxels]", sqrt(octree.getVoxelSquaredSideLen(displayedDepth)),
00216             displayCloud->points.size());
00217     viz.addText(level, 0, 30, 1.0, 0.0, 0.0, "level_t2");
00218 
00219     viz.removeShape("org_t");
00220     if (showPointsWithCubes)
00221       viz.addText("Displaying original cloud", 0, 15, 1.0, 0.0, 0.0, "org_t");
00222   }
00223 
00224   /* \brief Visual update. Create visualizations and add them to the viewer
00225    *
00226    */
00227   void update()
00228   {
00229     //remove existing shapes from visualizer
00230     clearView();
00231 
00232     //prevent the display of too many cubes
00233     bool displayCubeLegend = displayCubes && static_cast<int> (displayCloud->points.size ()) <= MAX_DISPLAYED_CUBES;
00234 
00235     showLegend(displayCubeLegend);
00236 
00237     if (displayCubeLegend)
00238     {
00239       //show octree as cubes
00240       showCubes(sqrt(octree.getVoxelSquaredSideLen(displayedDepth)));
00241       if (showPointsWithCubes)
00242       {
00243         //add original cloud in visualizer
00244         pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(cloud, "z");
00245         viz.addPointCloud(cloud, color_handler, "cloud");
00246       }
00247     }
00248     else
00249     {
00250       //add current cloud in visualizer
00251       pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(displayCloud,"z");
00252       viz.addPointCloud(displayCloud, color_handler, "cloud");
00253     }
00254   }
00255 
00256   /* \brief remove dynamic objects from the viewer
00257    *
00258    */
00259   void clearView()
00260   {
00261     //remove cubes if any
00262     vtkRenderer *renderer = viz.getRenderWindow()->GetRenderers()->GetFirstRenderer();
00263     while (renderer->GetActors()->GetNumberOfItems() > 0)
00264       renderer->RemoveActor(renderer->GetActors()->GetLastActor());
00265     //remove point clouds if any
00266     viz.removePointCloud("cloud");
00267   }
00268 
00269   /* \brief Create a vtkSmartPointer object containing a cube
00270    *
00271    */
00272   vtkSmartPointer<vtkPolyData> GetCuboid(double minX, double maxX, double minY, double maxY, double minZ, double maxZ)
00273   {
00274     vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New();
00275     cube->SetBounds(minX, maxX, minY, maxY, minZ, maxZ);
00276     return cube->GetOutput();
00277   }
00278 
00279   /* \brief display octree cubes via vtk-functions
00280    *
00281    */
00282   void showCubes(double voxelSideLen)
00283   {
00284     //get the renderer of the visualizer object
00285     vtkRenderer *renderer = viz.getRenderWindow()->GetRenderers()->GetFirstRenderer();
00286 
00287     vtkSmartPointer<vtkAppendPolyData> treeWireframe = vtkSmartPointer<vtkAppendPolyData>::New();
00288     size_t i;
00289     double s = voxelSideLen / 2.0;
00290     for (i = 0; i < displayCloud->points.size(); i++)
00291     {
00292 
00293       double x = displayCloud->points[i].x;
00294       double y = displayCloud->points[i].y;
00295       double z = displayCloud->points[i].z;
00296 
00297       treeWireframe->AddInput(GetCuboid(x - s, x + s, y - s, y + s, z - s, z + s));
00298     }
00299 
00300     vtkSmartPointer<vtkActor> treeActor = vtkSmartPointer<vtkActor>::New();
00301 
00302     vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New();
00303     mapper->SetInput(treeWireframe->GetOutput());
00304     treeActor->SetMapper(mapper);
00305 
00306     treeActor->GetProperty()->SetColor(1.0, 1.0, 1.0);
00307     treeActor->GetProperty()->SetLineWidth(2);
00308     if(wireframe)
00309     {
00310       treeActor->GetProperty()->SetRepresentationToWireframe();
00311       treeActor->GetProperty()->SetOpacity(0.35);
00312     }
00313     else
00314       treeActor->GetProperty()->SetRepresentationToSurface();
00315 
00316     renderer->AddActor(treeActor);
00317   }
00318 
00319   /* \brief Extracts all the points of depth = level from the octree
00320    *
00321    */
00322   void extractPointsAtLevel(int depth)
00323   {
00324     displayCloud->points.clear();
00325 
00326     pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::Iterator tree_it(octree);
00327 
00328     pcl::PointXYZ pt;
00329     std::cout << "===== Extracting data at depth " << depth << "... " << std::flush;
00330     double start = omp_get_wtime();
00331     while (*tree_it++)
00332     {
00333       if (static_cast<int> (tree_it.getCurrentOctreeDepth ()) != depth)
00334         continue;
00335 
00336       Eigen::Vector3f voxel_min, voxel_max;
00337       octree.getVoxelBounds(tree_it, voxel_min, voxel_max);
00338 
00339       pt.x = (voxel_min.x() + voxel_max.x()) / 2.0f;
00340       pt.y = (voxel_min.y() + voxel_max.y()) / 2.0f;
00341       pt.z = (voxel_min.z() + voxel_max.z()) / 2.0f;
00342       displayCloud->points.push_back(pt);
00343 
00344       //we are already the desired depth, there is no reason to go deeper.
00345       tree_it.skipChildVoxels();
00346     }
00347 
00348     double end = omp_get_wtime();
00349     printf("%zu pts, %.4gs. %.4gs./pt. =====\n", displayCloud->points.size (), end - start,
00350            (end - start) / static_cast<double> (displayCloud->points.size ()));
00351 
00352     update();
00353   }
00354 
00355   /* \brief Helper function to increase the octree display level by one
00356    *
00357    */
00358   bool IncrementLevel()
00359   {
00360     if (displayedDepth < static_cast<int> (octree.getTreeDepth ()))
00361     {
00362       displayedDepth++;
00363       extractPointsAtLevel(displayedDepth);
00364       return true;
00365     }
00366     else
00367       return false;
00368   }
00369 
00370   /* \brief Helper function to decrease the octree display level by one
00371    *
00372    */
00373   bool DecrementLevel()
00374   {
00375     if (displayedDepth > 0)
00376     {
00377       displayedDepth--;
00378       extractPointsAtLevel(displayedDepth);
00379       return true;
00380     }
00381     return false;
00382   }
00383 
00384 };
00385 
00386 int main(int argc, char ** argv)
00387 {
00388   if (argc != 3)
00389   {
00390     std::cerr << "ERROR: Syntax is octreeVisu <pcd file> <resolution>" << std::endl;
00391     std::cerr << "EXAMPLE: ./octreeVisu bun0.pcd 0.001" << std::endl;
00392     return -1;
00393   }
00394 
00395   std::string cloud_path(argv[1]);
00396   OctreeViewer v(cloud_path, atof(argv[2]));
00397 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:58