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.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