|
Point Cloud Library (PCL)
1.6.0
|
|
Point Cloud Library (PCL)
1.6.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 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: voxel_grid.h 5654 2012-05-01 05:32:03Z rusu $ 00037 * 00038 */ 00039 00040 #ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_ 00041 #define PCL_FILTERS_VOXEL_GRID_MAP_H_ 00042 00043 #include <pcl/filters/filter.h> 00044 #include <map> 00045 #include <boost/unordered_map.hpp> 00046 #include <boost/fusion/sequence/intrinsic/at_key.hpp> 00047 00048 namespace pcl 00049 { 00058 PCL_EXPORTS void 00059 getMinMax3D (const sensor_msgs::PointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx, 00060 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt); 00061 00076 PCL_EXPORTS void 00077 getMinMax3D (const sensor_msgs::PointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx, 00078 const std::string &distance_field_name, float min_distance, float max_distance, 00079 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false); 00080 00085 inline Eigen::MatrixXi 00086 getHalfNeighborCellIndices () 00087 { 00088 Eigen::MatrixXi relative_coordinates (3, 13); 00089 int idx = 0; 00090 00091 // 0 - 8 00092 for (int i = -1; i < 2; i++) 00093 { 00094 for (int j = -1; j < 2; j++) 00095 { 00096 relative_coordinates (0, idx) = i; 00097 relative_coordinates (1, idx) = j; 00098 relative_coordinates (2, idx) = -1; 00099 idx++; 00100 } 00101 } 00102 // 9 - 11 00103 for (int i = -1; i < 2; i++) 00104 { 00105 relative_coordinates (0, idx) = i; 00106 relative_coordinates (1, idx) = -1; 00107 relative_coordinates (2, idx) = 0; 00108 idx++; 00109 } 00110 // 12 00111 relative_coordinates (0, idx) = -1; 00112 relative_coordinates (1, idx) = 0; 00113 relative_coordinates (2, idx) = 0; 00114 00115 return (relative_coordinates); 00116 } 00117 00122 inline Eigen::MatrixXi 00123 getAllNeighborCellIndices () 00124 { 00125 Eigen::MatrixXi relative_coordinates = getHalfNeighborCellIndices (); 00126 Eigen::MatrixXi relative_coordinates_all( 3, 26); 00127 relative_coordinates_all.block<3, 13> (0, 0) = relative_coordinates; 00128 relative_coordinates_all.block<3, 13> (0, 13) = -relative_coordinates; 00129 return (relative_coordinates_all); 00130 } 00131 00143 template <typename PointT> void 00144 getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00145 const std::string &distance_field_name, float min_distance, float max_distance, 00146 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false); 00147 00160 template <typename PointT> 00161 class VoxelGrid: public Filter<PointT> 00162 { 00163 protected: 00164 using Filter<PointT>::filter_name_; 00165 using Filter<PointT>::getClassName; 00166 using Filter<PointT>::input_; 00167 using Filter<PointT>::indices_; 00168 00169 typedef typename Filter<PointT>::PointCloud PointCloud; 00170 typedef typename PointCloud::Ptr PointCloudPtr; 00171 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00172 00173 public: 00175 VoxelGrid () : 00176 leaf_size_ (Eigen::Vector4f::Zero ()), 00177 inverse_leaf_size_ (Eigen::Array4f::Zero ()), 00178 downsample_all_data_ (true), 00179 save_leaf_layout_ (false), 00180 leaf_layout_ (), 00181 min_b_ (Eigen::Vector4i::Zero ()), 00182 max_b_ (Eigen::Vector4i::Zero ()), 00183 div_b_ (Eigen::Vector4i::Zero ()), 00184 divb_mul_ (Eigen::Vector4i::Zero ()), 00185 filter_field_name_ (""), 00186 filter_limit_min_ (-FLT_MAX), 00187 filter_limit_max_ (FLT_MAX), 00188 filter_limit_negative_ (false) 00189 { 00190 filter_name_ = "VoxelGrid"; 00191 } 00192 00194 virtual ~VoxelGrid () 00195 { 00196 } 00197 00201 inline void 00202 setLeafSize (const Eigen::Vector4f &leaf_size) 00203 { 00204 leaf_size_ = leaf_size; 00205 // Avoid division errors 00206 if (leaf_size_[3] == 0) 00207 leaf_size_[3] = 1; 00208 // Use multiplications instead of divisions 00209 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); 00210 } 00211 00217 inline void 00218 setLeafSize (float lx, float ly, float lz) 00219 { 00220 leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz; 00221 // Avoid division errors 00222 if (leaf_size_[3] == 0) 00223 leaf_size_[3] = 1; 00224 // Use multiplications instead of divisions 00225 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); 00226 } 00227 00229 inline Eigen::Vector3f 00230 getLeafSize () { return (leaf_size_.head<3> ()); } 00231 00235 inline void 00236 setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; } 00237 00241 inline bool 00242 getDownsampleAllData () { return (downsample_all_data_); } 00243 00247 inline void 00248 setSaveLeafLayout (bool save_leaf_layout) { save_leaf_layout_ = save_leaf_layout; } 00249 00251 inline bool 00252 getSaveLeafLayout () { return (save_leaf_layout_); } 00253 00257 inline Eigen::Vector3i 00258 getMinBoxCoordinates () { return (min_b_.head<3> ()); } 00259 00263 inline Eigen::Vector3i 00264 getMaxBoxCoordinates () { return (max_b_.head<3> ()); } 00265 00269 inline Eigen::Vector3i 00270 getNrDivisions () { return (div_b_.head<3> ()); } 00271 00275 inline Eigen::Vector3i 00276 getDivisionMultiplier () { return (divb_mul_.head<3> ()); } 00277 00286 inline int 00287 getCentroidIndex (const PointT &p) 00288 { 00289 return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (floor (p.x * inverse_leaf_size_[0])), 00290 static_cast<int> (floor (p.y * inverse_leaf_size_[1])), 00291 static_cast<int> (floor (p.z * inverse_leaf_size_[2])), 0) - min_b_).dot (divb_mul_))); 00292 } 00293 00300 inline std::vector<int> 00301 getNeighborCentroidIndices (const PointT &reference_point, const Eigen::MatrixXi &relative_coordinates) 00302 { 00303 Eigen::Vector4i ijk (static_cast<int> (floor (reference_point.x * inverse_leaf_size_[0])), 00304 static_cast<int> (floor (reference_point.y * inverse_leaf_size_[1])), 00305 static_cast<int> (floor (reference_point.z * inverse_leaf_size_[2])), 0); 00306 Eigen::Array4i diff2min = min_b_ - ijk; 00307 Eigen::Array4i diff2max = max_b_ - ijk; 00308 std::vector<int> neighbors (relative_coordinates.cols()); 00309 for (int ni = 0; ni < relative_coordinates.cols (); ni++) 00310 { 00311 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished(); 00312 // checking if the specified cell is in the grid 00313 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all()) 00314 neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted 00315 else 00316 neighbors[ni] = -1; // cell is out of bounds, consider it empty 00317 } 00318 return (neighbors); 00319 } 00320 00324 inline std::vector<int> 00325 getLeafLayout () { return (leaf_layout_); } 00326 00332 inline Eigen::Vector3i 00333 getGridCoordinates (float x, float y, float z) 00334 { 00335 return (Eigen::Vector3i (static_cast<int> (floor (x * inverse_leaf_size_[0])), 00336 static_cast<int> (floor (y * inverse_leaf_size_[1])), 00337 static_cast<int> (floor (z * inverse_leaf_size_[2])))); 00338 } 00339 00343 inline int 00344 getCentroidIndexAt (const Eigen::Vector3i &ijk) 00345 { 00346 int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_); 00347 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 00348 { 00349 //if (verbose) 00350 // 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 ()); 00351 return (-1); 00352 } 00353 return (leaf_layout_[idx]); 00354 } 00355 00360 inline void 00361 setFilterFieldName (const std::string &field_name) 00362 { 00363 filter_field_name_ = field_name; 00364 } 00365 00367 inline std::string const 00368 getFilterFieldName () 00369 { 00370 return (filter_field_name_); 00371 } 00372 00377 inline void 00378 setFilterLimits (const double &limit_min, const double &limit_max) 00379 { 00380 filter_limit_min_ = limit_min; 00381 filter_limit_max_ = limit_max; 00382 } 00383 00388 inline void 00389 getFilterLimits (double &limit_min, double &limit_max) 00390 { 00391 limit_min = filter_limit_min_; 00392 limit_max = filter_limit_max_; 00393 } 00394 00399 inline void 00400 setFilterLimitsNegative (const bool limit_negative) 00401 { 00402 filter_limit_negative_ = limit_negative; 00403 } 00404 00408 inline void 00409 getFilterLimitsNegative (bool &limit_negative) 00410 { 00411 limit_negative = filter_limit_negative_; 00412 } 00413 00417 inline bool 00418 getFilterLimitsNegative () 00419 { 00420 return (filter_limit_negative_); 00421 } 00422 00423 protected: 00425 Eigen::Vector4f leaf_size_; 00426 00428 Eigen::Array4f inverse_leaf_size_; 00429 00431 bool downsample_all_data_; 00432 00434 bool save_leaf_layout_; 00435 00437 std::vector<int> leaf_layout_; 00438 00440 Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_; 00441 00443 std::string filter_field_name_; 00444 00446 double filter_limit_min_; 00447 00449 double filter_limit_max_; 00450 00452 bool filter_limit_negative_; 00453 00454 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00455 00459 void 00460 applyFilter (PointCloud &output); 00461 }; 00462 00475 template <> 00476 class PCL_EXPORTS VoxelGrid<sensor_msgs::PointCloud2> : public Filter<sensor_msgs::PointCloud2> 00477 { 00478 using Filter<sensor_msgs::PointCloud2>::filter_name_; 00479 using Filter<sensor_msgs::PointCloud2>::getClassName; 00480 00481 typedef sensor_msgs::PointCloud2 PointCloud2; 00482 typedef PointCloud2::Ptr PointCloud2Ptr; 00483 typedef PointCloud2::ConstPtr PointCloud2ConstPtr; 00484 00485 public: 00487 VoxelGrid () : 00488 leaf_size_ (Eigen::Vector4f::Zero ()), 00489 inverse_leaf_size_ (Eigen::Array4f::Zero ()), 00490 downsample_all_data_ (true), 00491 save_leaf_layout_ (false), 00492 leaf_layout_ (), 00493 min_b_ (Eigen::Vector4i::Zero ()), 00494 max_b_ (Eigen::Vector4i::Zero ()), 00495 div_b_ (Eigen::Vector4i::Zero ()), 00496 divb_mul_ (Eigen::Vector4i::Zero ()), 00497 filter_field_name_ (""), 00498 filter_limit_min_ (-FLT_MAX), 00499 filter_limit_max_ (FLT_MAX), 00500 filter_limit_negative_ (false) 00501 { 00502 filter_name_ = "VoxelGrid"; 00503 } 00504 00506 virtual ~VoxelGrid () 00507 { 00508 } 00509 00513 inline void 00514 setLeafSize (const Eigen::Vector4f &leaf_size) 00515 { 00516 leaf_size_ = leaf_size; 00517 // Avoid division errors 00518 if (leaf_size_[3] == 0) 00519 leaf_size_[3] = 1; 00520 // Use multiplications instead of divisions 00521 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); 00522 } 00523 00529 inline void 00530 setLeafSize (float lx, float ly, float lz) 00531 { 00532 leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz; 00533 // Avoid division errors 00534 if (leaf_size_[3] == 0) 00535 leaf_size_[3] = 1; 00536 // Use multiplications instead of divisions 00537 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array (); 00538 } 00539 00541 inline Eigen::Vector3f 00542 getLeafSize () { return (leaf_size_.head<3> ()); } 00543 00547 inline void 00548 setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; } 00549 00553 inline bool 00554 getDownsampleAllData () { return (downsample_all_data_); } 00555 00559 inline void 00560 setSaveLeafLayout (bool save_leaf_layout) { save_leaf_layout_ = save_leaf_layout; } 00561 00563 inline bool 00564 getSaveLeafLayout () { return (save_leaf_layout_); } 00565 00569 inline Eigen::Vector3i 00570 getMinBoxCoordinates () { return (min_b_.head<3> ()); } 00571 00575 inline Eigen::Vector3i 00576 getMaxBoxCoordinates () { return (max_b_.head<3> ()); } 00577 00581 inline Eigen::Vector3i 00582 getNrDivisions () { return (div_b_.head<3> ()); } 00583 00587 inline Eigen::Vector3i 00588 getDivisionMultiplier () { return (divb_mul_.head<3> ()); } 00589 00597 inline int 00598 getCentroidIndex (float x, float y, float z) 00599 { 00600 return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (floor (x * inverse_leaf_size_[0])), 00601 static_cast<int> (floor (y * inverse_leaf_size_[1])), 00602 static_cast<int> (floor (z * inverse_leaf_size_[2])), 00603 0) 00604 - min_b_).dot (divb_mul_))); 00605 } 00606 00615 inline std::vector<int> 00616 getNeighborCentroidIndices (float x, float y, float z, const Eigen::MatrixXi &relative_coordinates) 00617 { 00618 Eigen::Vector4i ijk (static_cast<int> (floor (x * inverse_leaf_size_[0])), 00619 static_cast<int> (floor (y * inverse_leaf_size_[1])), 00620 static_cast<int> (floor (z * inverse_leaf_size_[2])), 0); 00621 Eigen::Array4i diff2min = min_b_ - ijk; 00622 Eigen::Array4i diff2max = max_b_ - ijk; 00623 std::vector<int> neighbors (relative_coordinates.cols()); 00624 for (int ni = 0; ni < relative_coordinates.cols (); ni++) 00625 { 00626 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished(); 00627 // checking if the specified cell is in the grid 00628 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all()) 00629 neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted 00630 else 00631 neighbors[ni] = -1; // cell is out of bounds, consider it empty 00632 } 00633 return (neighbors); 00634 } 00635 00644 inline std::vector<int> 00645 getNeighborCentroidIndices (float x, float y, float z, const std::vector<Eigen::Vector3i> &relative_coordinates) 00646 { 00647 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); 00648 std::vector<int> neighbors; 00649 neighbors.reserve (relative_coordinates.size ()); 00650 for (std::vector<Eigen::Vector3i>::const_iterator it = relative_coordinates.begin (); it != relative_coordinates.end (); it++) 00651 neighbors.push_back (leaf_layout_[(ijk + (Eigen::Vector4i() << *it, 0).finished() - min_b_).dot (divb_mul_)]); 00652 return (neighbors); 00653 } 00654 00658 inline std::vector<int> 00659 getLeafLayout () { return (leaf_layout_); } 00660 00666 inline Eigen::Vector3i 00667 getGridCoordinates (float x, float y, float z) 00668 { 00669 return (Eigen::Vector3i (static_cast<int> (floor (x * inverse_leaf_size_[0])), 00670 static_cast<int> (floor (y * inverse_leaf_size_[1])), 00671 static_cast<int> (floor (z * inverse_leaf_size_[2])))); 00672 } 00673 00677 inline int 00678 getCentroidIndexAt (const Eigen::Vector3i &ijk) 00679 { 00680 int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_); 00681 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 00682 { 00683 //if (verbose) 00684 // 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 ()); 00685 return (-1); 00686 } 00687 return (leaf_layout_[idx]); 00688 } 00689 00694 inline void 00695 setFilterFieldName (const std::string &field_name) 00696 { 00697 filter_field_name_ = field_name; 00698 } 00699 00701 inline std::string const 00702 getFilterFieldName () 00703 { 00704 return (filter_field_name_); 00705 } 00706 00711 inline void 00712 setFilterLimits (const double &limit_min, const double &limit_max) 00713 { 00714 filter_limit_min_ = limit_min; 00715 filter_limit_max_ = limit_max; 00716 } 00717 00722 inline void 00723 getFilterLimits (double &limit_min, double &limit_max) 00724 { 00725 limit_min = filter_limit_min_; 00726 limit_max = filter_limit_max_; 00727 } 00728 00733 inline void 00734 setFilterLimitsNegative (const bool limit_negative) 00735 { 00736 filter_limit_negative_ = limit_negative; 00737 } 00738 00742 inline void 00743 getFilterLimitsNegative (bool &limit_negative) 00744 { 00745 limit_negative = filter_limit_negative_; 00746 } 00747 00751 inline bool 00752 getFilterLimitsNegative () 00753 { 00754 return (filter_limit_negative_); 00755 } 00756 00757 protected: 00759 Eigen::Vector4f leaf_size_; 00760 00762 Eigen::Array4f inverse_leaf_size_; 00763 00765 bool downsample_all_data_; 00766 00770 bool save_leaf_layout_; 00771 00775 std::vector<int> leaf_layout_; 00776 00780 Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_; 00781 00783 std::string filter_field_name_; 00784 00786 double filter_limit_min_; 00787 00789 double filter_limit_max_; 00790 00792 bool filter_limit_negative_; 00793 00797 void 00798 applyFilter (PointCloud2 &output); 00799 }; 00800 } 00801 00802 #endif //#ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_
1.7.6.1
