Point Cloud Library (PCL)  1.3.1
voxel_grid.hpp
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.hpp 2077 2011-08-15 02:27:39Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_FILTERS_IMPL_VOXEL_GRID_H_
00039 #define PCL_FILTERS_IMPL_VOXEL_GRID_H_
00040 
00041 #include "pcl/common/common.h"
00042 #include "pcl/filters/voxel_grid.h"
00043 
00044 template <typename PointT> void
00045 pcl::getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
00046                   const std::string &distance_field_name, float min_distance, float max_distance,
00047                   Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative)
00048 {
00049   Eigen::Array4f min_p, max_p;
00050   min_p.setConstant (FLT_MAX);
00051   max_p.setConstant (-FLT_MAX);
00052 
00053   // Get the fields list and the distance field index
00054   std::vector<sensor_msgs::PointField> fields;
00055   int distance_idx = pcl::getFieldIndex (*cloud, distance_field_name, fields);
00056 
00057   float distance_value;
00058   // If dense, no need to check for NaNs
00059   if (cloud->is_dense)
00060   {
00061     for (size_t i = 0; i < cloud->points.size (); ++i)
00062     {
00063       // Get the distance value
00064       uint8_t* pt_data = (uint8_t*)&cloud->points[i];
00065       memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
00066 
00067       if (limit_negative)
00068       {
00069         // Use a threshold for cutting out points which inside the interval
00070         if ((distance_value < max_distance) && (distance_value > min_distance))
00071           continue;
00072       }
00073       else
00074       {
00075         // Use a threshold for cutting out points which are too close/far away
00076         if ((distance_value > max_distance) || (distance_value < min_distance))
00077           continue;
00078       }
00079       // Create the point structure and get the min/max
00080       pcl::Array4fMapConst pt = cloud->points[i].getArray4fMap ();
00081       min_p = min_p.min (pt);
00082       max_p = max_p.max (pt);
00083     }
00084   }
00085   else
00086   {
00087     for (size_t i = 0; i < cloud->points.size (); ++i)
00088     {
00089       // Get the distance value
00090       uint8_t* pt_data = (uint8_t*)&cloud->points[i];
00091       memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
00092 
00093       if (limit_negative)
00094       {
00095         // Use a threshold for cutting out points which inside the interval
00096         if ((distance_value < max_distance) && (distance_value > min_distance))
00097           continue;
00098       }
00099       else
00100       {
00101         // Use a threshold for cutting out points which are too close/far away
00102         if ((distance_value > max_distance) || (distance_value < min_distance))
00103           continue;
00104       }
00105 
00106       // Check if the point is invalid
00107       if (!pcl_isfinite (cloud->points[i].x) || 
00108           !pcl_isfinite (cloud->points[i].y) || 
00109           !pcl_isfinite (cloud->points[i].z))
00110         continue;
00111       // Create the point structure and get the min/max
00112       pcl::Array4fMapConst pt = cloud->points[i].getArray4fMap ();
00113       min_p = min_p.min (pt);
00114       max_p = max_p.max (pt);
00115     }
00116   }
00117   min_pt = min_p;
00118   max_pt = max_p;
00119 }
00120 
00122 template <typename PointT> void
00123 pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output)
00124 {
00125   // Has the input dataset been set already?
00126   if (!input_)
00127   {
00128     PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
00129     output.width = output.height = 0;
00130     output.points.clear ();
00131     return;
00132   }
00133 
00134   // Copy the header (and thus the frame_id) + allocate enough space for points
00135   output.height       = 1;                    // downsampling breaks the organized structure
00136   output.is_dense     = true;                 // we filter out invalid points
00137 
00138   Eigen::Vector4f min_p, max_p;
00139   // Get the minimum and maximum dimensions
00140   if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud...
00141     getMinMax3D<PointT>(input_, filter_field_name_, filter_limit_min_, filter_limit_max_, min_p, max_p, filter_limit_negative_);
00142   else
00143     getMinMax3D<PointT>(*input_, min_p, max_p);
00144 
00145   // Compute the minimum and maximum bounding box values
00146 //  min_b_ = (min_p.array () * inverse_leaf_size_).template cast<int> ();
00147 //  max_b_ = (max_p.array () * inverse_leaf_size_).template cast<int> ();
00148   min_b_[0] = (int)(floor (min_p[0] * inverse_leaf_size_[0]));
00149   max_b_[0] = (int)(floor (max_p[0] * inverse_leaf_size_[0]));
00150   min_b_[1] = (int)(floor (min_p[1] * inverse_leaf_size_[1]));
00151   max_b_[1] = (int)(floor (max_p[1] * inverse_leaf_size_[1]));
00152   min_b_[2] = (int)(floor (min_p[2] * inverse_leaf_size_[2]));
00153   max_b_[2] = (int)(floor (max_p[2] * inverse_leaf_size_[2]));
00154 
00155   // Compute the number of divisions needed along all axis
00156   div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
00157   div_b_[3] = 0;
00158 
00159   // Clear the leaves
00160   leaves_.clear ();
00161 
00162   // Set up the division multiplier
00163   divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
00164 
00165   int centroid_size = 4;
00166   if (downsample_all_data_)
00167     centroid_size = boost::mpl::size<FieldList>::value;
00168 
00169   // ---[ RGB special case
00170   std::vector<sensor_msgs::PointField> fields;
00171   int rgba_index = -1;
00172   rgba_index = pcl::getFieldIndex (*input_, "rgb", fields);
00173   if (rgba_index == -1)
00174     rgba_index = pcl::getFieldIndex (*input_, "rgba", fields);
00175   if (rgba_index >= 0)
00176   {
00177     rgba_index = fields[rgba_index].offset;
00178     centroid_size += 3;
00179   }
00180 
00181   // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
00182   if (!filter_field_name_.empty ())
00183   {
00184     // Get the distance field index
00185     std::vector<sensor_msgs::PointField> fields;
00186     int distance_idx = pcl::getFieldIndex (*input_, filter_field_name_, fields);
00187     if (distance_idx == -1)
00188       PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
00189 
00190     // First pass: go over all points and insert them into the right leaf
00191     for (size_t cp = 0; cp < input_->points.size (); ++cp)
00192     {
00193       if (!input_->is_dense)
00194         // Check if the point is invalid
00195         if (!pcl_isfinite (input_->points[cp].x) || 
00196             !pcl_isfinite (input_->points[cp].y) || 
00197             !pcl_isfinite (input_->points[cp].z))
00198           continue;
00199 
00200       // Get the distance value
00201       uint8_t* pt_data = (uint8_t*)&input_->points[cp];
00202       float distance_value = 0;
00203       memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
00204 
00205       if (filter_limit_negative_)
00206       {
00207         // Use a threshold for cutting out points which inside the interval
00208         if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
00209           continue;
00210       }
00211       else
00212       {
00213         // Use a threshold for cutting out points which are too close/far away
00214         if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
00215           continue;
00216       }
00217 
00218       Eigen::Vector4i ijk = Eigen::Vector4i::Zero ();
00219       ijk[0] = (int)(floor (input_->points[cp].x * inverse_leaf_size_[0]));
00220       ijk[1] = (int)(floor (input_->points[cp].y * inverse_leaf_size_[1]));
00221       ijk[2] = (int)(floor (input_->points[cp].z * inverse_leaf_size_[2]));
00222 
00223       // Compute the centroid leaf index
00224       int idx = (ijk - min_b_).dot (divb_mul_);
00225 //      int idx = (((input_->points[cp].getArray4fMap () * inverse_leaf_size_).template cast<int> ()).matrix () - min_b_).dot (divb_mul_);
00226       Leaf& leaf = leaves_[idx];
00227       if (leaf.nr_points == 0)
00228       {
00229         leaf.centroid.resize (centroid_size);
00230         leaf.centroid.setZero ();
00231       }
00232 
00233       // Do we need to process all the fields?
00234       if (!downsample_all_data_)
00235       {
00236         Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0);
00237         leaf.centroid.template head<4> () += pt;
00238       }
00239       else
00240       {
00241         // Copy all the fields
00242         Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
00243         // ---[ RGB special case
00244         if (rgba_index >= 0)
00245         {
00246           // fill r/g/b data
00247           pcl::RGB rgb;
00248           memcpy (&rgb, ((char *)&(input_->points[cp])) + rgba_index, sizeof (RGB));
00249           centroid[centroid_size-3] = rgb.r;
00250           centroid[centroid_size-2] = rgb.g;
00251           centroid[centroid_size-1] = rgb.b;
00252         }
00253         pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <PointT> (input_->points[cp], centroid));
00254         leaf.centroid += centroid;
00255       }
00256       ++leaf.nr_points;
00257     }
00258   }
00259   // No distance filtering, process all data
00260   else
00261   {
00262     // First pass: go over all points and insert them into the right leaf
00263     for (size_t cp = 0; cp < input_->points.size (); ++cp)
00264     {
00265       if (!input_->is_dense)
00266         // Check if the point is invalid
00267         if (!pcl_isfinite (input_->points[cp].x) || 
00268             !pcl_isfinite (input_->points[cp].y) || 
00269             !pcl_isfinite (input_->points[cp].z))
00270           continue;
00271 
00272       Eigen::Vector4i ijk = Eigen::Vector4i::Zero ();
00273       ijk[0] = (int)(floor (input_->points[cp].x * inverse_leaf_size_[0]));
00274       ijk[1] = (int)(floor (input_->points[cp].y * inverse_leaf_size_[1]));
00275       ijk[2] = (int)(floor (input_->points[cp].z * inverse_leaf_size_[2]));
00276 
00277       // Compute the centroid leaf index
00278       int idx = (ijk - min_b_).dot (divb_mul_);
00279       //int idx = (((input_->points[cp].getArray4fMap () * inverse_leaf_size_).template cast<int> ()).matrix () - min_b_).dot (divb_mul_);
00280       Leaf& leaf = leaves_[idx];
00281       if (leaf.nr_points == 0)
00282       {
00283         leaf.centroid.resize (centroid_size);
00284         leaf.centroid.setZero ();
00285       }
00286 
00287       // Do we need to process all the fields?
00288       if (!downsample_all_data_)
00289       {
00290         Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0);
00291         leaf.centroid.template head<4> () += pt;
00292       }
00293       else
00294       {
00295         // Copy all the fields
00296         Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
00297         // ---[ RGB special case
00298         if (rgba_index >= 0)
00299         {
00300           // Fill r/g/b data, assuming that the order is BGRA
00301           pcl::RGB rgb;
00302           memcpy (&rgb, ((char *)&(input_->points[cp])) + rgba_index, sizeof (RGB));
00303           centroid[centroid_size-3] = rgb.r;
00304           centroid[centroid_size-2] = rgb.g;
00305           centroid[centroid_size-1] = rgb.b;
00306         }
00307         pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <PointT> (input_->points[cp], centroid));
00308         leaf.centroid += centroid;
00309       }
00310       ++leaf.nr_points;
00311     }
00312   }
00313 
00314   // Second pass: go over all leaves and compute centroids
00315   output.points.resize (leaves_.size ());
00316   int cp = 0, i = 0;
00317   if (save_leaf_layout_)
00318   {
00319     try
00320     {
00321       leaf_layout_.resize (div_b_[0]*div_b_[1]*div_b_[2], -1);
00322     }
00323     catch (std::bad_alloc&)
00324     {
00325       throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout", 
00326         "voxel_grid.hpp", "applyFilter"); 
00327     }
00328   }
00329   
00330   for (typename boost::unordered_map<size_t, Leaf>::const_iterator it = leaves_.begin (); it != leaves_.end (); ++it)
00331   {
00332     // Save leaf layout information for fast access to cells relative to current position
00333     if (save_leaf_layout_)
00334       leaf_layout_[it->first] = cp++;
00335 
00336     // Normalize the centroid
00337     const Leaf& leaf = it->second;
00338 
00339     // Normalize the centroid
00340     Eigen::VectorXf centroid = leaf.centroid / leaf.nr_points;
00341 
00342     // Do we need to process all the fields?
00343     if (!downsample_all_data_)
00344     {
00345       output.points[i].x = centroid[0];
00346       output.points[i].y = centroid[1];
00347       output.points[i].z = centroid[2];
00348     }
00349     else
00350     {
00351       pcl::for_each_type <FieldList> (pcl::NdCopyEigenPointFunctor <PointT> (centroid, output.points[i]));
00352       // ---[ RGB special case
00353       if (rgba_index >= 0)
00354       {
00355         // pack r/g/b into rgb
00356         float r = centroid[centroid_size-3], g = centroid[centroid_size-2], b = centroid[centroid_size-1];
00357         int rgb = ((int)r) << 16 | ((int)g) << 8 | ((int)b);
00358         memcpy (((char *)&output.points[i]) + rgba_index, &rgb, sizeof (float));
00359       }
00360     }
00361     i++;
00362   }
00363   output.width = output.points.size ();
00364 }
00365 
00366 #define PCL_INSTANTIATE_VoxelGrid(T) template class PCL_EXPORTS pcl::VoxelGrid<T>;
00367 #define PCL_INSTANTIATE_getMinMax3D(T) template PCL_EXPORTS void pcl::getMinMax3D<T> (const pcl::PointCloud<T>::ConstPtr &, const std::string &, float, float, Eigen::Vector4f &, Eigen::Vector4f &, bool);
00368 
00369 #endif    // PCL_FILTERS_IMPL_VOXEL_GRID_H_
00370 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines