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) 2010-2012, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 */ 00038 00039 #ifndef PCL_POINT_CLOUD_H_ 00040 #define PCL_POINT_CLOUD_H_ 00041 00042 #ifdef __GNUC__ 00043 #pragma GCC system_header 00044 #endif 00045 00046 #include <Eigen/StdVector> 00047 #include <Eigen/Geometry> 00048 #include <pcl/PCLHeader.h> 00049 #include <pcl/exceptions.h> 00050 #include <pcl/point_traits.h> 00051 00052 namespace pcl 00053 { 00054 namespace detail 00055 { 00056 struct FieldMapping 00057 { 00058 size_t serialized_offset; 00059 size_t struct_offset; 00060 size_t size; 00061 }; 00062 } // namespace detail 00063 00064 // Forward declarations 00065 template <typename PointT> class PointCloud; 00066 typedef std::vector<detail::FieldMapping> MsgFieldMap; 00067 00068 /** \brief Helper functor structure for copying data between an Eigen type and a PointT. */ 00069 template <typename PointOutT> 00070 struct NdCopyEigenPointFunctor 00071 { 00072 typedef typename traits::POD<PointOutT>::type Pod; 00073 00074 /** \brief Constructor 00075 * \param[in] p1 the input Eigen type 00076 * \param[out] p2 the output Point type 00077 */ 00078 NdCopyEigenPointFunctor (const Eigen::VectorXf &p1, PointOutT &p2) 00079 : p1_ (p1), 00080 p2_ (reinterpret_cast<Pod&>(p2)), 00081 f_idx_ (0) { } 00082 00083 /** \brief Operator. Data copy happens here. */ 00084 template<typename Key> inline void 00085 operator() () 00086 { 00087 //boost::fusion::at_key<Key> (p2_) = p1_[f_idx_++]; 00088 typedef typename pcl::traits::datatype<PointOutT, Key>::type T; 00089 uint8_t* data_ptr = reinterpret_cast<uint8_t*>(&p2_) + pcl::traits::offset<PointOutT, Key>::value; 00090 *reinterpret_cast<T*>(data_ptr) = static_cast<T> (p1_[f_idx_++]); 00091 } 00092 00093 private: 00094 const Eigen::VectorXf &p1_; 00095 Pod &p2_; 00096 int f_idx_; 00097 public: 00098 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00099 }; 00100 00101 /** \brief Helper functor structure for copying data between an Eigen type and a PointT. */ 00102 template <typename PointInT> 00103 struct NdCopyPointEigenFunctor 00104 { 00105 typedef typename traits::POD<PointInT>::type Pod; 00106 00107 /** \brief Constructor 00108 * \param[in] p1 the input Point type 00109 * \param[out] p2 the output Eigen type 00110 */ 00111 NdCopyPointEigenFunctor (const PointInT &p1, Eigen::VectorXf &p2) 00112 : p1_ (reinterpret_cast<const Pod&>(p1)), p2_ (p2), f_idx_ (0) { } 00113 00114 /** \brief Operator. Data copy happens here. */ 00115 template<typename Key> inline void 00116 operator() () 00117 { 00118 //p2_[f_idx_++] = boost::fusion::at_key<Key> (p1_); 00119 typedef typename pcl::traits::datatype<PointInT, Key>::type T; 00120 const uint8_t* data_ptr = reinterpret_cast<const uint8_t*>(&p1_) + pcl::traits::offset<PointInT, Key>::value; 00121 p2_[f_idx_++] = static_cast<float> (*reinterpret_cast<const T*>(data_ptr)); 00122 } 00123 00124 private: 00125 const Pod &p1_; 00126 Eigen::VectorXf &p2_; 00127 int f_idx_; 00128 public: 00129 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00130 }; 00131 00132 namespace detail 00133 { 00134 template <typename PointT> boost::shared_ptr<pcl::MsgFieldMap>& 00135 getMapping (pcl::PointCloud<PointT>& p); 00136 } // namespace detail 00137 00138 /** \brief PointCloud represents the base class in PCL for storing collections of 3D points. 00139 * 00140 * The class is templated, which means you need to specify the type of data 00141 * that it should contain. For example, to create a point cloud that holds 4 00142 * random XYZ data points, use: 00143 * 00144 * \code 00145 * pcl::PointCloud<pcl::PointXYZ> cloud; 00146 * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ())); 00147 * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ())); 00148 * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ())); 00149 * cloud.push_back (pcl::PointXYZ (rand (), rand (), rand ())); 00150 * \endcode 00151 * 00152 * The PointCloud class contains the following elements: 00153 * - \b width - specifies the width of the point cloud dataset in the number of points. WIDTH has two meanings: 00154 * - it can specify the total number of points in the cloud (equal with POINTS see below) for unorganized datasets; 00155 * - it can specify the width (total number of points in a row) of an organized point cloud dataset. 00156 * \a Mandatory. 00157 * - \b height - specifies the height of the point cloud dataset in the number of points. HEIGHT has two meanings: 00158 * - it can specify the height (total number of rows) of an organized point cloud dataset; 00159 * - it is set to 1 for unorganized datasets (thus used to check whether a dataset is organized or not). 00160 * \a Mandatory. 00161 * - \b points - the data array where all points of type <b>PointT</b> are stored. \a Mandatory. 00162 * 00163 * - \b is_dense - specifies if all the data in <b>points</b> is finite (true), or whether it might contain Inf/NaN values 00164 * (false). \a Mandatory. 00165 * 00166 * - \b sensor_origin_ - specifies the sensor acquisition pose (origin/translation). \a Optional. 00167 * - \b sensor_orientation_ - specifies the sensor acquisition pose (rotation). \a Optional. 00168 * 00169 * \author Patrick Mihelich, Radu B. Rusu 00170 */ 00171 template <typename PointT> 00172 class PointCloud 00173 { 00174 public: 00175 /** \brief Default constructor. Sets \ref is_dense to true, \ref width 00176 * and \ref height to 0, and the \ref sensor_origin_ and \ref 00177 * sensor_orientation_ to identity. 00178 */ 00179 PointCloud () : 00180 header (), points (), width (0), height (0), is_dense (true), 00181 sensor_origin_ (Eigen::Vector4f::Zero ()), sensor_orientation_ (Eigen::Quaternionf::Identity ()), 00182 mapping_ () 00183 {} 00184 00185 /** \brief Copy constructor (needed by compilers such as Intel C++) 00186 * \param[in] pc the cloud to copy into this 00187 */ 00188 PointCloud (PointCloud<PointT> &pc) : 00189 header (), points (), width (0), height (0), is_dense (true), 00190 sensor_origin_ (Eigen::Vector4f::Zero ()), sensor_orientation_ (Eigen::Quaternionf::Identity ()), 00191 mapping_ () 00192 { 00193 *this = pc; 00194 } 00195 00196 /** \brief Copy constructor (needed by compilers such as Intel C++) 00197 * \param[in] pc the cloud to copy into this 00198 */ 00199 PointCloud (const PointCloud<PointT> &pc) : 00200 header (), points (), width (0), height (0), is_dense (true), 00201 sensor_origin_ (Eigen::Vector4f::Zero ()), sensor_orientation_ (Eigen::Quaternionf::Identity ()), 00202 mapping_ () 00203 { 00204 *this = pc; 00205 } 00206 00207 /** \brief Copy constructor from point cloud subset 00208 * \param[in] pc the cloud to copy into this 00209 * \param[in] indices the subset to copy 00210 */ 00211 PointCloud (const PointCloud<PointT> &pc, 00212 const std::vector<int> &indices) : 00213 header (pc.header), points (indices.size ()), width (indices.size ()), height (1), is_dense (pc.is_dense), 00214 sensor_origin_ (pc.sensor_origin_), sensor_orientation_ (pc.sensor_orientation_), 00215 mapping_ () 00216 { 00217 // Copy the obvious 00218 assert (indices.size () <= pc.size ()); 00219 for (size_t i = 0; i < indices.size (); i++) 00220 points[i] = pc.points[indices[i]]; 00221 } 00222 00223 /** \brief Allocate constructor from point cloud subset 00224 * \param[in] width_ the cloud width 00225 * \param[in] height_ the cloud height 00226 * \param[in] value_ default value 00227 */ 00228 PointCloud (uint32_t width_, uint32_t height_, const PointT& value_ = PointT ()) 00229 : header () 00230 , points (width_ * height_, value_) 00231 , width (width_) 00232 , height (height_) 00233 , is_dense (true) 00234 , sensor_origin_ (Eigen::Vector4f::Zero ()) 00235 , sensor_orientation_ (Eigen::Quaternionf::Identity ()) 00236 , mapping_ () 00237 {} 00238 00239 /** \brief Destructor. */ 00240 virtual ~PointCloud () {} 00241 00242 /** \brief Add a point cloud to the current cloud. 00243 * \param[in] rhs the cloud to add to the current cloud 00244 * \return the new cloud as a concatenation of the current cloud and the new given cloud 00245 */ 00246 inline PointCloud& 00247 operator += (const PointCloud& rhs) 00248 { 00249 // Make the resultant point cloud take the newest stamp 00250 if (rhs.header.stamp > header.stamp) 00251 header.stamp = rhs.header.stamp; 00252 00253 size_t nr_points = points.size (); 00254 points.resize (nr_points + rhs.points.size ()); 00255 for (size_t i = nr_points; i < points.size (); ++i) 00256 points[i] = rhs.points[i - nr_points]; 00257 00258 width = static_cast<uint32_t>(points.size ()); 00259 height = 1; 00260 if (rhs.is_dense && is_dense) 00261 is_dense = true; 00262 else 00263 is_dense = false; 00264 return (*this); 00265 } 00266 00267 /** \brief Add a point cloud to another cloud. 00268 * \param[in] rhs the cloud to add to the current cloud 00269 * \return the new cloud as a concatenation of the current cloud and the new given cloud 00270 */ 00271 inline const PointCloud 00272 operator + (const PointCloud& rhs) 00273 { 00274 return (PointCloud (*this) += rhs); 00275 } 00276 00277 /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized 00278 * datasets (those that have height != 1). 00279 * \param[in] column the column coordinate 00280 * \param[in] row the row coordinate 00281 */ 00282 inline const PointT& 00283 at (int column, int row) const 00284 { 00285 if (this->height > 1) 00286 return (points.at (row * this->width + column)); 00287 else 00288 throw IsNotDenseException ("Can't use 2D indexing with a unorganized point cloud"); 00289 } 00290 00291 /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized 00292 * datasets (those that have height != 1). 00293 * \param[in] column the column coordinate 00294 * \param[in] row the row coordinate 00295 */ 00296 inline PointT& 00297 at (int column, int row) 00298 { 00299 if (this->height > 1) 00300 return (points.at (row * this->width + column)); 00301 else 00302 throw IsNotDenseException ("Can't use 2D indexing with a unorganized point cloud"); 00303 } 00304 00305 /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized 00306 * datasets (those that have height != 1). 00307 * \param[in] column the column coordinate 00308 * \param[in] row the row coordinate 00309 */ 00310 inline const PointT& 00311 operator () (size_t column, size_t row) const 00312 { 00313 return (points[row * this->width + column]); 00314 } 00315 00316 /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized 00317 * datasets (those that have height != 1). 00318 * \param[in] column the column coordinate 00319 * \param[in] row the row coordinate 00320 */ 00321 inline PointT& 00322 operator () (size_t column, size_t row) 00323 { 00324 return (points[row * this->width + column]); 00325 } 00326 00327 /** \brief Return whether a dataset is organized (e.g., arranged in a structured grid). 00328 * \note The height value must be different than 1 for a dataset to be organized. 00329 */ 00330 inline bool 00331 isOrganized () const 00332 { 00333 return (height > 1); 00334 } 00335 00336 /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the specified dimensions of the PointCloud. 00337 * \anchor getMatrixXfMap 00338 * \note This method is for advanced users only! Use with care! 00339 * 00340 * \attention Since 1.4.0, Eigen matrices are forced to Row Major to increase the efficiency of the algorithms in PCL 00341 * This means that the behavior of getMatrixXfMap changed, and is now correctly mapping 1-1 with a PointCloud structure, 00342 * that is: number of points in a cloud = rows in a matrix, number of point dimensions = columns in a matrix 00343 * 00344 * \param[in] dim the number of dimensions to consider for each point 00345 * \param[in] stride the number of values in each point (will be the number of values that separate two of the columns) 00346 * \param[in] offset the number of dimensions to skip from the beginning of each point 00347 * (stride = offset + dim + x, where x is the number of dimensions to skip from the end of each point) 00348 * \note for getting only XYZ coordinates out of PointXYZ use dim=3, stride=4 and offset=0 due to the alignment. 00349 * \attention PointT types are most of the time aligned, so the offsets are not continuous! 00350 */ 00351 inline Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > 00352 getMatrixXfMap (int dim, int stride, int offset) 00353 { 00354 if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit) 00355 return (Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(&points[0])+offset, points.size (), dim, Eigen::OuterStride<> (stride))); 00356 else 00357 return (Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(&points[0])+offset, dim, points.size (), Eigen::OuterStride<> (stride))); 00358 } 00359 00360 /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the specified dimensions of the PointCloud. 00361 * \anchor getMatrixXfMap 00362 * \note This method is for advanced users only! Use with care! 00363 * 00364 * \attention Since 1.4.0, Eigen matrices are forced to Row Major to increase the efficiency of the algorithms in PCL 00365 * This means that the behavior of getMatrixXfMap changed, and is now correctly mapping 1-1 with a PointCloud structure, 00366 * that is: number of points in a cloud = rows in a matrix, number of point dimensions = columns in a matrix 00367 * 00368 * \param[in] dim the number of dimensions to consider for each point 00369 * \param[in] stride the number of values in each point (will be the number of values that separate two of the columns) 00370 * \param[in] offset the number of dimensions to skip from the beginning of each point 00371 * (stride = offset + dim + x, where x is the number of dimensions to skip from the end of each point) 00372 * \note for getting only XYZ coordinates out of PointXYZ use dim=3, stride=4 and offset=0 due to the alignment. 00373 * \attention PointT types are most of the time aligned, so the offsets are not continuous! 00374 */ 00375 inline const Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > 00376 getMatrixXfMap (int dim, int stride, int offset) const 00377 { 00378 if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit) 00379 return (Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(const_cast<PointT*>(&points[0]))+offset, points.size (), dim, Eigen::OuterStride<> (stride))); 00380 else 00381 return (Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >(reinterpret_cast<float*>(const_cast<PointT*>(&points[0]))+offset, dim, points.size (), Eigen::OuterStride<> (stride))); 00382 } 00383 00384 /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud. 00385 * \note This method is for advanced users only! Use with care! 00386 * \attention PointT types are most of the time aligned, so the offsets are not continuous! 00387 * See \ref getMatrixXfMap for more information. 00388 */ 00389 inline Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > 00390 getMatrixXfMap () 00391 { 00392 return (getMatrixXfMap (sizeof (PointT) / sizeof (float), sizeof (PointT) / sizeof (float), 0)); 00393 } 00394 00395 /** \brief Return an Eigen MatrixXf (assumes float values) mapped to the PointCloud. 00396 * \note This method is for advanced users only! Use with care! 00397 * \attention PointT types are most of the time aligned, so the offsets are not continuous! 00398 * See \ref getMatrixXfMap for more information. 00399 */ 00400 inline const Eigen::Map<const Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > 00401 getMatrixXfMap () const 00402 { 00403 return (getMatrixXfMap (sizeof (PointT) / sizeof (float), sizeof (PointT) / sizeof (float), 0)); 00404 } 00405 00406 /** \brief The point cloud header. It contains information about the acquisition time. */ 00407 pcl::PCLHeader header; 00408 00409 /** \brief The point data. */ 00410 std::vector<PointT, Eigen::aligned_allocator<PointT> > points; 00411 00412 /** \brief The point cloud width (if organized as an image-structure). */ 00413 uint32_t width; 00414 /** \brief The point cloud height (if organized as an image-structure). */ 00415 uint32_t height; 00416 00417 /** \brief True if no points are invalid (e.g., have NaN or Inf values). */ 00418 bool is_dense; 00419 00420 /** \brief Sensor acquisition pose (origin/translation). */ 00421 Eigen::Vector4f sensor_origin_; 00422 /** \brief Sensor acquisition pose (rotation). */ 00423 Eigen::Quaternionf sensor_orientation_; 00424 00425 typedef PointT PointType; // Make the template class available from the outside 00426 typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > VectorType; 00427 typedef std::vector<PointCloud<PointT>, Eigen::aligned_allocator<PointCloud<PointT> > > CloudVectorType; 00428 typedef boost::shared_ptr<PointCloud<PointT> > Ptr; 00429 typedef boost::shared_ptr<const PointCloud<PointT> > ConstPtr; 00430 00431 // iterators 00432 typedef typename VectorType::iterator iterator; 00433 typedef typename VectorType::const_iterator const_iterator; 00434 inline iterator begin () { return (points.begin ()); } 00435 inline iterator end () { return (points.end ()); } 00436 inline const_iterator begin () const { return (points.begin ()); } 00437 inline const_iterator end () const { return (points.end ()); } 00438 00439 //capacity 00440 inline size_t size () const { return (points.size ()); } 00441 inline void reserve (size_t n) { points.reserve (n); } 00442 inline bool empty () const { return points.empty (); } 00443 00444 /** \brief Resize the cloud 00445 * \param[in] n the new cloud size 00446 */ 00447 inline void resize (size_t n) 00448 { 00449 points.resize (n); 00450 if (width * height != n) 00451 { 00452 width = static_cast<uint32_t> (n); 00453 height = 1; 00454 } 00455 } 00456 00457 //element access 00458 inline const PointT& operator[] (size_t n) const { return (points[n]); } 00459 inline PointT& operator[] (size_t n) { return (points[n]); } 00460 inline const PointT& at (size_t n) const { return (points.at (n)); } 00461 inline PointT& at (size_t n) { return (points.at (n)); } 00462 inline const PointT& front () const { return (points.front ()); } 00463 inline PointT& front () { return (points.front ()); } 00464 inline const PointT& back () const { return (points.back ()); } 00465 inline PointT& back () { return (points.back ()); } 00466 00467 /** \brief Insert a new point in the cloud, at the end of the container. 00468 * \note This breaks the organized structure of the cloud by setting the height to 1! 00469 * \param[in] pt the point to insert 00470 */ 00471 inline void 00472 push_back (const PointT& pt) 00473 { 00474 points.push_back (pt); 00475 width = static_cast<uint32_t> (points.size ()); 00476 height = 1; 00477 } 00478 00479 /** \brief Insert a new point in the cloud, given an iterator. 00480 * \note This breaks the organized structure of the cloud by setting the height to 1! 00481 * \param[in] position where to insert the point 00482 * \param[in] pt the point to insert 00483 * \return returns the new position iterator 00484 */ 00485 inline iterator 00486 insert (iterator position, const PointT& pt) 00487 { 00488 iterator it = points.insert (position, pt); 00489 width = static_cast<uint32_t> (points.size ()); 00490 height = 1; 00491 return (it); 00492 } 00493 00494 /** \brief Insert a new point in the cloud N times, given an iterator. 00495 * \note This breaks the organized structure of the cloud by setting the height to 1! 00496 * \param[in] position where to insert the point 00497 * \param[in] n the number of times to insert the point 00498 * \param[in] pt the point to insert 00499 */ 00500 inline void 00501 insert (iterator position, size_t n, const PointT& pt) 00502 { 00503 points.insert (position, n, pt); 00504 width = static_cast<uint32_t> (points.size ()); 00505 height = 1; 00506 } 00507 00508 /** \brief Insert a new range of points in the cloud, at a certain position. 00509 * \note This breaks the organized structure of the cloud by setting the height to 1! 00510 * \param[in] position where to insert the data 00511 * \param[in] first where to start inserting the points from 00512 * \param[in] last where to stop inserting the points from 00513 */ 00514 template <class InputIterator> inline void 00515 insert (iterator position, InputIterator first, InputIterator last) 00516 { 00517 points.insert (position, first, last); 00518 width = static_cast<uint32_t> (points.size ()); 00519 height = 1; 00520 } 00521 00522 /** \brief Erase a point in the cloud. 00523 * \note This breaks the organized structure of the cloud by setting the height to 1! 00524 * \param[in] position what data point to erase 00525 * \return returns the new position iterator 00526 */ 00527 inline iterator 00528 erase (iterator position) 00529 { 00530 iterator it = points.erase (position); 00531 width = static_cast<uint32_t> (points.size ()); 00532 height = 1; 00533 return (it); 00534 } 00535 00536 /** \brief Erase a set of points given by a (first, last) iterator pair 00537 * \note This breaks the organized structure of the cloud by setting the height to 1! 00538 * \param[in] first where to start erasing points from 00539 * \param[in] last where to stop erasing points from 00540 * \return returns the new position iterator 00541 */ 00542 inline iterator 00543 erase (iterator first, iterator last) 00544 { 00545 iterator it = points.erase (first, last); 00546 width = static_cast<uint32_t> (points.size ()); 00547 height = 1; 00548 return (it); 00549 } 00550 00551 /** \brief Swap a point cloud with another cloud. 00552 * \param[in,out] rhs point cloud to swap this with 00553 */ 00554 inline void 00555 swap (PointCloud<PointT> &rhs) 00556 { 00557 this->points.swap (rhs.points); 00558 std::swap (width, rhs.width); 00559 std::swap (height, rhs.height); 00560 std::swap (is_dense, rhs.is_dense); 00561 std::swap (sensor_origin_, rhs.sensor_origin_); 00562 std::swap (sensor_orientation_, rhs.sensor_orientation_); 00563 } 00564 00565 /** \brief Removes all points in a cloud and sets the width and height to 0. */ 00566 inline void 00567 clear () 00568 { 00569 points.clear (); 00570 width = 0; 00571 height = 0; 00572 } 00573 00574 /** \brief Copy the cloud to the heap and return a smart pointer 00575 * Note that deep copy is performed, so avoid using this function on non-empty clouds. 00576 * The changes of the returned cloud are not mirrored back to this one. 00577 * \return shared pointer to the copy of the cloud 00578 */ 00579 inline Ptr 00580 makeShared () const { return Ptr (new PointCloud<PointT> (*this)); } 00581 00582 protected: 00583 // This is motivated by ROS integration. Users should not need to access mapping_. 00584 boost::shared_ptr<MsgFieldMap> mapping_; 00585 00586 friend boost::shared_ptr<MsgFieldMap>& detail::getMapping<PointT>(pcl::PointCloud<PointT> &p); 00587 00588 public: 00589 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00590 }; 00591 00592 namespace detail 00593 { 00594 template <typename PointT> boost::shared_ptr<pcl::MsgFieldMap>& 00595 getMapping (pcl::PointCloud<PointT>& p) 00596 { 00597 return (p.mapping_); 00598 } 00599 } // namespace detail 00600 00601 template <typename PointT> std::ostream& 00602 operator << (std::ostream& s, const pcl::PointCloud<PointT> &p) 00603 { 00604 s << "points[]: " << p.points.size () << std::endl; 00605 s << "width: " << p.width << std::endl; 00606 s << "height: " << p.height << std::endl; 00607 s << "is_dense: " << p.is_dense << std::endl; 00608 s << "sensor origin (xyz): [" << 00609 p.sensor_origin_.x () << ", " << 00610 p.sensor_origin_.y () << ", " << 00611 p.sensor_origin_.z () << "] / orientation (xyzw): [" << 00612 p.sensor_orientation_.x () << ", " << 00613 p.sensor_orientation_.y () << ", " << 00614 p.sensor_orientation_.z () << ", " << 00615 p.sensor_orientation_.w () << "]" << 00616 std::endl; 00617 return (s); 00618 } 00619 } 00620 00621 #define PCL_INSTANTIATE_PointCloud(T) template class PCL_EXPORTS pcl::PointCloud<T>; 00622 00623 #endif //#ifndef PCL_POINT_CLOUD_H_