Point Cloud Library (PCL)
1.3.1
|
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_