pcl_base.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: pcl_base.h 3006 2011-10-31 23:25:06Z rusu $
00037  *
00038  */
00039 #ifndef PCL_PCL_BASE_H_
00040 #define PCL_PCL_BASE_H_
00041 
00042 #include <cstddef>
00043 // Eigen includes
00044 #include <Eigen/StdVector>
00045 // STD includes
00046 #include <vector>
00047 
00048 // Include PCL macros such as PCL_ERROR, etc
00049 #include "pcl/pcl_macros.h"
00050 
00051 // Boost includes. Needed everywhere.
00052 #include <boost/shared_ptr.hpp>
00053 
00054 // Point Cloud message includes. Needed everywhere.
00055 #include <sensor_msgs/PointCloud2.h>
00056 #include "pcl/point_cloud.h"
00057 #include "pcl/PointIndices.h"
00058 #include "pcl/win32_macros.h"
00059 
00060 #include <pcl/console/print.h>
00061 
00062 namespace pcl
00063 {
00064   // definitions used everywhere
00065   typedef boost::shared_ptr <std::vector<int> > IndicesPtr;
00066   typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr;
00067 
00069 
00070   template <typename PointT>
00071   class PCLBase
00072   {
00073     public:
00074       typedef pcl::PointCloud<PointT> PointCloud;
00075       typedef typename PointCloud::Ptr PointCloudPtr;
00076       typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00077 
00078       typedef PointIndices::Ptr PointIndicesPtr;
00079       typedef PointIndices::ConstPtr PointIndicesConstPtr;
00080 
00082       PCLBase () : input_ (), indices_ (), use_indices_ (false), fake_indices_ (false) {}
00083 
00085       virtual ~PCLBase() 
00086       {
00087         input_.reset ();
00088         indices_.reset ();
00089       }
00090       
00094       virtual inline void 
00095       setInputCloud (const PointCloudConstPtr &cloud) { input_ = cloud; }
00096 
00098       inline PointCloudConstPtr const 
00099       getInputCloud () { return (input_); }
00100 
00104       inline void
00105       setIndices (const IndicesPtr &indices)
00106       {
00107         indices_ = indices;
00108         fake_indices_ = false;
00109         use_indices_  = true;
00110       }
00111 
00115       inline void
00116       setIndices (const PointIndicesConstPtr &indices)
00117       {
00118         indices_.reset (new std::vector<int> (indices->indices));
00119         fake_indices_ = false;
00120         use_indices_  = true;
00121       }
00122 
00131       inline void 
00132       setIndices (size_t row_start, size_t col_start, size_t nb_rows, size_t nb_cols)
00133       {
00134         if ((nb_rows > input_->height) || (row_start > input_->height))
00135         {
00136           PCL_ERROR ("[PCLBase::setIndices] cloud is only %d height", input_->height);
00137           return;
00138         }
00139 
00140         if ((nb_cols > input_->width) || (col_start > input_->width))
00141         {
00142           PCL_ERROR ("[PCLBase::setIndices] cloud is only %d width", input_->width);
00143           return;
00144         }
00145 
00146         size_t row_end = row_start + nb_rows;
00147         if (row_end > input_->height)
00148         {
00149           PCL_ERROR ("[PCLBase::setIndices] %d is out of rows range %d", row_end, input_->height);
00150           return;
00151         }
00152 
00153         size_t col_end = col_start + nb_cols;
00154         if (col_end > input_->width)
00155         {
00156           PCL_ERROR ("[PCLBase::setIndices] %d is out of columns range %d", col_end, input_->width);
00157           return;
00158         }
00159 
00160         indices_.reset (new std::vector<int>);
00161         indices_->reserve (nb_cols * nb_rows);
00162         for(size_t i = row_start; i < row_end; i++)
00163           for(size_t j = col_start; j < col_end; j++)
00164             indices_->push_back ((i * input_->width) + j);
00165         fake_indices_ = false;
00166         use_indices_  = true;
00167       }
00168 
00170       inline IndicesPtr const 
00171       getIndices () { return (indices_); }
00172 
00178       const PointT& operator[] (size_t pos)
00179       {
00180         return ((*input_)[(*indices_)[pos]]);
00181       }
00182 
00183     protected:
00185       PointCloudConstPtr input_;
00186 
00188       IndicesPtr indices_;
00189 
00191       bool use_indices_;
00192 
00194       bool fake_indices_;
00195 
00208       inline bool
00209       initCompute ()
00210       {
00211         // Check if input was set
00212         if (!input_)
00213           return (false);
00214 
00215         // If no point indices have been given, construct a set of indices for the entire input point cloud
00216         if (!indices_)
00217         {
00218           fake_indices_ = true;
00219           indices_.reset (new std::vector<int>);
00220           try
00221           {
00222             indices_->resize (input_->points.size ());
00223           }
00224           catch (std::bad_alloc)
00225           {
00226             PCL_ERROR ("[initCompute] Failed to allocate %lu indices.\n", (unsigned long)input_->points.size ());
00227           }
00228           for (size_t i = 0; i < indices_->size (); ++i) { (*indices_)[i] = (int) i; }
00229         }
00230 
00231         // If we have a set of fake indices, but they do not match the number of points in the cloud, update them
00232         if (fake_indices_ && indices_->size () != input_->points.size ())
00233         {
00234           size_t indices_size = indices_->size ();
00235           indices_->resize (input_->points.size ());
00236           for (size_t i = indices_size; i < indices_->size (); ++i) { (*indices_)[i] = (int) i; }
00237         }
00238 
00239         return (true);
00240       }
00241 
00245       inline bool
00246       deinitCompute ()
00247       {
00248         return (true);
00249       }
00250     public:
00251       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00252   };
00253 
00255   template <>
00256   class PCL_EXPORTS PCLBase<sensor_msgs::PointCloud2>
00257   {
00258     public:
00259       typedef sensor_msgs::PointCloud2 PointCloud2;
00260       typedef PointCloud2::Ptr PointCloud2Ptr;
00261       typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
00262 
00263       typedef PointIndices::Ptr PointIndicesPtr;
00264       typedef PointIndices::ConstPtr PointIndicesConstPtr;
00265 
00267       PCLBase () : input_ (), indices_ (), use_indices_ (false), fake_indices_ (false),
00268                    x_field_name_ ("x"), y_field_name_ ("y"), z_field_name_ ("z")
00269       {};
00270 
00272       virtual ~PCLBase() 
00273       {
00274         input_.reset ();
00275         indices_.reset ();
00276       }
00277 
00281       void 
00282       setInputCloud (const PointCloud2ConstPtr &cloud);
00283 
00285       inline PointCloud2ConstPtr const 
00286       getInputCloud () { return (input_); }
00287 
00291       inline void
00292       setIndices (const IndicesPtr &indices)
00293       {
00294         indices_ = indices;
00295         fake_indices_ = false;
00296         use_indices_  = true;
00297       }
00298 
00302       inline void
00303       setIndices (const PointIndicesConstPtr &indices)
00304       {
00305         indices_.reset (new std::vector<int> (indices->indices));
00306         fake_indices_ = false;
00307         use_indices_  = true;
00308       }
00309 
00311       inline IndicesPtr const 
00312       getIndices () { return (indices_); }
00313 
00314     protected:
00316       PointCloud2ConstPtr input_;
00317 
00319       IndicesPtr indices_;
00320 
00322       bool use_indices_;
00323 
00325       bool fake_indices_;
00326 
00328       std::vector<int> field_sizes_;
00329 
00331       int x_idx_, y_idx_, z_idx_;
00332 
00334       std::string x_field_name_, y_field_name_, z_field_name_;
00335 
00336       bool initCompute ();
00337       bool deinitCompute ();
00338     public:
00339       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00340   };
00341 }
00342 
00343 #endif  //#ifndef PCL_PCL_BASE_H_