point_cloud.h

Go to the documentation of this file.
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_