Point Cloud Library (PCL)
1.7.0
|
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 the copyright holder(s) 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$ 00037 * 00038 */ 00039 00040 #ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_ 00041 #define PCL_FILTERS_VOXEL_GRID_MAP_H_ 00042 00043 #include <pcl/filters/boost.h> 00044 #include <pcl/filters/filter.h> 00045 #include <map> 00046 00047 namespace pcl 00048 { 00049 /** \brief Obtain the maximum and minimum points in 3D from a given point cloud. 00050 * \param[in] cloud the pointer to a pcl::PCLPointCloud2 dataset 00051 * \param[in] x_idx the index of the X channel 00052 * \param[in] y_idx the index of the Y channel 00053 * \param[in] z_idx the index of the Z channel 00054 * \param[out] min_pt the minimum data point 00055 * \param[out] max_pt the maximum data point 00056 */ 00057 PCL_EXPORTS void 00058 getMinMax3D (const pcl::PCLPointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx, 00059 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt); 00060 00061 /** \brief Obtain the maximum and minimum points in 3D from a given point cloud. 00062 * \note Performs internal data filtering as well. 00063 * \param[in] cloud the pointer to a pcl::PCLPointCloud2 dataset 00064 * \param[in] x_idx the index of the X channel 00065 * \param[in] y_idx the index of the Y channel 00066 * \param[in] z_idx the index of the Z channel 00067 * \param[in] distance_field_name the name of the dimension to filter data along to 00068 * \param[in] min_distance the minimum acceptable value in \a distance_field_name data 00069 * \param[in] max_distance the maximum acceptable value in \a distance_field_name data 00070 * \param[out] min_pt the minimum data point 00071 * \param[out] max_pt the maximum data point 00072 * \param[in] limit_negative \b false if data \b inside of the [min_distance; max_distance] interval should be 00073 * considered, \b true otherwise. 00074 */ 00075 PCL_EXPORTS void 00076 getMinMax3D (const pcl::PCLPointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx, 00077 const std::string &distance_field_name, float min_distance, float max_distance, 00078 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false); 00079 00080 /** \brief Get the relative cell indices of the "upper half" 13 neighbors. 00081 * \note Useful in combination with getNeighborCentroidIndices() from \ref VoxelGrid 00082 * \ingroup filters 00083 */ 00084 inline Eigen::MatrixXi 00085 getHalfNeighborCellIndices () 00086 { 00087 Eigen::MatrixXi relative_coordinates (3, 13); 00088 int idx = 0; 00089 00090 // 0 - 8 00091 for (int i = -1; i < 2; i++) 00092 { 00093 for (int j = -1; j < 2; j++) 00094 { 00095 relative_coordinates (0, idx) = i; 00096 relative_coordinates (1, idx) = j; 00097 relative_coordinates (2, idx) = -1; 00098 idx++; 00099 } 00100 } 00101 // 9 - 11 00102 for (int i = -1; i < 2; i++) 00103 { 00104 relative_coordinates (0, idx) = i; 00105 relative_coordinates (1, idx) = -1; 00106 relative_coordinates (2, idx) = 0; 00107 idx++; 00108 } 00109 // 12 00110 relative_coordinates (0, idx) = -1; 00111 relative_coordinates (1, idx) = 0; 00112 relative_coordinates (2, idx) = 0; 00113 00114 return (relative_coordinates); 00115 } 00116 00117 /** \brief Get the relative cell indices of all the 26 neighbors. 00118 * \note Useful in combination with getNeighborCentroidIndices() from \ref VoxelGrid 00119 * \ingroup filters 00120 */ 00121 inline Eigen::MatrixXi 00122 getAllNeighborCellIndices () 00123 { 00124 Eigen::MatrixXi relative_coordinates = getHalfNeighborCellIndices (); 00125 Eigen::MatrixXi relative_coordinates_all( 3, 26); 00126 relative_coordinates_all.block<3, 13> (0, 0) = relative_coordinates; 00127 relative_coordinates_all.block<3, 13> (0, 13) = -relative_coordinates; 00128 return (relative_coordinates_all); 00129 } 00130 00131 /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions 00132 * in a given pointcloud, without considering points outside of a distance threshold from the laser origin 00133 * \param[in] cloud the point cloud data message 00134 * \param[in] distance_field_name the field name that contains the distance values 00135 * \param[in] min_distance the minimum distance a point will be considered from 00136 * \param[in] max_distance the maximum distance a point will be considered to 00137 * \param[out] min_pt the resultant minimum bounds 00138 * \param[out] max_pt the resultant maximum bounds 00139 * \param[in] limit_negative if set to true, then all points outside of the interval (min_distance;max_distace) are considered 00140 * \ingroup filters 00141 */ 00142 template <typename PointT> void 00143 getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00144 const std::string &distance_field_name, float min_distance, float max_distance, 00145 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false); 00146 00147 /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions 00148 * in a given pointcloud, without considering points outside of a distance threshold from the laser origin 00149 * \param[in] cloud the point cloud data message 00150 * \param[in] indices the vector of indices to use 00151 * \param[in] distance_field_name the field name that contains the distance values 00152 * \param[in] min_distance the minimum distance a point will be considered from 00153 * \param[in] max_distance the maximum distance a point will be considered to 00154 * \param[out] min_pt the resultant minimum bounds 00155 * \param[out] max_pt the resultant maximum bounds 00156 * \param[in] limit_negative if set to true, then all points outside of the interval (min_distance;max_distace) are considered 00157 * \ingroup filters 00158 */ 00159 template <typename PointT> void 00160 getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00161 const std::vector<int> &indices, 00162 const std::string &distance_field_name, float min_distance, float max_distance, 00163 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false); 00164 00165 /** \brief VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. 00166 * 00167 * The VoxelGrid class creates a *3D voxel grid* (think about a voxel 00168 * grid as a set of tiny 3D boxes in space) over the input point cloud data. 00169 * Then, in each *voxel* (i.e., 3D box), all the points present will be 00170 * approximated (i.e., *downsampled*) with their centroid. This approach is 00171 * a bit slower than approximating them with the center of the voxel, but it 00172 * represents the underlying surface more accurately. 00173 * 00174 * \author Radu B. Rusu, Bastian Steder 00175 * \ingroup filters 00176 */ 00177 template <typename PointT> 00178 class VoxelGrid: public Filter<PointT> 00179 { 00180 protected: 00181 using Filter<PointT>::filter_name_; 00182 using Filter<PointT>::getClassName; 00183 using Filter<PointT>::input_; 00184 using Filter<PointT>::indices_; 00185 00186 typedef typename Filter<PointT>::PointCloud PointCloud; 00187 typedef typename PointCloud::Ptr PointCloudPtr; 00188 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00189 typedef boost::shared_ptr< VoxelGrid<PointT> > Ptr; 00190 typedef boost::shared_ptr< const VoxelGrid<PointT> > ConstPtr; 00191 00192 00193 public: 00194 /** \brief Empty constructor. */ 00195 VoxelGrid () : 00196 leaf_size_ (Eigen::Vector4f::Zero ()), 00197 inverse_leaf_size_ (Eigen::Array4f::Zero ()), 00198 downsample_all_data_ (true), 00199 save_leaf_layout_ (false), 00200 leaf_layout_ (), 00201 min_b_ (Eigen::Vector4i::Zero ()), 00202 max_b_ (Eigen::Vector4i::Zero ()), 00203 div_b_ (Eigen::Vector4i::Zero ()), 00204 divb_mul_ (Eigen::Vector4i::Zero ()), 00205 filter_field_name_ (""), 00206 filter_limit_min_ (-FLT_MAX), 00207 filter_limit_max_ (FLT_MAX), 00208 filter_limit_negative_ (false) 00209 { 00210 filter_name_ = "VoxelGrid"; 00211 } 00212 00213 /** \brief Destructor. */ 00214 virtual ~VoxelGrid () 00215 { 00216 } 00217 00218 /** \brief Set the voxel grid leaf size. 00219 * \param[in] leaf_size the voxel grid leaf size 00220 */ 00221 inline void 00222 setLeafSize (const Eigen::Vector4f &leaf_size) 00223 { 00224 leaf_size_ = leaf_size; 00225 // Avoid division errors 00226 if (leaf_size_[3] == 0) 00227 leaf_size_[3] = 1; 00228 // Use multiplications instead of divisions 00229 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); 00230 } 00231 00232 /** \brief Set the voxel grid leaf size. 00233 * \param[in] lx the leaf size for X 00234 * \param[in] ly the leaf size for Y 00235 * \param[in] lz the leaf size for Z 00236 */ 00237 inline void 00238 setLeafSize (float lx, float ly, float lz) 00239 { 00240 leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz; 00241 // Avoid division errors 00242 if (leaf_size_[3] == 0) 00243 leaf_size_[3] = 1; 00244 // Use multiplications instead of divisions 00245 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); 00246 } 00247 00248 /** \brief Get the voxel grid leaf size. */ 00249 inline Eigen::Vector3f 00250 getLeafSize () { return (leaf_size_.head<3> ()); } 00251 00252 /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. 00253 * \param[in] downsample the new value (true/false) 00254 */ 00255 inline void 00256 setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; } 00257 00258 /** \brief Get the state of the internal downsampling parameter (true if 00259 * all fields need to be downsampled, false if just XYZ). 00260 */ 00261 inline bool 00262 getDownsampleAllData () { return (downsample_all_data_); } 00263 00264 /** \brief Set to true if leaf layout information needs to be saved for later access. 00265 * \param[in] save_leaf_layout the new value (true/false) 00266 */ 00267 inline void 00268 setSaveLeafLayout (bool save_leaf_layout) { save_leaf_layout_ = save_leaf_layout; } 00269 00270 /** \brief Returns true if leaf layout information will to be saved for later access. */ 00271 inline bool 00272 getSaveLeafLayout () { return (save_leaf_layout_); } 00273 00274 /** \brief Get the minimum coordinates of the bounding box (after 00275 * filtering is performed). 00276 */ 00277 inline Eigen::Vector3i 00278 getMinBoxCoordinates () { return (min_b_.head<3> ()); } 00279 00280 /** \brief Get the minimum coordinates of the bounding box (after 00281 * filtering is performed). 00282 */ 00283 inline Eigen::Vector3i 00284 getMaxBoxCoordinates () { return (max_b_.head<3> ()); } 00285 00286 /** \brief Get the number of divisions along all 3 axes (after filtering 00287 * is performed). 00288 */ 00289 inline Eigen::Vector3i 00290 getNrDivisions () { return (div_b_.head<3> ()); } 00291 00292 /** \brief Get the multipliers to be applied to the grid coordinates in 00293 * order to find the centroid index (after filtering is performed). 00294 */ 00295 inline Eigen::Vector3i 00296 getDivisionMultiplier () { return (divb_mul_.head<3> ()); } 00297 00298 /** \brief Returns the index in the resulting downsampled cloud of the specified point. 00299 * 00300 * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering 00301 * performed, and that the point is inside the grid, to avoid invalid access (or use 00302 * getGridCoordinates+getCentroidIndexAt) 00303 * 00304 * \param[in] p the point to get the index at 00305 */ 00306 inline int 00307 getCentroidIndex (const PointT &p) 00308 { 00309 return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (floor (p.x * inverse_leaf_size_[0])), 00310 static_cast<int> (floor (p.y * inverse_leaf_size_[1])), 00311 static_cast<int> (floor (p.z * inverse_leaf_size_[2])), 0) - min_b_).dot (divb_mul_))); 00312 } 00313 00314 /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates, 00315 * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds). 00316 * \param[in] reference_point the coordinates of the reference point (corresponding cell is allowed to be empty/out of bounds) 00317 * \param[in] relative_coordinates matrix with the columns being the coordinates of the requested cells, relative to the reference point's cell 00318 * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed 00319 */ 00320 inline std::vector<int> 00321 getNeighborCentroidIndices (const PointT &reference_point, const Eigen::MatrixXi &relative_coordinates) 00322 { 00323 Eigen::Vector4i ijk (static_cast<int> (floor (reference_point.x * inverse_leaf_size_[0])), 00324 static_cast<int> (floor (reference_point.y * inverse_leaf_size_[1])), 00325 static_cast<int> (floor (reference_point.z * inverse_leaf_size_[2])), 0); 00326 Eigen::Array4i diff2min = min_b_ - ijk; 00327 Eigen::Array4i diff2max = max_b_ - ijk; 00328 std::vector<int> neighbors (relative_coordinates.cols()); 00329 for (int ni = 0; ni < relative_coordinates.cols (); ni++) 00330 { 00331 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished(); 00332 // checking if the specified cell is in the grid 00333 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all()) 00334 neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted 00335 else 00336 neighbors[ni] = -1; // cell is out of bounds, consider it empty 00337 } 00338 return (neighbors); 00339 } 00340 00341 /** \brief Returns the layout of the leafs for fast access to cells relative to current position. 00342 * \note position at (i-min_x) + (j-min_y)*div_x + (k-min_z)*div_x*div_y holds the index of the element at coordinates (i,j,k) in the grid (-1 if empty) 00343 */ 00344 inline std::vector<int> 00345 getLeafLayout () { return (leaf_layout_); } 00346 00347 /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z). 00348 * \param[in] x the X point coordinate to get the (i, j, k) index at 00349 * \param[in] y the Y point coordinate to get the (i, j, k) index at 00350 * \param[in] z the Z point coordinate to get the (i, j, k) index at 00351 */ 00352 inline Eigen::Vector3i 00353 getGridCoordinates (float x, float y, float z) 00354 { 00355 return (Eigen::Vector3i (static_cast<int> (floor (x * inverse_leaf_size_[0])), 00356 static_cast<int> (floor (y * inverse_leaf_size_[1])), 00357 static_cast<int> (floor (z * inverse_leaf_size_[2])))); 00358 } 00359 00360 /** \brief Returns the index in the downsampled cloud corresponding to a given set of coordinates. 00361 * \param[in] ijk the coordinates (i,j,k) in the grid (-1 if empty) 00362 */ 00363 inline int 00364 getCentroidIndexAt (const Eigen::Vector3i &ijk) 00365 { 00366 int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_); 00367 if (idx < 0 || idx >= static_cast<int> (leaf_layout_.size ())) // this checks also if leaf_layout_.size () == 0 i.e. everything was computed as needed 00368 { 00369 //if (verbose) 00370 // 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 ()); 00371 return (-1); 00372 } 00373 return (leaf_layout_[idx]); 00374 } 00375 00376 /** \brief Provide the name of the field to be used for filtering data. In conjunction with \a setFilterLimits, 00377 * points having values outside this interval will be discarded. 00378 * \param[in] field_name the name of the field that contains values used for filtering 00379 */ 00380 inline void 00381 setFilterFieldName (const std::string &field_name) 00382 { 00383 filter_field_name_ = field_name; 00384 } 00385 00386 /** \brief Get the name of the field used for filtering. */ 00387 inline std::string const 00388 getFilterFieldName () 00389 { 00390 return (filter_field_name_); 00391 } 00392 00393 /** \brief Set the field filter limits. All points having field values outside this interval will be discarded. 00394 * \param[in] limit_min the minimum allowed field value 00395 * \param[in] limit_max the maximum allowed field value 00396 */ 00397 inline void 00398 setFilterLimits (const double &limit_min, const double &limit_max) 00399 { 00400 filter_limit_min_ = limit_min; 00401 filter_limit_max_ = limit_max; 00402 } 00403 00404 /** \brief Get the field filter limits (min/max) set by the user. The default values are -FLT_MAX, FLT_MAX. 00405 * \param[out] limit_min the minimum allowed field value 00406 * \param[out] limit_max the maximum allowed field value 00407 */ 00408 inline void 00409 getFilterLimits (double &limit_min, double &limit_max) 00410 { 00411 limit_min = filter_limit_min_; 00412 limit_max = filter_limit_max_; 00413 } 00414 00415 /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max). 00416 * Default: false. 00417 * \param[in] limit_negative return data inside the interval (false) or outside (true) 00418 */ 00419 inline void 00420 setFilterLimitsNegative (const bool limit_negative) 00421 { 00422 filter_limit_negative_ = limit_negative; 00423 } 00424 00425 /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). 00426 * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise 00427 */ 00428 inline void 00429 getFilterLimitsNegative (bool &limit_negative) 00430 { 00431 limit_negative = filter_limit_negative_; 00432 } 00433 00434 /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). 00435 * \return true if data \b outside the interval [min; max] is to be returned, false otherwise 00436 */ 00437 inline bool 00438 getFilterLimitsNegative () 00439 { 00440 return (filter_limit_negative_); 00441 } 00442 00443 protected: 00444 /** \brief The size of a leaf. */ 00445 Eigen::Vector4f leaf_size_; 00446 00447 /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */ 00448 Eigen::Array4f inverse_leaf_size_; 00449 00450 /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */ 00451 bool downsample_all_data_; 00452 00453 /** \brief Set to true if leaf layout information needs to be saved in \a leaf_layout_. */ 00454 bool save_leaf_layout_; 00455 00456 /** \brief The leaf layout information for fast access to cells relative to current position **/ 00457 std::vector<int> leaf_layout_; 00458 00459 /** \brief The minimum and maximum bin coordinates, the number of divisions, and the division multiplier. */ 00460 Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_; 00461 00462 /** \brief The desired user filter field name. */ 00463 std::string filter_field_name_; 00464 00465 /** \brief The minimum allowed filter value a point will be considered from. */ 00466 double filter_limit_min_; 00467 00468 /** \brief The maximum allowed filter value a point will be considered from. */ 00469 double filter_limit_max_; 00470 00471 /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */ 00472 bool filter_limit_negative_; 00473 00474 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00475 00476 /** \brief Downsample a Point Cloud using a voxelized grid approach 00477 * \param[out] output the resultant point cloud message 00478 */ 00479 void 00480 applyFilter (PointCloud &output); 00481 }; 00482 00483 /** \brief VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. 00484 * 00485 * The VoxelGrid class creates a *3D voxel grid* (think about a voxel 00486 * grid as a set of tiny 3D boxes in space) over the input point cloud data. 00487 * Then, in each *voxel* (i.e., 3D box), all the points present will be 00488 * approximated (i.e., *downsampled*) with their centroid. This approach is 00489 * a bit slower than approximating them with the center of the voxel, but it 00490 * represents the underlying surface more accurately. 00491 * 00492 * \author Radu B. Rusu, Bastian Steder, Radoslaw Cybulski 00493 * \ingroup filters 00494 */ 00495 template <> 00496 class PCL_EXPORTS VoxelGrid<pcl::PCLPointCloud2> : public Filter<pcl::PCLPointCloud2> 00497 { 00498 using Filter<pcl::PCLPointCloud2>::filter_name_; 00499 using Filter<pcl::PCLPointCloud2>::getClassName; 00500 00501 typedef pcl::PCLPointCloud2 PCLPointCloud2; 00502 typedef PCLPointCloud2::Ptr PCLPointCloud2Ptr; 00503 typedef PCLPointCloud2::ConstPtr PCLPointCloud2ConstPtr; 00504 00505 public: 00506 /** \brief Empty constructor. */ 00507 VoxelGrid () : 00508 leaf_size_ (Eigen::Vector4f::Zero ()), 00509 inverse_leaf_size_ (Eigen::Array4f::Zero ()), 00510 downsample_all_data_ (true), 00511 save_leaf_layout_ (false), 00512 leaf_layout_ (), 00513 min_b_ (Eigen::Vector4i::Zero ()), 00514 max_b_ (Eigen::Vector4i::Zero ()), 00515 div_b_ (Eigen::Vector4i::Zero ()), 00516 divb_mul_ (Eigen::Vector4i::Zero ()), 00517 filter_field_name_ (""), 00518 filter_limit_min_ (-FLT_MAX), 00519 filter_limit_max_ (FLT_MAX), 00520 filter_limit_negative_ (false) 00521 { 00522 filter_name_ = "VoxelGrid"; 00523 } 00524 00525 /** \brief Destructor. */ 00526 virtual ~VoxelGrid () 00527 { 00528 } 00529 00530 /** \brief Set the voxel grid leaf size. 00531 * \param[in] leaf_size the voxel grid leaf size 00532 */ 00533 inline void 00534 setLeafSize (const Eigen::Vector4f &leaf_size) 00535 { 00536 leaf_size_ = leaf_size; 00537 // Avoid division errors 00538 if (leaf_size_[3] == 0) 00539 leaf_size_[3] = 1; 00540 // Use multiplications instead of divisions 00541 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); 00542 } 00543 00544 /** \brief Set the voxel grid leaf size. 00545 * \param[in] lx the leaf size for X 00546 * \param[in] ly the leaf size for Y 00547 * \param[in] lz the leaf size for Z 00548 */ 00549 inline void 00550 setLeafSize (float lx, float ly, float lz) 00551 { 00552 leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz; 00553 // Avoid division errors 00554 if (leaf_size_[3] == 0) 00555 leaf_size_[3] = 1; 00556 // Use multiplications instead of divisions 00557 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); 00558 } 00559 00560 /** \brief Get the voxel grid leaf size. */ 00561 inline Eigen::Vector3f 00562 getLeafSize () { return (leaf_size_.head<3> ()); } 00563 00564 /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. 00565 * \param[in] downsample the new value (true/false) 00566 */ 00567 inline void 00568 setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; } 00569 00570 /** \brief Get the state of the internal downsampling parameter (true if 00571 * all fields need to be downsampled, false if just XYZ). 00572 */ 00573 inline bool 00574 getDownsampleAllData () { return (downsample_all_data_); } 00575 00576 /** \brief Set to true if leaf layout information needs to be saved for later access. 00577 * \param[in] save_leaf_layout the new value (true/false) 00578 */ 00579 inline void 00580 setSaveLeafLayout (bool save_leaf_layout) { save_leaf_layout_ = save_leaf_layout; } 00581 00582 /** \brief Returns true if leaf layout information will to be saved for later access. */ 00583 inline bool 00584 getSaveLeafLayout () { return (save_leaf_layout_); } 00585 00586 /** \brief Get the minimum coordinates of the bounding box (after 00587 * filtering is performed). 00588 */ 00589 inline Eigen::Vector3i 00590 getMinBoxCoordinates () { return (min_b_.head<3> ()); } 00591 00592 /** \brief Get the minimum coordinates of the bounding box (after 00593 * filtering is performed). 00594 */ 00595 inline Eigen::Vector3i 00596 getMaxBoxCoordinates () { return (max_b_.head<3> ()); } 00597 00598 /** \brief Get the number of divisions along all 3 axes (after filtering 00599 * is performed). 00600 */ 00601 inline Eigen::Vector3i 00602 getNrDivisions () { return (div_b_.head<3> ()); } 00603 00604 /** \brief Get the multipliers to be applied to the grid coordinates in 00605 * order to find the centroid index (after filtering is performed). 00606 */ 00607 inline Eigen::Vector3i 00608 getDivisionMultiplier () { return (divb_mul_.head<3> ()); } 00609 00610 /** \brief Returns the index in the resulting downsampled cloud of the specified point. 00611 * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed, 00612 * and that the point is inside the grid, to avoid invalid access (or use getGridCoordinates+getCentroidIndexAt) 00613 * \param[in] x the X point coordinate to get the index at 00614 * \param[in] y the Y point coordinate to get the index at 00615 * \param[in] z the Z point coordinate to get the index at 00616 */ 00617 inline int 00618 getCentroidIndex (float x, float y, float z) 00619 { 00620 return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (floor (x * inverse_leaf_size_[0])), 00621 static_cast<int> (floor (y * inverse_leaf_size_[1])), 00622 static_cast<int> (floor (z * inverse_leaf_size_[2])), 00623 0) 00624 - min_b_).dot (divb_mul_))); 00625 } 00626 00627 /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates, 00628 * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds). 00629 * \param[in] x the X coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds) 00630 * \param[in] y the Y coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds) 00631 * \param[in] z the Z coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds) 00632 * \param[out] relative_coordinates matrix with the columns being the coordinates of the requested cells, relative to the reference point's cell 00633 * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed 00634 */ 00635 inline std::vector<int> 00636 getNeighborCentroidIndices (float x, float y, float z, const Eigen::MatrixXi &relative_coordinates) 00637 { 00638 Eigen::Vector4i ijk (static_cast<int> (floor (x * inverse_leaf_size_[0])), 00639 static_cast<int> (floor (y * inverse_leaf_size_[1])), 00640 static_cast<int> (floor (z * inverse_leaf_size_[2])), 0); 00641 Eigen::Array4i diff2min = min_b_ - ijk; 00642 Eigen::Array4i diff2max = max_b_ - ijk; 00643 std::vector<int> neighbors (relative_coordinates.cols()); 00644 for (int ni = 0; ni < relative_coordinates.cols (); ni++) 00645 { 00646 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished(); 00647 // checking if the specified cell is in the grid 00648 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all()) 00649 neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted 00650 else 00651 neighbors[ni] = -1; // cell is out of bounds, consider it empty 00652 } 00653 return (neighbors); 00654 } 00655 00656 /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates, 00657 * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds). 00658 * \param[in] x the X coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds) 00659 * \param[in] y the Y coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds) 00660 * \param[in] z the Z coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds) 00661 * \param[out] relative_coordinates vector with the elements being the coordinates of the requested cells, relative to the reference point's cell 00662 * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed 00663 */ 00664 inline std::vector<int> 00665 getNeighborCentroidIndices (float x, float y, float z, const std::vector<Eigen::Vector3i> &relative_coordinates) 00666 { 00667 Eigen::Vector4i ijk (static_cast<int> (floorf (x * inverse_leaf_size_[0])), static_cast<int> (floorf (y * inverse_leaf_size_[1])), static_cast<int> (floorf (z * inverse_leaf_size_[2])), 0); 00668 std::vector<int> neighbors; 00669 neighbors.reserve (relative_coordinates.size ()); 00670 for (std::vector<Eigen::Vector3i>::const_iterator it = relative_coordinates.begin (); it != relative_coordinates.end (); it++) 00671 neighbors.push_back (leaf_layout_[(ijk + (Eigen::Vector4i() << *it, 0).finished() - min_b_).dot (divb_mul_)]); 00672 return (neighbors); 00673 } 00674 00675 /** \brief Returns the layout of the leafs for fast access to cells relative to current position. 00676 * \note position at (i-min_x) + (j-min_y)*div_x + (k-min_z)*div_x*div_y holds the index of the element at coordinates (i,j,k) in the grid (-1 if empty) 00677 */ 00678 inline std::vector<int> 00679 getLeafLayout () { return (leaf_layout_); } 00680 00681 /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z). 00682 * \param[in] x the X point coordinate to get the (i, j, k) index at 00683 * \param[in] y the Y point coordinate to get the (i, j, k) index at 00684 * \param[in] z the Z point coordinate to get the (i, j, k) index at 00685 */ 00686 inline Eigen::Vector3i 00687 getGridCoordinates (float x, float y, float z) 00688 { 00689 return (Eigen::Vector3i (static_cast<int> (floor (x * inverse_leaf_size_[0])), 00690 static_cast<int> (floor (y * inverse_leaf_size_[1])), 00691 static_cast<int> (floor (z * inverse_leaf_size_[2])))); 00692 } 00693 00694 /** \brief Returns the index in the downsampled cloud corresponding to a given set of coordinates. 00695 * \param[in] ijk the coordinates (i,j,k) in the grid (-1 if empty) 00696 */ 00697 inline int 00698 getCentroidIndexAt (const Eigen::Vector3i &ijk) 00699 { 00700 int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_); 00701 if (idx < 0 || idx >= static_cast<int> (leaf_layout_.size ())) // this checks also if leaf_layout_.size () == 0 i.e. everything was computed as needed 00702 { 00703 //if (verbose) 00704 // 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 ()); 00705 return (-1); 00706 } 00707 return (leaf_layout_[idx]); 00708 } 00709 00710 /** \brief Provide the name of the field to be used for filtering data. In conjunction with \a setFilterLimits, 00711 * points having values outside this interval will be discarded. 00712 * \param[in] field_name the name of the field that contains values used for filtering 00713 */ 00714 inline void 00715 setFilterFieldName (const std::string &field_name) 00716 { 00717 filter_field_name_ = field_name; 00718 } 00719 00720 /** \brief Get the name of the field used for filtering. */ 00721 inline std::string const 00722 getFilterFieldName () 00723 { 00724 return (filter_field_name_); 00725 } 00726 00727 /** \brief Set the field filter limits. All points having field values outside this interval will be discarded. 00728 * \param[in] limit_min the minimum allowed field value 00729 * \param[in] limit_max the maximum allowed field value 00730 */ 00731 inline void 00732 setFilterLimits (const double &limit_min, const double &limit_max) 00733 { 00734 filter_limit_min_ = limit_min; 00735 filter_limit_max_ = limit_max; 00736 } 00737 00738 /** \brief Get the field filter limits (min/max) set by the user. The default values are -FLT_MAX, FLT_MAX. 00739 * \param[out] limit_min the minimum allowed field value 00740 * \param[out] limit_max the maximum allowed field value 00741 */ 00742 inline void 00743 getFilterLimits (double &limit_min, double &limit_max) 00744 { 00745 limit_min = filter_limit_min_; 00746 limit_max = filter_limit_max_; 00747 } 00748 00749 /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max). 00750 * Default: false. 00751 * \param[in] limit_negative return data inside the interval (false) or outside (true) 00752 */ 00753 inline void 00754 setFilterLimitsNegative (const bool limit_negative) 00755 { 00756 filter_limit_negative_ = limit_negative; 00757 } 00758 00759 /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). 00760 * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise 00761 */ 00762 inline void 00763 getFilterLimitsNegative (bool &limit_negative) 00764 { 00765 limit_negative = filter_limit_negative_; 00766 } 00767 00768 /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). 00769 * \return true if data \b outside the interval [min; max] is to be returned, false otherwise 00770 */ 00771 inline bool 00772 getFilterLimitsNegative () 00773 { 00774 return (filter_limit_negative_); 00775 } 00776 00777 protected: 00778 /** \brief The size of a leaf. */ 00779 Eigen::Vector4f leaf_size_; 00780 00781 /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */ 00782 Eigen::Array4f inverse_leaf_size_; 00783 00784 /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */ 00785 bool downsample_all_data_; 00786 00787 /** \brief Set to true if leaf layout information needs to be saved in \a 00788 * leaf_layout. 00789 */ 00790 bool save_leaf_layout_; 00791 00792 /** \brief The leaf layout information for fast access to cells relative 00793 * to current position 00794 */ 00795 std::vector<int> leaf_layout_; 00796 00797 /** \brief The minimum and maximum bin coordinates, the number of 00798 * divisions, and the division multiplier. 00799 */ 00800 Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_; 00801 00802 /** \brief The desired user filter field name. */ 00803 std::string filter_field_name_; 00804 00805 /** \brief The minimum allowed filter value a point will be considered from. */ 00806 double filter_limit_min_; 00807 00808 /** \brief The maximum allowed filter value a point will be considered from. */ 00809 double filter_limit_max_; 00810 00811 /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */ 00812 bool filter_limit_negative_; 00813 00814 /** \brief Downsample a Point Cloud using a voxelized grid approach 00815 * \param[out] output the resultant point cloud 00816 */ 00817 void 00818 applyFilter (PCLPointCloud2 &output); 00819 }; 00820 } 00821 00822 #ifdef PCL_NO_PRECOMPILE 00823 #include <pcl/filters/impl/voxel_grid.hpp> 00824 #endif 00825 00826 #endif //#ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_