Point Cloud Library (PCL)
1.3.1
|
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.h 2596 2011-09-26 20:06:51Z jspricke $ 00037 * 00038 */ 00039 00040 #ifndef PCL_POINT_CLOUD_H_ 00041 #define PCL_POINT_CLOUD_H_ 00042 00043 #include <cstddef> 00044 #include "pcl/pcl_macros.h" 00045 #include <Eigen/StdVector> 00046 #include <Eigen/Geometry> 00047 #include <std_msgs/Header.h> 00048 #include <pcl/exceptions.h> 00049 #include <pcl/console/print.h> 00050 #include <boost/shared_ptr.hpp> 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 typedef std::vector<detail::FieldMapping> MsgFieldMap; 00065 00066 // Forward declarations 00067 template <typename PointT> class PointCloud; 00068 00069 namespace detail 00070 { 00071 template <typename PointT> boost::shared_ptr<pcl::MsgFieldMap>& 00072 getMapping (pcl::PointCloud<PointT>& p); 00073 } // namespace detail 00074 00078 template <typename PointT> 00079 class PointCloud 00080 { 00081 public: 00082 PointCloud () : width (0), height (0), is_dense (true), 00083 sensor_origin_ (Eigen::Vector4f::Zero ()), sensor_orientation_ (Eigen::Quaternionf::Identity ()) 00084 {} 00085 00089 inline PointCloud (PointCloud<PointT> &pc) 00090 { 00091 *this = pc; 00092 } 00093 00097 inline PointCloud (const PointCloud<PointT> &pc) 00098 { 00099 *this = pc; 00100 } 00101 00106 inline PointCloud (const PointCloud<PointT> &pc, 00107 const std::vector<size_t> &indices) 00108 { 00109 assert(indices.size () <= pc.size ()); 00110 this->resize (indices.size ()); 00111 for(size_t i = 0; i < indices.size (); i++) 00112 { 00113 this->push_back (pc[indices[i]]); 00114 } 00115 } 00116 00118 inline PointCloud& 00119 operator += (const PointCloud& rhs) 00120 { 00121 if (rhs.header.frame_id != header.frame_id) 00122 { 00123 PCL_ERROR ("PointCloud frame IDs do not match (%s != %s) for += . Cancelling operation...\n", 00124 rhs.header.frame_id.c_str (), header.frame_id.c_str ()); 00125 return (*this); 00126 } 00127 00128 // Make the resultant point cloud take the newest stamp 00129 if (rhs.header.stamp > header.stamp) 00130 header.stamp = rhs.header.stamp; 00131 00132 size_t nr_points = points.size (); 00133 points.resize (nr_points + rhs.points.size ()); 00134 for (size_t i = nr_points; i < points.size (); ++i) 00135 points[i] = rhs.points[i - nr_points]; 00136 00137 width = (uint32_t) points.size (); 00138 height = 1; 00139 if (rhs.is_dense && is_dense) 00140 is_dense = true; 00141 else 00142 is_dense = false; 00143 return (*this); 00144 } 00145 00147 inline PointT 00148 at (int u, int v) const 00149 { 00150 if (this->height > 1) 00151 return (points.at (v * this->width + u)); 00152 else 00153 throw IsNotDenseException ("Can't use 2D indexing with a sparse point cloud"); 00154 } 00155 00157 inline const PointT& 00158 operator () (int u, int v) const 00159 { 00160 return (points[v * this->width + u]); 00161 } 00162 00163 inline PointT& 00164 operator () (int u, int v) 00165 { 00166 return (points[v * this->width + u]); 00167 } 00168 00170 00172 inline bool 00173 isOrganized () const 00174 { 00175 return (height != 1); 00176 } 00177 00179 00186 inline Eigen::MatrixXf 00187 getMatrixXfMap (int dim, int stride, int offset) 00188 { 00189 return Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> >((float*)(&points[0])+offset, dim, points.size(), Eigen::OuterStride<>(stride)); 00190 } 00191 00193 00195 inline Eigen::MatrixXf 00196 getMatrixXfMap () 00197 { 00198 return getMatrixXfMap (sizeof (PointT) / sizeof (float), sizeof (PointT) / sizeof (float), 0); 00199 } 00200 00203 std_msgs::Header header; 00204 00206 std::vector<PointT, Eigen::aligned_allocator<PointT> > points; 00207 00209 uint32_t width; 00211 uint32_t height; 00212 00214 bool is_dense; 00215 00217 Eigen::Vector4f sensor_origin_; 00219 Eigen::Quaternionf sensor_orientation_; 00220 00221 typedef PointT PointType; // Make the template class available from the outside 00222 typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > VectorType; 00223 typedef boost::shared_ptr<PointCloud<PointT> > Ptr; 00224 typedef boost::shared_ptr<const PointCloud<PointT> > ConstPtr; 00225 00226 // iterators 00227 typedef typename VectorType::iterator iterator; 00228 typedef typename VectorType::const_iterator const_iterator; 00229 inline iterator begin () { return (points.begin ()); } 00230 inline iterator end () { return (points.end ()); } 00231 inline const_iterator begin () const { return (points.begin ()); } 00232 inline const_iterator end () const { return (points.end ()); } 00233 00234 //capacity 00235 inline size_t size () const { return (points.size ()); } 00236 inline void reserve(size_t n) { points.reserve (n); } 00237 inline void resize(size_t n) { points.resize (n); } 00238 inline bool empty() const { return points.empty (); } 00239 00240 //element access 00241 inline const PointT& operator[] (size_t n) const { return points[n]; } 00242 inline PointT& operator[] (size_t n) { return points[n]; } 00243 inline const PointT& at (size_t n) const { return points.at (n); } 00244 inline PointT& at (size_t n) { return points.at (n); } 00245 inline const PointT& front () const { return points.front (); } 00246 inline PointT& front () { return points.front (); } 00247 inline const PointT& back () const { return points.back (); } 00248 inline PointT& back () { return points.back (); } 00249 00250 //modifiers 00251 inline void push_back (const PointT& p) { 00252 points.push_back (p); 00253 width = points.size (); 00254 height = 1; 00255 } 00256 inline iterator insert ( iterator position, const PointT& x ) 00257 { 00258 iterator it = points.insert (position, x); 00259 width = points.size (); 00260 height = 1; 00261 return it; 00262 } 00263 inline void insert ( iterator position, size_t n, const PointT& x ) 00264 { 00265 points.insert (position, n, x); 00266 width = points.size (); 00267 height = 1; 00268 } 00269 template <class InputIterator> 00270 inline void insert ( iterator position, InputIterator first, InputIterator last ) 00271 { 00272 points.insert(position, first, last); 00273 } 00274 inline iterator erase ( iterator position ) 00275 { 00276 iterator it = points.erase (position); 00277 width = points.size (); 00278 height = 1; 00279 return it; 00280 } 00281 inline iterator erase ( iterator first, iterator last ) 00282 { 00283 iterator it = points.erase (first, last); 00284 width = points.size (); 00285 height = 1; 00286 return it; 00287 } 00288 inline void swap (PointCloud<PointT> &rhs) 00289 { 00290 this->points.swap (rhs.points); 00291 std::swap (width, rhs.width); 00292 std::swap (height, rhs.height); 00293 } 00294 inline void clear () 00295 { 00296 points.clear (); 00297 width = 0; 00298 height = 0; 00299 } 00300 00306 inline Ptr makeShared () { return Ptr (new PointCloud<PointT> (*this)); } 00307 00312 inline ConstPtr makeShared () const { return ConstPtr (new PointCloud<PointT> (*this)); } 00313 00314 protected: 00315 // This is motivated by ROS integration. Users should not need to access mapping_. 00316 boost::shared_ptr<MsgFieldMap> mapping_; 00317 00318 friend boost::shared_ptr<MsgFieldMap>& detail::getMapping<PointT>(pcl::PointCloud<PointT> &p); 00319 00320 public: 00321 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 00322 }; 00323 00324 namespace detail 00325 { 00326 template <typename PointT> boost::shared_ptr<pcl::MsgFieldMap>& 00327 getMapping (pcl::PointCloud<PointT>& p) 00328 { 00329 return (p.mapping_); 00330 } 00331 } // namespace detail 00332 00333 template <typename PointT> std::ostream& 00334 operator << (std::ostream& s, const pcl::PointCloud<PointT> &p) 00335 { 00336 s << "header: " << std::endl; 00337 s << p.header; 00338 s << "points[]: " << p.points.size () << std::endl; 00339 s << "width: " << p.width << std::endl; 00340 s << "height: " << p.height << std::endl; 00341 s << "sensor_origin_: " 00342 << p.sensor_origin_[0] << ' ' 00343 << p.sensor_origin_[1] << ' ' 00344 << p.sensor_origin_[2] << std::endl; 00345 s << "sensor_orientation_: " 00346 << p.sensor_orientation_.x() << ' ' 00347 << p.sensor_orientation_.y() << ' ' 00348 << p.sensor_orientation_.z() << ' ' 00349 << p.sensor_orientation_.w() << std::endl; 00350 s << "is_dense: " << p.is_dense << std::endl; 00351 return (s); 00352 } 00353 } 00354 00355 #endif //#ifndef PCL_POINT_CLOUD_H_