Point Cloud Library (PCL)  1.3.1
voxel_grid.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: voxel_grid.h 3028 2011-11-01 04:12:17Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_
00039 #define PCL_FILTERS_VOXEL_GRID_MAP_H_
00040 
00041 #include "pcl/filters/filter.h"
00042 #include <map>
00043 #include <boost/unordered_map.hpp>
00044 #include <boost/mpl/size.hpp>
00045 #include <boost/fusion/sequence/intrinsic/at_key.hpp>
00046 
00047 namespace pcl
00048 {
00049   PCL_EXPORTS void 
00050   getMinMax3D (const sensor_msgs::PointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx, 
00051                Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
00052 
00053   PCL_EXPORTS void 
00054   getMinMax3D (const sensor_msgs::PointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx, 
00055                const std::string &distance_field_name, float min_distance, float max_distance, 
00056                Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false);
00057 
00068   template <typename PointT> void 
00069   getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 
00070                const std::string &distance_field_name, float min_distance, float max_distance,
00071                Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false);
00072 
00074 
00075   template <typename PointT>
00076   struct NdCopyEigenPointFunctor
00077   {
00078     typedef typename traits::POD<PointT>::type Pod;
00079     
00080     NdCopyEigenPointFunctor (const Eigen::VectorXf &p1, PointT &p2)
00081       : p1_ (p1),
00082         p2_ (reinterpret_cast<Pod&>(p2)),
00083         f_idx_ (0) { }
00084 
00085     template<typename Key> inline void operator() ()
00086     {
00087       //boost::fusion::at_key<Key> (p2_) = p1_[f_idx_++];
00088       typedef typename pcl::traits::datatype<PointT, Key>::type T;
00089       uint8_t* data_ptr = reinterpret_cast<uint8_t*>(&p2_) + pcl::traits::offset<PointT, Key>::value;
00090       *reinterpret_cast<T*>(data_ptr) = p1_[f_idx_++];
00091     }
00092 
00093     private:
00094       const Eigen::VectorXf &p1_;
00095       Pod &p2_;
00096       int f_idx_;
00097   };
00098 
00100   template <typename PointT>
00101   struct NdCopyPointEigenFunctor
00102   {
00103     typedef typename traits::POD<PointT>::type Pod;
00104     
00105     NdCopyPointEigenFunctor (const PointT &p1, Eigen::VectorXf &p2)
00106       : p1_ (reinterpret_cast<const Pod&>(p1)), p2_ (p2), f_idx_ (0) { }
00107 
00108     template<typename Key> inline void operator() ()
00109     {
00110       //p2_[f_idx_++] = boost::fusion::at_key<Key> (p1_);
00111       typedef typename pcl::traits::datatype<PointT, Key>::type T;
00112       const uint8_t* data_ptr = reinterpret_cast<const uint8_t*>(&p1_) + pcl::traits::offset<PointT, Key>::value;
00113       p2_[f_idx_++] = *reinterpret_cast<const T*>(data_ptr);
00114     }
00115 
00116     private:
00117       const Pod &p1_;
00118       Eigen::VectorXf &p2_;
00119       int f_idx_;
00120   };
00121 
00134   template <typename PointT>
00135   class VoxelGrid: public Filter<PointT>
00136   {
00137     using Filter<PointT>::filter_name_;
00138     using Filter<PointT>::getClassName;
00139     using Filter<PointT>::input_;
00140     using Filter<PointT>::indices_;
00141     using Filter<PointT>::filter_limit_negative_;
00142     using Filter<PointT>::filter_limit_min_;
00143     using Filter<PointT>::filter_limit_max_;
00144     using Filter<PointT>::filter_field_name_;
00145 
00146     typedef typename Filter<PointT>::PointCloud PointCloud;
00147     typedef typename PointCloud::Ptr PointCloudPtr;
00148     typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00149 
00150     public:
00152       VoxelGrid () : downsample_all_data_ (true), save_leaf_layout_ (false)
00153       {
00154         leaf_size_.setZero ();
00155         min_b_.setZero ();
00156         max_b_.setZero ();
00157         filter_name_ = "VoxelGrid";
00158       }
00159 
00161       virtual ~VoxelGrid ()
00162       {
00163         leaves_.clear();
00164       }
00165 
00169       inline void 
00170       setLeafSize (const Eigen::Vector4f &leaf_size) 
00171       { 
00172         leaf_size_ = leaf_size; 
00173         // Avoid division errors
00174         if (leaf_size_[3] == 0)
00175           leaf_size_[3] = 1;
00176         // Use multiplications instead of divisions
00177         inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
00178       }
00179 
00185       inline void
00186       setLeafSize (float lx, float ly, float lz)
00187       {
00188         leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
00189         // Avoid division errors
00190         if (leaf_size_[3] == 0)
00191           leaf_size_[3] = 1;
00192         // Use multiplications instead of divisions
00193         inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
00194       }
00195 
00197       inline Eigen::Vector3f 
00198       getLeafSize () { return (leaf_size_.head<3> ()); }
00199 
00203       inline void 
00204       setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
00205 
00209       inline bool 
00210       getDownsampleAllData () { return (downsample_all_data_); }
00211 
00215       inline void 
00216       setSaveLeafLayout (bool save_leaf_layout) { save_leaf_layout_ = save_leaf_layout; }
00217 
00219       inline bool 
00220       getSaveLeafLayout () { return (save_leaf_layout_); }
00221 
00225       inline Eigen::Vector3i 
00226       getMinBoxCoordinates () { return (min_b_.head<3> ()); }
00227 
00231       inline Eigen::Vector3i 
00232       getMaxBoxCoordinates () { return (max_b_.head<3> ()); }
00233 
00237       inline Eigen::Vector3i 
00238       getNrDivisions () { return (div_b_.head<3> ()); }
00239 
00243       inline Eigen::Vector3i 
00244       getDivisionMultiplier () { return (divb_mul_.head<3> ()); }
00245 
00254       inline int 
00255       getCentroidIndex (const PointT &p)
00256       {
00257         return leaf_layout_.at ((Eigen::Vector4i (floor (p.x / leaf_size_[0]), floor (p.y / leaf_size_[1]), floor (p.z / leaf_size_[2]), 0) - min_b_).dot (divb_mul_));
00258       }
00259 
00266       inline std::vector<int> 
00267       getNeighborCentroidIndices (const PointT &reference_point, const Eigen::MatrixXi &relative_coordinates)
00268       {
00269         Eigen::Vector4i ijk (floor (reference_point.x / leaf_size_[0]), floor (reference_point.y / leaf_size_[1]), floor (reference_point.z / leaf_size_[2]), 0);
00270         Eigen::Array4i diff2min = min_b_ - ijk;
00271         Eigen::Array4i diff2max = max_b_ - ijk;
00272         std::vector<int> neighbors (relative_coordinates.cols());
00273         for (int ni = 0; ni < relative_coordinates.cols (); ni++)
00274         {
00275           Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
00276           // checking if the specified cell is in the grid
00277           if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
00278             neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted
00279           else
00280             neighbors[ni] = -1; // cell is out of bounds, consider it empty
00281         }
00282         return neighbors;
00283       }
00284 
00288       inline std::vector<int> 
00289       getLeafLayout () { return (leaf_layout_); }
00290 
00292       inline Eigen::Vector3i 
00293       getGridCoordinates (float x, float y, float z) 
00294       { 
00295         return Eigen::Vector3i (floor (x / leaf_size_[0]), floor (y / leaf_size_[1]), floor (z / leaf_size_[2])); 
00296       }
00297 
00299       inline int 
00300       getCentroidIndexAt (const Eigen::Vector3i &ijk, bool verbose = true)
00301       {
00302         int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
00303         if (idx < 0 || idx >= (int)leaf_layout_.size ()) // this checks also if leaf_layout_.size () == 0 i.e. everything was computed as needed
00304         {
00305           if (verbose)
00306             PCL_ERROR ("[pcl::%s::getCentroidIndexAt] Specified coordinate is outside grid bounds, or leaf layout is not saved, make sure to call setSaveLeafLayout(true) and filter(output) first!\n", getClassName ().c_str ());
00307           return -1;
00308         }
00309         return leaf_layout_[idx];
00310       }
00311 
00312     protected:
00315       struct Leaf
00316       {
00317         Leaf () : nr_points(0) {}
00318         Eigen::VectorXf centroid;    // @todo we do not support FLOAT64 just yet due to memory issues. Need to fix this.
00319         int nr_points;
00320       };
00321 
00323       boost::unordered_map<size_t, Leaf> leaves_;
00324 
00326       Eigen::Vector4f leaf_size_;
00327 
00329       Eigen::Array4f inverse_leaf_size_;
00330 
00332       bool downsample_all_data_;
00333 
00335       bool save_leaf_layout_;
00336 
00338       std::vector<int> leaf_layout_;
00339 
00341       Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;
00342 
00343       typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00344 
00348       void 
00349       applyFilter (PointCloud &output);
00350   };
00351 
00364   template <>
00365   class PCL_EXPORTS VoxelGrid<sensor_msgs::PointCloud2> : public Filter<sensor_msgs::PointCloud2>
00366   {
00367     using Filter<sensor_msgs::PointCloud2>::filter_name_;
00368     using Filter<sensor_msgs::PointCloud2>::getClassName;
00369 
00370     typedef sensor_msgs::PointCloud2 PointCloud2;
00371     typedef PointCloud2::Ptr PointCloud2Ptr;
00372     typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
00373 
00374     public:
00376       VoxelGrid () : downsample_all_data_ (true), save_leaf_layout_ (false)
00377       {
00378         leaf_size_.setZero ();
00379         min_b_.setZero ();
00380         max_b_.setZero ();
00381         filter_name_ = "VoxelGrid";
00382       }
00383 
00385       virtual ~VoxelGrid ()
00386       {
00387         leaves_.clear();
00388       }
00389 
00393       inline void 
00394       setLeafSize (const Eigen::Vector4f &leaf_size) 
00395       { 
00396         leaf_size_ = leaf_size; 
00397         // Avoid division errors
00398         if (leaf_size_[3] == 0)
00399           leaf_size_[3] = 1;
00400         // Use multiplications instead of divisions
00401         inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
00402       }
00403 
00409       inline void
00410       setLeafSize (float lx, float ly, float lz)
00411       {
00412         leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
00413         // Avoid division errors
00414         if (leaf_size_[3] == 0)
00415           leaf_size_[3] = 1;
00416         // Use multiplications instead of divisions
00417         inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
00418       }
00419 
00421       inline Eigen::Vector3f 
00422       getLeafSize () { return (leaf_size_.head<3> ()); }
00423 
00427       inline void 
00428       setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
00429 
00433       inline bool 
00434       getDownsampleAllData () { return (downsample_all_data_); }
00435 
00439       inline void 
00440       setSaveLeafLayout (bool save_leaf_layout) { save_leaf_layout_ = save_leaf_layout; }
00441 
00443       inline bool 
00444       getSaveLeafLayout () { return (save_leaf_layout_); }
00445 
00449       inline Eigen::Vector3i 
00450       getMinBoxCoordinates () { return (min_b_.head<3> ()); }
00451 
00455       inline Eigen::Vector3i 
00456       getMaxBoxCoordinates () { return (max_b_.head<3> ()); }
00457 
00461       inline Eigen::Vector3i 
00462       getNrDivisions () { return (div_b_.head<3> ()); }
00463 
00467       inline Eigen::Vector3i 
00468       getDivisionMultiplier () { return (divb_mul_.head<3> ()); }
00469 
00474       inline int 
00475       getCentroidIndex (float x, float y, float z)
00476       {
00477         return leaf_layout_.at ((Eigen::Vector4i ((int) floor (x / leaf_size_[0]), (int) floor (y / leaf_size_[1]), (int) floor (z / leaf_size_[2]), 0) - min_b_).dot (divb_mul_));
00478       }
00479 
00488       inline std::vector<int> 
00489       getNeighborCentroidIndices (float x, float y, float z, const Eigen::MatrixXi &relative_coordinates)
00490       {
00491         Eigen::Vector4i ijk ((int) floor (x / leaf_size_[0]), (int) floor (y / leaf_size_[1]), (int) floor (z / leaf_size_[2]), 0);
00492         Eigen::Array4i diff2min = min_b_ - ijk;
00493         Eigen::Array4i diff2max = max_b_ - ijk;
00494         std::vector<int> neighbors (relative_coordinates.cols());
00495         for (int ni = 0; ni < relative_coordinates.cols (); ni++)
00496         {
00497           Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
00498           // checking if the specified cell is in the grid
00499           if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
00500             neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted
00501           else
00502             neighbors[ni] = -1; // cell is out of bounds, consider it empty
00503         }
00504         return neighbors;
00505       }
00506       
00507       inline std::vector<int> 
00508       getNeighborCentroidIndices (float x, float y, float z, const std::vector<Eigen::Vector3i> &relative_coordinates)
00509       {
00510         Eigen::Vector4i ijk ((int) floor (x / leaf_size_[0]), (int) floor (y / leaf_size_[1]), (int) floor (z / leaf_size_[2]), 0);
00511         std::vector<int> neighbors;
00512         neighbors.reserve (relative_coordinates.size ());
00513         for (std::vector<Eigen::Vector3i>::const_iterator it = relative_coordinates.begin (); it != relative_coordinates.end (); it++)
00514           neighbors.push_back (leaf_layout_[(ijk + (Eigen::Vector4i() << *it, 0).finished() - min_b_).dot (divb_mul_)]);
00515         return neighbors;
00516       }
00517 
00521       inline std::vector<int> 
00522       getLeafLayout () { return (leaf_layout_); }
00523 
00525       inline Eigen::Vector3i 
00526       getGridCoordinates (float x, float y, float z) 
00527       { 
00528         return Eigen::Vector3i ((int) floor (x / leaf_size_[0]), (int) floor (y / leaf_size_[1]), (int) floor (z / leaf_size_[2])); 
00529       }
00530 
00532       inline int 
00533       getCentroidIndexAt (const Eigen::Vector3i &ijk, bool verbose = true)
00534       {
00535         int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
00536         if (idx < 0 || idx >= (int)leaf_layout_.size ()) // this checks also if leaf_layout_.size () == 0 i.e. everything was computed as needed
00537         {
00538           if (verbose)
00539             PCL_ERROR ("[pcl::%s::getCentroidIndexAt] Specified coordinate is outside grid bounds, or leaf layout is not saved, make sure to call setSaveLeafLayout(true) and filter(output) first!\n", getClassName ().c_str ());
00540           return -1;
00541         }
00542         return leaf_layout_[idx];
00543       }
00544 
00545     protected:
00548       struct Leaf
00549       {
00550         Leaf () : nr_points(0) { }
00551         Eigen::VectorXf centroid;    // @todo we do not support FLOAT64 just yet due to memory issues. Need to fix this.
00552         int nr_points;
00553       };
00554 
00556       boost::unordered_map<size_t, Leaf> leaves_;
00557 
00559       Eigen::Vector4f leaf_size_;
00560 
00562       Eigen::Array4f inverse_leaf_size_;
00563 
00565       bool downsample_all_data_;
00566 
00570       bool save_leaf_layout_;
00571 
00575       std::vector<int> leaf_layout_;
00576 
00580       Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;
00581 
00585       void 
00586       applyFilter (PointCloud2 &output);
00587   };
00588 }
00589 
00590 #endif  //#ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines