Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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 */ 00037 #ifndef PCL_POINT_CLOUD_GEOMETRY_HANDLERS_H_ 00038 #define PCL_POINT_CLOUD_GEOMETRY_HANDLERS_H_ 00039 00040 #if defined __GNUC__ 00041 #pragma GCC system_header 00042 #endif 00043 00044 // PCL includes 00045 #include <pcl/point_cloud.h> 00046 #include <pcl/common/io.h> 00047 // VTK includes 00048 #include <vtkSmartPointer.h> 00049 #include <vtkPoints.h> 00050 #include <vtkFloatArray.h> 00051 00052 namespace pcl 00053 { 00054 namespace visualization 00055 { 00056 /** \brief Base handler class for PointCloud geometry. 00057 * \author Radu B. Rusu 00058 * \ingroup visualization 00059 */ 00060 template <typename PointT> 00061 class PointCloudGeometryHandler 00062 { 00063 public: 00064 typedef pcl::PointCloud<PointT> PointCloud; 00065 typedef typename PointCloud::Ptr PointCloudPtr; 00066 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00067 00068 typedef typename boost::shared_ptr<PointCloudGeometryHandler<PointT> > Ptr; 00069 typedef typename boost::shared_ptr<const PointCloudGeometryHandler<PointT> > ConstPtr; 00070 00071 /** \brief Constructor. */ 00072 PointCloudGeometryHandler (const PointCloudConstPtr &cloud) : 00073 cloud_ (cloud), capable_ (false), 00074 field_x_idx_ (-1), field_y_idx_ (-1), field_z_idx_ (-1), 00075 fields_ () 00076 {} 00077 00078 /** \brief Destructor. */ 00079 virtual ~PointCloudGeometryHandler () {} 00080 00081 /** \brief Abstract getName method. 00082 * \return the name of the class/object. 00083 */ 00084 virtual std::string 00085 getName () const = 0; 00086 00087 /** \brief Abstract getFieldName method. */ 00088 virtual std::string 00089 getFieldName () const = 0; 00090 00091 /** \brief Checl if this handler is capable of handling the input data or not. */ 00092 inline bool 00093 isCapable () const { return (capable_); } 00094 00095 /** \brief Obtain the actual point geometry for the input dataset in VTK format. 00096 * \param[out] points the resultant geometry 00097 */ 00098 virtual void 00099 getGeometry (vtkSmartPointer<vtkPoints> &points) const = 0; 00100 00101 /** \brief Set the input cloud to be used. 00102 * \param[in] cloud the input cloud to be used by the handler 00103 */ 00104 void 00105 setInputCloud (const PointCloudConstPtr &cloud) 00106 { 00107 cloud_ = cloud; 00108 } 00109 00110 protected: 00111 /** \brief A pointer to the input dataset. */ 00112 PointCloudConstPtr cloud_; 00113 00114 /** \brief True if this handler is capable of handling the input data, false 00115 * otherwise. 00116 */ 00117 bool capable_; 00118 00119 /** \brief The index of the field holding the X data. */ 00120 int field_x_idx_; 00121 00122 /** \brief The index of the field holding the Y data. */ 00123 int field_y_idx_; 00124 00125 /** \brief The index of the field holding the Z data. */ 00126 int field_z_idx_; 00127 00128 /** \brief The list of fields available for this PointCloud. */ 00129 std::vector<pcl::PCLPointField> fields_; 00130 }; 00131 00132 ////////////////////////////////////////////////////////////////////////////////////// 00133 /** \brief XYZ handler class for PointCloud geometry. Given an input dataset, all XYZ 00134 * data present in fields "x", "y", and "z" is extracted and displayed on screen. 00135 * \author Radu B. Rusu 00136 * \ingroup visualization 00137 */ 00138 template <typename PointT> 00139 class PointCloudGeometryHandlerXYZ : public PointCloudGeometryHandler<PointT> 00140 { 00141 public: 00142 typedef typename PointCloudGeometryHandler<PointT>::PointCloud PointCloud; 00143 typedef typename PointCloud::Ptr PointCloudPtr; 00144 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00145 00146 typedef typename boost::shared_ptr<PointCloudGeometryHandlerXYZ<PointT> > Ptr; 00147 typedef typename boost::shared_ptr<const PointCloudGeometryHandlerXYZ<PointT> > ConstPtr; 00148 00149 /** \brief Constructor. */ 00150 PointCloudGeometryHandlerXYZ (const PointCloudConstPtr &cloud); 00151 00152 /** \brief Destructor. */ 00153 virtual ~PointCloudGeometryHandlerXYZ () {}; 00154 00155 /** \brief Class getName method. */ 00156 virtual std::string 00157 getName () const { return ("PointCloudGeometryHandlerXYZ"); } 00158 00159 /** \brief Get the name of the field used. */ 00160 virtual std::string 00161 getFieldName () const { return ("xyz"); } 00162 00163 /** \brief Obtain the actual point geometry for the input dataset in VTK format. 00164 * \param[out] points the resultant geometry 00165 */ 00166 virtual void 00167 getGeometry (vtkSmartPointer<vtkPoints> &points) const; 00168 00169 private: 00170 // Members derived from the base class 00171 using PointCloudGeometryHandler<PointT>::cloud_; 00172 using PointCloudGeometryHandler<PointT>::capable_; 00173 using PointCloudGeometryHandler<PointT>::field_x_idx_; 00174 using PointCloudGeometryHandler<PointT>::field_y_idx_; 00175 using PointCloudGeometryHandler<PointT>::field_z_idx_; 00176 using PointCloudGeometryHandler<PointT>::fields_; 00177 }; 00178 00179 ////////////////////////////////////////////////////////////////////////////////////// 00180 /** \brief Surface normal handler class for PointCloud geometry. Given an input 00181 * dataset, all data present in fields "normal_x", "normal_y", and "normal_z" is 00182 * extracted and dislayed on screen as XYZ data. 00183 * \author Radu B. Rusu 00184 * \ingroup visualization 00185 */ 00186 template <typename PointT> 00187 class PointCloudGeometryHandlerSurfaceNormal : public PointCloudGeometryHandler<PointT> 00188 { 00189 public: 00190 typedef typename PointCloudGeometryHandler<PointT>::PointCloud PointCloud; 00191 typedef typename PointCloud::Ptr PointCloudPtr; 00192 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00193 00194 typedef typename boost::shared_ptr<PointCloudGeometryHandlerSurfaceNormal<PointT> > Ptr; 00195 typedef typename boost::shared_ptr<const PointCloudGeometryHandlerSurfaceNormal<PointT> > ConstPtr; 00196 00197 /** \brief Constructor. */ 00198 PointCloudGeometryHandlerSurfaceNormal (const PointCloudConstPtr &cloud); 00199 00200 /** \brief Class getName method. */ 00201 virtual std::string 00202 getName () const { return ("PointCloudGeometryHandlerSurfaceNormal"); } 00203 00204 /** \brief Get the name of the field used. */ 00205 virtual std::string 00206 getFieldName () const { return ("normal_xyz"); } 00207 00208 /** \brief Obtain the actual point geometry for the input dataset in VTK format. 00209 * \param[out] points the resultant geometry 00210 */ 00211 virtual void 00212 getGeometry (vtkSmartPointer<vtkPoints> &points) const; 00213 00214 private: 00215 // Members derived from the base class 00216 using PointCloudGeometryHandler<PointT>::cloud_; 00217 using PointCloudGeometryHandler<PointT>::capable_; 00218 using PointCloudGeometryHandler<PointT>::field_x_idx_; 00219 using PointCloudGeometryHandler<PointT>::field_y_idx_; 00220 using PointCloudGeometryHandler<PointT>::field_z_idx_; 00221 using PointCloudGeometryHandler<PointT>::fields_; 00222 }; 00223 00224 ////////////////////////////////////////////////////////////////////////////////////// 00225 /** \brief Custom handler class for PointCloud geometry. Given an input dataset and 00226 * three user defined fields, all data present in them is extracted and displayed on 00227 * screen as XYZ data. 00228 * \author Radu B. Rusu 00229 * \ingroup visualization 00230 */ 00231 template <typename PointT> 00232 class PointCloudGeometryHandlerCustom : public PointCloudGeometryHandler<PointT> 00233 { 00234 public: 00235 typedef typename PointCloudGeometryHandler<PointT>::PointCloud PointCloud; 00236 typedef typename PointCloud::Ptr PointCloudPtr; 00237 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00238 00239 typedef typename boost::shared_ptr<PointCloudGeometryHandlerCustom<PointT> > Ptr; 00240 typedef typename boost::shared_ptr<const PointCloudGeometryHandlerCustom<PointT> > ConstPtr; 00241 00242 /** \brief Constructor. */ 00243 PointCloudGeometryHandlerCustom (const PointCloudConstPtr &cloud, 00244 const std::string &x_field_name, 00245 const std::string &y_field_name, 00246 const std::string &z_field_name) 00247 { 00248 field_x_idx_ = pcl::getFieldIndex (*cloud, x_field_name, fields_); 00249 if (field_x_idx_ == -1) 00250 return; 00251 field_y_idx_ = pcl::getFieldIndex (*cloud, y_field_name, fields_); 00252 if (field_y_idx_ == -1) 00253 return; 00254 field_z_idx_ = pcl::getFieldIndex (*cloud, z_field_name, fields_); 00255 if (field_z_idx_ == -1) 00256 return; 00257 field_name_ = x_field_name + y_field_name + z_field_name; 00258 capable_ = true; 00259 } 00260 00261 /** \brief Class getName method. */ 00262 virtual std::string 00263 getName () const { return ("PointCloudGeometryHandlerCustom"); } 00264 00265 /** \brief Get the name of the field used. */ 00266 virtual std::string 00267 getFieldName () const { return (field_name_); } 00268 00269 /** \brief Obtain the actual point geometry for the input dataset in VTK format. 00270 * \param[out] points the resultant geometry 00271 */ 00272 virtual void 00273 getGeometry (vtkSmartPointer<vtkPoints> &points) const 00274 { 00275 if (!capable_) 00276 return; 00277 00278 if (!points) 00279 points = vtkSmartPointer<vtkPoints>::New (); 00280 points->SetDataTypeToFloat (); 00281 points->SetNumberOfPoints (cloud_->points.size ()); 00282 00283 float data; 00284 // Add all points 00285 double p[3]; 00286 for (vtkIdType i = 0; i < static_cast<vtkIdType> (cloud_->points.size ()); ++i) 00287 { 00288 // Copy the value at the specified field 00289 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud_->points[i]); 00290 memcpy (&data, pt_data + fields_[field_x_idx_].offset, sizeof (float)); 00291 p[0] = data; 00292 00293 memcpy (&data, pt_data + fields_[field_y_idx_].offset, sizeof (float)); 00294 p[1] = data; 00295 00296 memcpy (&data, pt_data + fields_[field_z_idx_].offset, sizeof (float)); 00297 p[2] = data; 00298 00299 points->SetPoint (i, p); 00300 } 00301 } 00302 00303 private: 00304 // Members derived from the base class 00305 using PointCloudGeometryHandler<PointT>::cloud_; 00306 using PointCloudGeometryHandler<PointT>::capable_; 00307 using PointCloudGeometryHandler<PointT>::field_x_idx_; 00308 using PointCloudGeometryHandler<PointT>::field_y_idx_; 00309 using PointCloudGeometryHandler<PointT>::field_z_idx_; 00310 using PointCloudGeometryHandler<PointT>::fields_; 00311 00312 /** \brief Name of the field used to create the geometry handler. */ 00313 std::string field_name_; 00314 }; 00315 00316 /////////////////////////////////////////////////////////////////////////////////////// 00317 /** \brief Base handler class for PointCloud geometry. 00318 * \author Radu B. Rusu 00319 * \ingroup visualization 00320 */ 00321 template <> 00322 class PCL_EXPORTS PointCloudGeometryHandler<pcl::PCLPointCloud2> 00323 { 00324 public: 00325 typedef pcl::PCLPointCloud2 PointCloud; 00326 typedef PointCloud::Ptr PointCloudPtr; 00327 typedef PointCloud::ConstPtr PointCloudConstPtr; 00328 00329 typedef boost::shared_ptr<PointCloudGeometryHandler<PointCloud> > Ptr; 00330 typedef boost::shared_ptr<const PointCloudGeometryHandler<PointCloud> > ConstPtr; 00331 00332 /** \brief Constructor. */ 00333 PointCloudGeometryHandler (const PointCloudConstPtr &cloud, const Eigen::Vector4f & = Eigen::Vector4f::Zero ()) 00334 : cloud_ (cloud) 00335 , capable_ (false) 00336 , field_x_idx_ (-1) 00337 , field_y_idx_ (-1) 00338 , field_z_idx_ (-1) 00339 , fields_ (cloud_->fields) 00340 { 00341 } 00342 00343 /** \brief Destructor. */ 00344 virtual ~PointCloudGeometryHandler () {} 00345 00346 /** \brief Abstract getName method. */ 00347 virtual std::string 00348 getName () const = 0; 00349 00350 /** \brief Abstract getFieldName method. */ 00351 virtual std::string 00352 getFieldName () const = 0; 00353 00354 /** \brief Check if this handler is capable of handling the input data or not. */ 00355 inline bool 00356 isCapable () const { return (capable_); } 00357 00358 /** \brief Obtain the actual point geometry for the input dataset in VTK format. 00359 * \param[out] points the resultant geometry 00360 */ 00361 virtual void 00362 getGeometry (vtkSmartPointer<vtkPoints> &points) const; 00363 00364 /** \brief Set the input cloud to be used. 00365 * \param[in] cloud the input cloud to be used by the handler 00366 */ 00367 void 00368 setInputCloud (const PointCloudConstPtr &cloud) 00369 { 00370 cloud_ = cloud; 00371 } 00372 00373 protected: 00374 /** \brief A pointer to the input dataset. */ 00375 PointCloudConstPtr cloud_; 00376 00377 /** \brief True if this handler is capable of handling the input data, false 00378 * otherwise. 00379 */ 00380 bool capable_; 00381 00382 /** \brief The index of the field holding the X data. */ 00383 int field_x_idx_; 00384 00385 /** \brief The index of the field holding the Y data. */ 00386 int field_y_idx_; 00387 00388 /** \brief The index of the field holding the Z data. */ 00389 int field_z_idx_; 00390 00391 /** \brief The list of fields available for this PointCloud. */ 00392 std::vector<pcl::PCLPointField> fields_; 00393 }; 00394 00395 ////////////////////////////////////////////////////////////////////////////////////// 00396 /** \brief XYZ handler class for PointCloud geometry. Given an input dataset, all XYZ 00397 * data present in fields "x", "y", and "z" is extracted and displayed on screen. 00398 * \author Radu B. Rusu 00399 * \ingroup visualization 00400 */ 00401 template <> 00402 class PCL_EXPORTS PointCloudGeometryHandlerXYZ<pcl::PCLPointCloud2> : public PointCloudGeometryHandler<pcl::PCLPointCloud2> 00403 { 00404 public: 00405 typedef PointCloudGeometryHandler<pcl::PCLPointCloud2>::PointCloud PointCloud; 00406 typedef PointCloud::Ptr PointCloudPtr; 00407 typedef PointCloud::ConstPtr PointCloudConstPtr; 00408 00409 typedef boost::shared_ptr<PointCloudGeometryHandlerXYZ<PointCloud> > Ptr; 00410 typedef boost::shared_ptr<const PointCloudGeometryHandlerXYZ<PointCloud> > ConstPtr; 00411 00412 /** \brief Constructor. */ 00413 PointCloudGeometryHandlerXYZ (const PointCloudConstPtr &cloud); 00414 00415 /** \brief Destructor. */ 00416 virtual ~PointCloudGeometryHandlerXYZ () {} 00417 00418 /** \brief Class getName method. */ 00419 virtual std::string 00420 getName () const { return ("PointCloudGeometryHandlerXYZ"); } 00421 00422 /** \brief Get the name of the field used. */ 00423 virtual std::string 00424 getFieldName () const { return ("xyz"); } 00425 }; 00426 00427 ////////////////////////////////////////////////////////////////////////////////////// 00428 /** \brief Surface normal handler class for PointCloud geometry. Given an input 00429 * dataset, all data present in fields "normal_x", "normal_y", and "normal_z" is 00430 * extracted and dislayed on screen as XYZ data. 00431 * \author Radu B. Rusu 00432 * \ingroup visualization 00433 */ 00434 template <> 00435 class PCL_EXPORTS PointCloudGeometryHandlerSurfaceNormal<pcl::PCLPointCloud2> : public PointCloudGeometryHandler<pcl::PCLPointCloud2> 00436 { 00437 public: 00438 typedef PointCloudGeometryHandler<pcl::PCLPointCloud2>::PointCloud PointCloud; 00439 typedef PointCloud::Ptr PointCloudPtr; 00440 typedef PointCloud::ConstPtr PointCloudConstPtr; 00441 00442 typedef boost::shared_ptr<PointCloudGeometryHandlerSurfaceNormal<PointCloud> > Ptr; 00443 typedef boost::shared_ptr<const PointCloudGeometryHandlerSurfaceNormal<PointCloud> > ConstPtr; 00444 00445 /** \brief Constructor. */ 00446 PointCloudGeometryHandlerSurfaceNormal (const PointCloudConstPtr &cloud); 00447 00448 /** \brief Class getName method. */ 00449 virtual std::string 00450 getName () const { return ("PointCloudGeometryHandlerSurfaceNormal"); } 00451 00452 /** \brief Get the name of the field used. */ 00453 virtual std::string 00454 getFieldName () const { return ("normal_xyz"); } 00455 }; 00456 00457 ////////////////////////////////////////////////////////////////////////////////////// 00458 /** \brief Custom handler class for PointCloud geometry. Given an input dataset and 00459 * three user defined fields, all data present in them is extracted and displayed on 00460 * screen as XYZ data. 00461 * \author Radu B. Rusu 00462 * \ingroup visualization 00463 */ 00464 template <> 00465 class PCL_EXPORTS PointCloudGeometryHandlerCustom<pcl::PCLPointCloud2> : public PointCloudGeometryHandler<pcl::PCLPointCloud2> 00466 { 00467 public: 00468 typedef PointCloudGeometryHandler<pcl::PCLPointCloud2>::PointCloud PointCloud; 00469 typedef PointCloud::Ptr PointCloudPtr; 00470 typedef PointCloud::ConstPtr PointCloudConstPtr; 00471 00472 /** \brief Constructor. */ 00473 PointCloudGeometryHandlerCustom (const PointCloudConstPtr &cloud, 00474 const std::string &x_field_name, 00475 const std::string &y_field_name, 00476 const std::string &z_field_name); 00477 00478 /** \brief Destructor. */ 00479 virtual ~PointCloudGeometryHandlerCustom () {} 00480 00481 /** \brief Class getName method. */ 00482 virtual std::string 00483 getName () const { return ("PointCloudGeometryHandlerCustom"); } 00484 00485 /** \brief Get the name of the field used. */ 00486 virtual std::string 00487 getFieldName () const { return (field_name_); } 00488 00489 private: 00490 /** \brief Name of the field used to create the geometry handler. */ 00491 std::string field_name_; 00492 }; 00493 } 00494 } 00495 00496 #ifdef PCL_NO_PRECOMPILE 00497 #include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp> 00498 #endif 00499 00500 #endif // PCL_POINT_CLOUD_GEOMETRY_HANDLERS_H_ 00501