|
Point Cloud Library (PCL)
1.4.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, 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: point_cloud_handlers.h 3771 2012-01-01 06:58:14Z rusu $ 00037 * 00038 */ 00039 #ifndef PCL_POINT_CLOUD_HANDLERS_H_ 00040 #define PCL_POINT_CLOUD_HANDLERS_H_ 00041 00042 #include <pcl/visualization/common/common.h> 00043 // PCL includes 00044 #include <pcl/point_types.h> 00045 #include <pcl/point_cloud.h> 00046 #include <pcl/common/io.h> 00047 // VTK includes 00048 #include <vtkSmartPointer.h> 00049 #include <vtkDataArray.h> 00050 #include <vtkUnsignedCharArray.h> 00051 #include <vtkFloatArray.h> 00052 #include <vtkPoints.h> 00053 00054 namespace pcl 00055 { 00056 namespace visualization 00057 { 00061 template <typename PointT> 00062 class PointCloudGeometryHandler 00063 { 00064 public: 00065 typedef pcl::PointCloud<PointT> PointCloud; 00066 typedef typename PointCloud::Ptr PointCloudPtr; 00067 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00068 00069 typedef typename boost::shared_ptr<PointCloudGeometryHandler<PointT> > Ptr; 00070 typedef typename boost::shared_ptr<const PointCloudGeometryHandler<PointT> > ConstPtr; 00071 00073 PointCloudGeometryHandler (const PointCloudConstPtr &cloud) : 00074 cloud_ (cloud), capable_ (false) 00075 {} 00076 00080 virtual std::string 00081 getName () const = 0; 00082 00084 virtual std::string 00085 getFieldName () const = 0; 00086 00088 inline bool 00089 isCapable () const { return (capable_); } 00090 00094 virtual void 00095 getGeometry (vtkSmartPointer<vtkPoints> &points) const = 0; 00096 00098 virtual Eigen::Vector4f 00099 getOrigin () const { return (cloud_->sensor_origin_); } 00100 00102 virtual Eigen::Quaternion<float> 00103 getOrientation () const { return (cloud_->sensor_orientation_); } 00104 00105 protected: 00107 PointCloudConstPtr cloud_; 00108 00112 bool capable_; 00113 00115 int field_x_idx_; 00116 00118 int field_y_idx_; 00119 00121 int field_z_idx_; 00122 00124 std::vector<sensor_msgs::PointField> fields_; 00125 }; 00126 00128 00132 template <typename PointT> 00133 class PointCloudGeometryHandlerXYZ : public PointCloudGeometryHandler<PointT> 00134 { 00135 public: 00136 typedef typename PointCloudGeometryHandler<PointT>::PointCloud PointCloud; 00137 typedef typename PointCloud::Ptr PointCloudPtr; 00138 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00139 00140 typedef typename boost::shared_ptr<PointCloudGeometryHandlerXYZ<PointT> > Ptr; 00141 typedef typename boost::shared_ptr<const PointCloudGeometryHandlerXYZ<PointT> > ConstPtr; 00142 00144 PointCloudGeometryHandlerXYZ (const PointCloudConstPtr &cloud); 00145 00147 virtual inline std::string 00148 getName () const { return ("PointCloudGeometryHandlerXYZ"); } 00149 00151 virtual std::string 00152 getFieldName () const { return ("xyz"); } 00153 00157 virtual void 00158 getGeometry (vtkSmartPointer<vtkPoints> &points) const; 00159 00160 private: 00161 // Members derived from the base class 00162 using PointCloudGeometryHandler<PointT>::cloud_; 00163 using PointCloudGeometryHandler<PointT>::capable_; 00164 using PointCloudGeometryHandler<PointT>::field_x_idx_; 00165 using PointCloudGeometryHandler<PointT>::field_y_idx_; 00166 using PointCloudGeometryHandler<PointT>::field_z_idx_; 00167 using PointCloudGeometryHandler<PointT>::fields_; 00168 }; 00169 00171 00176 template <typename PointT> 00177 class PointCloudGeometryHandlerSurfaceNormal : public PointCloudGeometryHandler<PointT> 00178 { 00179 public: 00180 typedef typename PointCloudGeometryHandler<PointT>::PointCloud PointCloud; 00181 typedef typename PointCloud::Ptr PointCloudPtr; 00182 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00183 00184 typedef typename boost::shared_ptr<PointCloudGeometryHandlerSurfaceNormal<PointT> > Ptr; 00185 typedef typename boost::shared_ptr<const PointCloudGeometryHandlerSurfaceNormal<PointT> > ConstPtr; 00186 00188 PointCloudGeometryHandlerSurfaceNormal (const PointCloudConstPtr &cloud); 00189 00191 virtual inline std::string 00192 getName () const { return ("PointCloudGeometryHandlerSurfaceNormal"); } 00193 00195 virtual std::string 00196 getFieldName () const { return ("normal_xyz"); } 00197 00201 virtual void 00202 getGeometry (vtkSmartPointer<vtkPoints> &points) const; 00203 00204 private: 00205 // Members derived from the base class 00206 using PointCloudGeometryHandler<PointT>::cloud_; 00207 using PointCloudGeometryHandler<PointT>::capable_; 00208 using PointCloudGeometryHandler<PointT>::field_x_idx_; 00209 using PointCloudGeometryHandler<PointT>::field_y_idx_; 00210 using PointCloudGeometryHandler<PointT>::field_z_idx_; 00211 using PointCloudGeometryHandler<PointT>::fields_; 00212 }; 00213 00215 00220 template <typename PointT> 00221 class PointCloudGeometryHandlerCustom : public PointCloudGeometryHandler<PointT> 00222 { 00223 public: 00224 typedef typename PointCloudGeometryHandler<PointT>::PointCloud PointCloud; 00225 typedef typename PointCloud::Ptr PointCloudPtr; 00226 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00227 00228 typedef typename boost::shared_ptr<PointCloudGeometryHandlerCustom<PointT> > Ptr; 00229 typedef typename boost::shared_ptr<const PointCloudGeometryHandlerCustom<PointT> > ConstPtr; 00230 00232 PointCloudGeometryHandlerCustom (const PointCloudConstPtr &cloud, 00233 const std::string &x_field_name, 00234 const std::string &y_field_name, 00235 const std::string &z_field_name); 00236 00238 virtual inline std::string 00239 getName () const { return ("PointCloudGeometryHandlerCustom"); } 00240 00242 virtual std::string 00243 getFieldName () const { return (field_name_); } 00244 00248 virtual void 00249 getGeometry (vtkSmartPointer<vtkPoints> &points) const; 00250 00251 private: 00252 // Members derived from the base class 00253 using PointCloudGeometryHandler<PointT>::cloud_; 00254 using PointCloudGeometryHandler<PointT>::capable_; 00255 using PointCloudGeometryHandler<PointT>::field_x_idx_; 00256 using PointCloudGeometryHandler<PointT>::field_y_idx_; 00257 using PointCloudGeometryHandler<PointT>::field_z_idx_; 00258 using PointCloudGeometryHandler<PointT>::fields_; 00259 00261 std::string field_name_; 00262 }; 00263 00265 00268 template <> 00269 class PCL_EXPORTS PointCloudGeometryHandler<sensor_msgs::PointCloud2> 00270 { 00271 public: 00272 typedef sensor_msgs::PointCloud2 PointCloud; 00273 typedef PointCloud::Ptr PointCloudPtr; 00274 typedef PointCloud::ConstPtr PointCloudConstPtr; 00275 00276 typedef boost::shared_ptr<PointCloudGeometryHandler<PointCloud> > Ptr; 00277 typedef boost::shared_ptr<const PointCloudGeometryHandler<PointCloud> > ConstPtr; 00278 00280 PointCloudGeometryHandler (const PointCloudConstPtr &cloud) : 00281 cloud_ (cloud), capable_ (false) 00282 { 00283 fields_ = cloud_->fields; 00284 } 00285 00287 virtual std::string 00288 getName () const = 0; 00289 00291 virtual std::string 00292 getFieldName () const = 0; 00293 00295 inline bool 00296 isCapable () const { return (capable_); } 00297 00301 virtual void 00302 getGeometry (vtkSmartPointer<vtkPoints> &points) const; 00303 00304 protected: 00306 PointCloudConstPtr cloud_; 00307 00311 bool capable_; 00312 00314 int field_x_idx_; 00315 00317 int field_y_idx_; 00318 00320 int field_z_idx_; 00321 00323 std::vector<sensor_msgs::PointField> fields_; 00324 }; 00325 00327 00331 template <> 00332 class PCL_EXPORTS PointCloudGeometryHandlerXYZ<sensor_msgs::PointCloud2> : public PointCloudGeometryHandler<sensor_msgs::PointCloud2> 00333 { 00334 public: 00335 typedef PointCloudGeometryHandler<sensor_msgs::PointCloud2>::PointCloud PointCloud; 00336 typedef PointCloud::Ptr PointCloudPtr; 00337 typedef PointCloud::ConstPtr PointCloudConstPtr; 00338 00339 typedef boost::shared_ptr<PointCloudGeometryHandlerXYZ<PointCloud> > Ptr; 00340 typedef boost::shared_ptr<const PointCloudGeometryHandlerXYZ<PointCloud> > ConstPtr; 00341 00343 PointCloudGeometryHandlerXYZ (const PointCloudConstPtr &cloud); 00344 00346 virtual inline 00347 std::string getName () const { return ("PointCloudGeometryHandlerXYZ"); } 00348 00350 virtual std::string 00351 getFieldName () const { return ("xyz"); } 00352 }; 00353 00355 00360 template <> 00361 class PCL_EXPORTS PointCloudGeometryHandlerSurfaceNormal<sensor_msgs::PointCloud2> : public PointCloudGeometryHandler<sensor_msgs::PointCloud2> 00362 { 00363 public: 00364 typedef PointCloudGeometryHandler<sensor_msgs::PointCloud2>::PointCloud PointCloud; 00365 typedef PointCloud::Ptr PointCloudPtr; 00366 typedef PointCloud::ConstPtr PointCloudConstPtr; 00367 00368 typedef boost::shared_ptr<PointCloudGeometryHandlerSurfaceNormal<PointCloud> > Ptr; 00369 typedef boost::shared_ptr<const PointCloudGeometryHandlerSurfaceNormal<PointCloud> > ConstPtr; 00370 00372 PointCloudGeometryHandlerSurfaceNormal (const PointCloudConstPtr &cloud); 00373 00375 virtual inline std::string 00376 getName () const { return ("PointCloudGeometryHandlerSurfaceNormal"); } 00377 00379 virtual std::string 00380 getFieldName () const { return ("normal_xyz"); } 00381 }; 00382 00384 00389 template <> 00390 class PCL_EXPORTS PointCloudGeometryHandlerCustom<sensor_msgs::PointCloud2> : public PointCloudGeometryHandler<sensor_msgs::PointCloud2> 00391 { 00392 public: 00393 typedef PointCloudGeometryHandler<sensor_msgs::PointCloud2>::PointCloud PointCloud; 00394 typedef PointCloud::Ptr PointCloudPtr; 00395 typedef PointCloud::ConstPtr PointCloudConstPtr; 00396 00398 PointCloudGeometryHandlerCustom (const PointCloudConstPtr &cloud, 00399 const std::string &x_field_name, 00400 const std::string &y_field_name, 00401 const std::string &z_field_name); 00402 00404 virtual inline std::string 00405 getName () const { return ("PointCloudGeometryHandlerCustom"); } 00406 00408 virtual std::string 00409 getFieldName () const { return (field_name_); } 00410 00411 private: 00413 std::string field_name_; 00414 }; 00415 00417 00420 template <typename PointT> 00421 class PointCloudColorHandler 00422 { 00423 public: 00424 typedef pcl::PointCloud<PointT> PointCloud; 00425 typedef typename PointCloud::Ptr PointCloudPtr; 00426 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00427 00428 typedef boost::shared_ptr<PointCloudColorHandler<PointT> > Ptr; 00429 typedef boost::shared_ptr<const PointCloudColorHandler<PointT> > ConstPtr; 00430 00432 PointCloudColorHandler (const PointCloudConstPtr &cloud) : 00433 cloud_ (cloud), capable_ (false) 00434 {} 00435 00437 inline bool 00438 isCapable () const { return (capable_); } 00439 00441 virtual std::string 00442 getName () const = 0; 00443 00445 virtual std::string 00446 getFieldName () const = 0; 00447 00451 virtual void 00452 getColor (vtkSmartPointer<vtkDataArray> &scalars) const = 0; 00453 00454 protected: 00456 PointCloudConstPtr cloud_; 00457 00461 bool capable_; 00462 00464 int field_idx_; 00465 00467 std::vector<sensor_msgs::PointField> fields_; 00468 }; 00469 00471 00474 template <typename PointT> 00475 class PointCloudColorHandlerRandom : public PointCloudColorHandler<PointT> 00476 { 00477 typedef typename PointCloudColorHandler<PointT>::PointCloud PointCloud; 00478 typedef typename PointCloud::Ptr PointCloudPtr; 00479 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00480 00481 public: 00482 typedef boost::shared_ptr<PointCloudColorHandlerRandom<PointT> > Ptr; 00483 typedef boost::shared_ptr<const PointCloudColorHandlerRandom<PointT> > ConstPtr; 00484 00486 PointCloudColorHandlerRandom (const PointCloudConstPtr &cloud) : 00487 PointCloudColorHandler<PointT> (cloud) 00488 { 00489 capable_ = true; 00490 } 00491 00493 virtual inline std::string 00494 getName () const { return ("PointCloudColorHandlerRandom"); } 00495 00497 virtual std::string 00498 getFieldName () const { return ("[random]"); } 00499 00503 virtual void 00504 getColor (vtkSmartPointer<vtkDataArray> &scalars) const; 00505 00506 protected: 00507 // Members derived from the base class 00508 using PointCloudColorHandler<PointT>::cloud_; 00509 using PointCloudColorHandler<PointT>::capable_; 00510 }; 00511 00513 00517 template <typename PointT> 00518 class PointCloudColorHandlerCustom : public PointCloudColorHandler<PointT> 00519 { 00520 typedef typename PointCloudColorHandler<PointT>::PointCloud PointCloud; 00521 typedef typename PointCloud::Ptr PointCloudPtr; 00522 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00523 00524 public: 00525 typedef boost::shared_ptr<PointCloudColorHandlerCustom<PointT> > Ptr; 00526 typedef boost::shared_ptr<const PointCloudColorHandlerCustom<PointT> > ConstPtr; 00527 00529 PointCloudColorHandlerCustom (const PointCloudConstPtr &cloud, 00530 double r, double g, double b) 00531 : PointCloudColorHandler<PointT> (cloud) 00532 { 00533 capable_ = true; 00534 r_ = r; 00535 g_ = g; 00536 b_ = b; 00537 } 00538 00540 virtual inline std::string 00541 getName () const { return ("PointCloudColorHandlerCustom"); } 00542 00544 virtual std::string 00545 getFieldName () const { return (""); } 00546 00550 virtual void 00551 getColor (vtkSmartPointer<vtkDataArray> &scalars) const; 00552 00553 protected: 00554 // Members derived from the base class 00555 using PointCloudColorHandler<PointT>::cloud_; 00556 using PointCloudColorHandler<PointT>::capable_; 00557 00559 double r_, g_, b_; 00560 }; 00561 00563 00567 template <typename PointT> 00568 class PointCloudColorHandlerRGBField : public PointCloudColorHandler<PointT> 00569 { 00570 typedef typename PointCloudColorHandler<PointT>::PointCloud PointCloud; 00571 typedef typename PointCloud::Ptr PointCloudPtr; 00572 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00573 00574 public: 00575 typedef boost::shared_ptr<PointCloudColorHandlerRGBField<PointT> > Ptr; 00576 typedef boost::shared_ptr<const PointCloudColorHandlerRGBField<PointT> > ConstPtr; 00577 00579 PointCloudColorHandlerRGBField (const PointCloudConstPtr &cloud); 00580 00582 virtual std::string 00583 getFieldName () const { return ("rgb"); } 00584 00588 virtual void 00589 getColor (vtkSmartPointer<vtkDataArray> &scalars) const; 00590 00591 protected: 00593 virtual inline std::string 00594 getName () const { return ("PointCloudColorHandlerRGBField"); } 00595 00596 private: 00597 // Members derived from the base class 00598 using PointCloudColorHandler<PointT>::cloud_; 00599 using PointCloudColorHandler<PointT>::capable_; 00600 using PointCloudColorHandler<PointT>::field_idx_; 00601 using PointCloudColorHandler<PointT>::fields_; 00602 }; 00603 00605 00609 template <typename PointT> 00610 class PointCloudColorHandlerHSVField : public PointCloudColorHandler<PointT> 00611 { 00612 typedef typename PointCloudColorHandler<PointT>::PointCloud PointCloud; 00613 typedef typename PointCloud::Ptr PointCloudPtr; 00614 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00615 00616 public: 00617 typedef boost::shared_ptr<PointCloudColorHandlerHSVField<PointT> > Ptr; 00618 typedef boost::shared_ptr<const PointCloudColorHandlerHSVField<PointT> > ConstPtr; 00619 00621 PointCloudColorHandlerHSVField (const PointCloudConstPtr &cloud); 00622 00624 virtual std::string 00625 getFieldName () const { return ("hsv"); } 00626 00630 virtual void 00631 getColor (vtkSmartPointer<vtkDataArray> &scalars) const; 00632 00633 protected: 00635 virtual inline std::string 00636 getName () const { return ("PointCloudColorHandlerHSVField"); } 00637 00639 int s_field_idx_; 00640 00642 int v_field_idx_; 00643 private: 00644 // Members derived from the base class 00645 using PointCloudColorHandler<PointT>::cloud_; 00646 using PointCloudColorHandler<PointT>::capable_; 00647 using PointCloudColorHandler<PointT>::field_idx_; 00648 using PointCloudColorHandler<PointT>::fields_; 00649 }; 00650 00652 00656 template <typename PointT> 00657 class PointCloudColorHandlerGenericField : public PointCloudColorHandler<PointT> 00658 { 00659 typedef typename PointCloudColorHandler<PointT>::PointCloud PointCloud; 00660 typedef typename PointCloud::Ptr PointCloudPtr; 00661 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00662 00663 public: 00664 typedef boost::shared_ptr<PointCloudColorHandlerGenericField<PointT> > Ptr; 00665 typedef boost::shared_ptr<const PointCloudColorHandlerGenericField<PointT> > ConstPtr; 00666 00668 PointCloudColorHandlerGenericField (const PointCloudConstPtr &cloud, 00669 const std::string &field_name); 00670 00672 virtual std::string getFieldName () const { return (field_name_); } 00673 00677 virtual void 00678 getColor (vtkSmartPointer<vtkDataArray> &scalars) const; 00679 00680 protected: 00682 virtual inline std::string 00683 getName () const { return ("PointCloudColorHandlerGenericField"); } 00684 00685 private: 00686 using PointCloudColorHandler<PointT>::cloud_; 00687 using PointCloudColorHandler<PointT>::capable_; 00688 using PointCloudColorHandler<PointT>::field_idx_; 00689 using PointCloudColorHandler<PointT>::fields_; 00690 00692 std::string field_name_; 00693 }; 00694 00696 00699 template <> 00700 class PCL_EXPORTS PointCloudColorHandler<sensor_msgs::PointCloud2> 00701 { 00702 public: 00703 typedef sensor_msgs::PointCloud2 PointCloud; 00704 typedef PointCloud::Ptr PointCloudPtr; 00705 typedef PointCloud::ConstPtr PointCloudConstPtr; 00706 00707 typedef boost::shared_ptr<PointCloudColorHandler<PointCloud> > Ptr; 00708 typedef boost::shared_ptr<const PointCloudColorHandler<PointCloud> > ConstPtr; 00709 00711 PointCloudColorHandler (const PointCloudConstPtr &cloud) : 00712 cloud_ (cloud), capable_ (false) 00713 {} 00714 00716 inline bool 00717 isCapable () const { return (capable_); } 00718 00720 virtual std::string 00721 getName () const = 0; 00722 00724 virtual std::string 00725 getFieldName () const = 0; 00726 00730 virtual void 00731 getColor (vtkSmartPointer<vtkDataArray> &scalars) const = 0; 00732 00733 protected: 00735 PointCloudConstPtr cloud_; 00736 00740 bool capable_; 00741 00743 int field_idx_; 00744 }; 00745 00747 00750 template <> 00751 class PCL_EXPORTS PointCloudColorHandlerRandom<sensor_msgs::PointCloud2> : public PointCloudColorHandler<sensor_msgs::PointCloud2> 00752 { 00753 typedef PointCloudColorHandler<sensor_msgs::PointCloud2>::PointCloud PointCloud; 00754 typedef PointCloud::Ptr PointCloudPtr; 00755 typedef PointCloud::ConstPtr PointCloudConstPtr; 00756 00757 public: 00758 typedef boost::shared_ptr<PointCloudColorHandlerRandom<PointCloud> > Ptr; 00759 typedef boost::shared_ptr<const PointCloudColorHandlerRandom<PointCloud> > ConstPtr; 00760 00762 PointCloudColorHandlerRandom (const PointCloudConstPtr &cloud) : 00763 PointCloudColorHandler<sensor_msgs::PointCloud2> (cloud) 00764 { 00765 capable_ = true; 00766 } 00767 00769 virtual inline std::string 00770 getName () const { return ("PointCloudColorHandlerRandom"); } 00771 00773 virtual std::string 00774 getFieldName () const { return ("[random]"); } 00775 00779 virtual void 00780 getColor (vtkSmartPointer<vtkDataArray> &scalars) const; 00781 }; 00782 00784 00788 template <> 00789 class PCL_EXPORTS PointCloudColorHandlerCustom<sensor_msgs::PointCloud2> : public PointCloudColorHandler<sensor_msgs::PointCloud2> 00790 { 00791 typedef PointCloudColorHandler<sensor_msgs::PointCloud2>::PointCloud PointCloud; 00792 typedef PointCloud::Ptr PointCloudPtr; 00793 typedef PointCloud::ConstPtr PointCloudConstPtr; 00794 00795 public: 00797 PointCloudColorHandlerCustom (const PointCloudConstPtr &cloud, 00798 double r, double g, double b) : 00799 PointCloudColorHandler<sensor_msgs::PointCloud2> (cloud) 00800 { 00801 capable_ = true; 00802 r_ = r; 00803 g_ = g; 00804 b_ = b; 00805 } 00806 00808 virtual inline std::string 00809 getName () const { return ("PointCloudColorHandlerCustom"); } 00810 00812 virtual std::string 00813 getFieldName () const { return (""); } 00814 00818 virtual void 00819 getColor (vtkSmartPointer<vtkDataArray> &scalars) const; 00820 00821 protected: 00823 double r_, g_, b_; 00824 }; 00825 00827 00831 template <> 00832 class PCL_EXPORTS PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2> : public PointCloudColorHandler<sensor_msgs::PointCloud2> 00833 { 00834 typedef PointCloudColorHandler<sensor_msgs::PointCloud2>::PointCloud PointCloud; 00835 typedef PointCloud::Ptr PointCloudPtr; 00836 typedef PointCloud::ConstPtr PointCloudConstPtr; 00837 00838 public: 00839 typedef boost::shared_ptr<PointCloudColorHandlerRGBField<PointCloud> > Ptr; 00840 typedef boost::shared_ptr<const PointCloudColorHandlerRGBField<PointCloud> > ConstPtr; 00841 00843 PointCloudColorHandlerRGBField (const PointCloudConstPtr &cloud); 00844 00848 virtual void 00849 getColor (vtkSmartPointer<vtkDataArray> &scalars) const; 00850 00851 protected: 00853 virtual inline std::string 00854 getName () const { return ("PointCloudColorHandlerRGBField"); } 00855 00857 virtual std::string 00858 getFieldName () const { return ("rgb"); } 00859 }; 00860 00862 00866 template <> 00867 class PCL_EXPORTS PointCloudColorHandlerHSVField<sensor_msgs::PointCloud2> : public PointCloudColorHandler<sensor_msgs::PointCloud2> 00868 { 00869 typedef PointCloudColorHandler<sensor_msgs::PointCloud2>::PointCloud PointCloud; 00870 typedef PointCloud::Ptr PointCloudPtr; 00871 typedef PointCloud::ConstPtr PointCloudConstPtr; 00872 00873 public: 00874 typedef boost::shared_ptr<PointCloudColorHandlerHSVField<PointCloud> > Ptr; 00875 typedef boost::shared_ptr<const PointCloudColorHandlerHSVField<PointCloud> > ConstPtr; 00876 00878 PointCloudColorHandlerHSVField (const PointCloudConstPtr &cloud); 00879 00883 virtual void 00884 getColor (vtkSmartPointer<vtkDataArray> &scalars) const; 00885 00886 protected: 00888 virtual inline std::string 00889 getName () const { return ("PointCloudColorHandlerHSVField"); } 00890 00892 virtual std::string 00893 getFieldName () const { return ("hsv"); } 00894 00896 int s_field_idx_; 00897 00899 int v_field_idx_; 00900 }; 00901 00903 00907 template <> 00908 class PCL_EXPORTS PointCloudColorHandlerGenericField<sensor_msgs::PointCloud2> : public PointCloudColorHandler<sensor_msgs::PointCloud2> 00909 { 00910 typedef PointCloudColorHandler<sensor_msgs::PointCloud2>::PointCloud PointCloud; 00911 typedef PointCloud::Ptr PointCloudPtr; 00912 typedef PointCloud::ConstPtr PointCloudConstPtr; 00913 00914 public: 00915 typedef boost::shared_ptr<PointCloudColorHandlerGenericField<PointCloud> > Ptr; 00916 typedef boost::shared_ptr<const PointCloudColorHandlerGenericField<PointCloud> > ConstPtr; 00917 00919 PointCloudColorHandlerGenericField (const PointCloudConstPtr &cloud, 00920 const std::string &field_name); 00921 00925 virtual void 00926 getColor (vtkSmartPointer<vtkDataArray> &scalars) const; 00927 00928 protected: 00930 virtual inline std::string 00931 getName () const { return ("PointCloudColorHandlerGenericField"); } 00932 00934 virtual std::string 00935 getFieldName () const { return (field_name_); } 00936 00937 private: 00939 std::string field_name_; 00940 }; 00941 00942 } 00943 } 00944 00945 #include "pcl/visualization/impl/point_cloud_handlers.hpp" 00946 00947 #endif
1.7.6.1