pcl_visualization: pcl_visualization: pcl

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  * $Id: pcl_visualizer.h 36441 2011-03-18 17:49:42Z ethanrublee $
00035  *
00036  */
00037 #ifndef PCL_PCL_VISUALIZER_H_
00038 #define PCL_PCL_VISUALIZER_H_
00039 
00040 // PCL includes
00041 #include <pcl/point_types.h>
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/PolygonMesh.h>
00044 // 
00045 #include <terminal_tools/print.h>
00046 #include <pcl_visualization/interactor.h>
00047 #include <pcl_visualization/interactor_style.h>
00048 #include <pcl_visualization/common/common.h>
00049 #include <pcl_visualization/common/shapes.h>
00050 #include <boost/make_shared.hpp>
00051 #include <boost/algorithm/string/split.hpp>
00052 #include <boost/algorithm/string/classification.hpp>
00053 // VTK includes
00054 #include <vtkAxes.h>
00055 #include <vtkFloatArray.h>
00056 #include <vtkAppendPolyData.h>
00057 #include <vtkPointData.h>
00058 #include <vtkTubeFilter.h>
00059 #include <vtkPolyDataMapper.h>
00060 #include <vtkDataSetMapper.h>
00061 #include <vtkCellArray.h>
00062 #include <vtkCommand.h>
00063 #include <vtkPLYReader.h>
00064 #include <vtkTransformFilter.h>
00065 
00066 namespace pcl_visualization
00067 {
00071   class PCLVisualizer
00072   {
00073     public:
00074       typedef PointCloudGeometryHandler<sensor_msgs::PointCloud2> GeometryHandler;
00075       typedef GeometryHandler::Ptr GeometryHandlerPtr;
00076       typedef GeometryHandler::ConstPtr GeometryHandlerConstPtr;
00077 
00078       typedef PointCloudColorHandler<sensor_msgs::PointCloud2> ColorHandler;
00079       typedef ColorHandler::Ptr ColorHandlerPtr;
00080       typedef ColorHandler::ConstPtr ColorHandlerConstPtr;
00081 
00085       PCLVisualizer (const std::string &name = "");
00092       PCLVisualizer (int &argc, char **argv, const std::string &name = "", PCLVisualizerInteractorStyle* style = PCLVisualizerInteractorStyle::New ());
00093 
00095       virtual ~PCLVisualizer ();
00096 
00098       void spin ();
00099       
00105       void spinOnce (int time = 1, bool force_redraw = false);
00106 
00111       void addCoordinateSystem (double scale = 1.0, int viewport = 0);
00119       void addCoordinateSystem (double scale, float x, float y, float z, int viewport = 0);
00120 
00124       void removeCoordinateSystem (int viewport = 0);
00125 
00130       bool removePointCloud (const std::string &id = "cloud", int viewport = 0);
00131 
00136       bool removeShape (const std::string &id = "cloud", int viewport = 0);
00137 
00144       void setBackgroundColor (const double &r, const double &g, const double &b, int viewport = 0);
00145 
00153       bool addText (const std::string &text, int xpos, int ypos, const std::string &id = "", int viewport = 0);
00154 
00165       bool addText (const std::string &text, int xpos, int ypos, int r, int g, int b, const std::string &id = "", int viewport = 0);
00166 
00174       template <typename PointNT>
00175       bool addPointCloudNormals (const pcl::PointCloud<PointNT> &cloud,
00176                                  int level = 100, double scale = 0.02,
00177                                  const std::string &id = "cloud", int viewport = 0);
00186       template <typename PointT, typename PointNT>
00187       bool addPointCloudNormals (const pcl::PointCloud<PointT> &cloud,
00188                                  const pcl::PointCloud<PointNT> &normals,
00189                                  int level = 100, double scale = 0.02,
00190                                  const std::string &id = "cloud", int viewport = 0);
00200       bool addPointCloudPrincipalCurvatures (const pcl::PointCloud<pcl::PointXYZ> &cloud, 
00201                                              const pcl::PointCloud<pcl::Normal> &normals,
00202                                              const pcl::PointCloud<pcl::PrincipalCurvatures> &pcs,
00203                                              int level = 100, double scale = 1.0,
00204                                              const std::string &id = "cloud", int viewport = 0);
00205 
00211       bool addPointCloud (const pcl::PointCloud<pcl::PointXYZ> &cloud, 
00212                           const std::string &id = "cloud", int viewport = 0);
00213 
00219       template <typename PointT> bool 
00220         addPointCloud (const pcl::PointCloud<PointT> &cloud, 
00221                        const std::string &id = "cloud", int viewport = 0);
00222 
00229       template <typename PointT> bool 
00230         addPointCloud (const pcl::PointCloud<PointT> &cloud, 
00231                        const PointCloudGeometryHandler<PointT> &geometry_handler,
00232                        const std::string &id = "cloud", int viewport = 0);
00233 
00240       template <typename PointT> bool 
00241         addPointCloud (const pcl::PointCloud<PointT> &cloud, 
00242                        const GeometryHandlerConstPtr &geometry_handler,
00243                        const std::string &id = "cloud", int viewport = 0);
00244 
00251       template <typename PointT> bool 
00252         addPointCloud (const pcl::PointCloud<PointT> &cloud, 
00253                        const PointCloudColorHandler<PointT> &color_handler,
00254                        const std::string &id = "cloud", int viewport = 0);
00255 
00262       template <typename PointT> bool 
00263         addPointCloud (const pcl::PointCloud<PointT> &cloud, 
00264                        const ColorHandlerConstPtr &color_handler,
00265                        const std::string &id = "cloud", int viewport = 0);
00266 
00274       template <typename PointT> bool 
00275         addPointCloud (const pcl::PointCloud<PointT> &cloud, 
00276                        const GeometryHandlerConstPtr &geometry_handler,
00277                        const ColorHandlerConstPtr &color_handler,
00278                        const std::string &id = "cloud", int viewport = 0);
00279 
00287       template <typename PointT> bool 
00288         addPointCloud (const pcl::PointCloud<PointT> &cloud, 
00289                        const PointCloudColorHandler<PointT> &color_handler,
00290                        const PointCloudGeometryHandler<PointT> &geometry_handler,
00291                        const std::string &id = "cloud", int viewport = 0);
00292 
00296       inline int getColorHandlerIndex (const std::string &id)
00297       {
00298         CloudActorMap::iterator am_it = style_->getCloudActorMap ()->find (id);
00299         if (am_it == cloud_actor_map_.end ())
00300           return (-1);
00301 
00302         return (am_it->second.color_handler_index_);
00303       }
00307       inline int getGeometryHandlerIndex (const std::string &id)
00308       {
00309         CloudActorMap::iterator am_it = style_->getCloudActorMap ()->find (id);
00310         if (am_it != cloud_actor_map_.end ())
00311           return (-1);
00312 
00313         return (am_it->second.geometry_handler_index_);
00314       }
00315 
00320       bool updateColorHandlerIndex (const std::string &id, int index);
00321 
00330       bool setPointCloudRenderingProperties (int property, double val1, double val2, double val3, const std::string &id = "cloud", int viewport = 0);
00331 
00338       bool setPointCloudRenderingProperties (int property, double value, const std::string &id = "cloud", int viewport = 0);
00339 
00345       bool getPointCloudRenderingProperties (int property, double &value, const std::string &id = "cloud");
00346       
00353       bool setShapeRenderingProperties (int property, double value, const std::string &id, int viewport = 0);
00354 
00356       bool wasStopped () const { return (interactor_->stopped); }
00358       void resetStoppedFlag () { interactor_->stopped = false; }
00359 
00367       void createViewPort (double xmin, double ymin, double xmax, double ymax, int &viewport);
00368 
00378       template <typename PointT> bool
00379         addPolygon (const pcl::PointCloud<PointT> &cloud, double r, double g, double b, const std::string &id = "polygon", int viewport = 0);
00380 
00387       template <typename PointT> bool
00388         addPolygon (const pcl::PointCloud<PointT> &cloud, const std::string &id = "polygon", int viewport = 0);
00389 
00396       template <typename P1, typename P2> bool addLine (const P1 &pt1, const P2 &pt2, const std::string &id = "line", int viewport = 0);
00397 
00407       template <typename P1, typename P2> bool addLine (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id = "line", int viewport = 0);
00408 
00415 /*      template <typename P1, typename P2> inline bool addLineToGroup (const P1 &pt1, const P2 &pt2, const std::string &group_id = "line_group", int viewport = 0);*/
00416       
00423       template <typename PointT> bool addSphere (const PointT &center, double radius, const std::string &id = "sphere", int viewport = 0);
00424 
00434       template <typename PointT> bool addSphere (const PointT &center, double radius, double r, double g, double b, const std::string &id = "sphere", int viewport = 0);
00435 
00441       bool addPolygonMesh (const pcl::PolygonMesh & polyMesh, const std::string & id,
00442                                    int viewport);
00448       bool addModelFromPLYFile (const std::string & filename, const std::string & id = "PLYModel", int viewport = 0);
00449 
00457       bool addModelFromPLYFile (const std::string & filename, vtkSmartPointer<vtkTransform> transform, const std::string & id =
00458           "PLYModel", int viewport = 0);
00459 
00465       bool addCylinder (const pcl::ModelCoefficients &coefficients, const std::string &id = "cylinder", int viewport = 0);
00471       bool addSphere (const pcl::ModelCoefficients &coefficients, const std::string &id = "sphere", int viewport = 0);
00477       bool addLine (const pcl::ModelCoefficients &coefficients, const std::string &id = "line", int viewport = 0);
00483       bool addPlane (const pcl::ModelCoefficients &coefficients, const std::string &id = "plane", int viewport = 0);
00489       bool addCircle (const pcl::ModelCoefficients &coefficients, const std::string &id = "circle", int viewport = 0);
00495       bool addCone (const pcl::ModelCoefficients &coefficients, const std::string &id = "cone", int viewport = 0);
00496 
00498       Camera camera_;
00499 
00501       void initCameraParameters ();
00502 
00507       bool getCameraParameters (int argc, char **argv);
00508 
00510       void updateCamera ();
00511 
00513       void resetCamera ();
00514 
00515     protected:
00517       vtkSmartPointer<PCLVisualizerInteractor> interactor_;
00518 
00519     private:
00520       struct ExitMainLoopTimerCallback : public vtkCommand
00521       {
00522         static ExitMainLoopTimerCallback* New()
00523         {
00524           return new ExitMainLoopTimerCallback;
00525         }
00526         virtual void Execute(vtkObject* vtkNotUsed(caller), unsigned long event_id, void* call_data)
00527         {
00528           if (event_id != vtkCommand::TimerEvent)
00529             return;
00530           int timer_id = *(int*)call_data;
00531           //cout << "Timer "<<timer_id<<" called.\n";
00532           if (timer_id != right_timer_id)
00533             return;
00534           // Stop vtk loop and send notification to app to wake it up
00535           pcl_visualizer->interactor_->stopLoop ();
00536         }
00537         int right_timer_id;
00538         PCLVisualizer* pcl_visualizer;
00539       };
00540       struct ExitCallback : public vtkCommand
00541       {
00542         static ExitCallback* New ()
00543         {
00544           return new ExitCallback;
00545         }
00546         virtual void Execute (vtkObject* caller, unsigned long event_id, void* call_data)
00547         {
00548           if (event_id != vtkCommand::ExitEvent)
00549             return;
00550           //cout << "Exit event called.\n";
00551           pcl_visualizer->interactor_->stopped = true;
00552         }
00553         PCLVisualizer* pcl_visualizer;
00554       };
00555 
00556       
00558       vtkSmartPointer<ExitMainLoopTimerCallback> exit_main_loop_timer_callback_;
00559       vtkSmartPointer<ExitCallback> exit_callback_;
00560 
00562       vtkSmartPointer<vtkRendererCollection> rens_;
00564       vtkSmartPointer<vtkRenderWindow> win_;
00565 
00567       vtkSmartPointer<PCLVisualizerInteractorStyle> style_;
00568 
00570       CloudActorMap cloud_actor_map_;
00571 
00573       ShapeActorMap shape_actor_map_;
00574 
00575     
00580       void removeActorFromRenderer (const vtkSmartPointer<vtkLODActor> &actor, int viewport = 0);
00581 
00586       void addActorToRenderer (const vtkSmartPointer<vtkProp> &actor, int viewport = 0);
00587 
00592       void removeActorFromRenderer (const vtkSmartPointer<vtkProp> &actor, int viewport = 0);
00593 
00598       void createActorFromVTKDataSet (const vtkSmartPointer<vtkDataSet> &data, vtkSmartPointer<vtkLODActor> &actor);
00599 
00604       void convertPointCloudToVTKPolyData (const pcl::PointCloud<pcl::PointXYZ> &cloud, vtkSmartPointer<vtkPolyData> &polydata);
00605 
00610       template <typename PointT> void 
00611       convertPointCloudToVTKPolyData (const pcl::PointCloud<PointT> &cloud, vtkSmartPointer<vtkPolyData> &polydata);
00612 
00617       template <typename PointT> void 
00618         convertPointCloudToVTKPolyData (const PointCloudGeometryHandler<PointT> &geometry_handler, vtkSmartPointer<vtkPolyData> &polydata);
00619 
00624       void convertPointCloudToVTKPolyData (const GeometryHandlerConstPtr &geometry_handler, vtkSmartPointer<vtkPolyData> &polydata);
00625   };
00626 }
00627 
00628 #include "libpcl_visualization/pcl_visualizer.hpp"
00629 
00630 #endif