Point Cloud Library (PCL)
1.7.0
|
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 the copyright holder(s) 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$ 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/common/io.h> 00043 #include <pcl/filters/voxel_grid.h> 00044 00045 /////////////////////////////////////////////////////////////////////////////////////////// 00046 template <typename PointT> void 00047 pcl::getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00048 const std::string &distance_field_name, float min_distance, float max_distance, 00049 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative) 00050 { 00051 Eigen::Array4f min_p, max_p; 00052 min_p.setConstant (FLT_MAX); 00053 max_p.setConstant (-FLT_MAX); 00054 00055 // Get the fields list and the distance field index 00056 std::vector<pcl::PCLPointField> fields; 00057 int distance_idx = pcl::getFieldIndex (*cloud, distance_field_name, fields); 00058 00059 float distance_value; 00060 // If dense, no need to check for NaNs 00061 if (cloud->is_dense) 00062 { 00063 for (size_t i = 0; i < cloud->points.size (); ++i) 00064 { 00065 // Get the distance value 00066 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->points[i]); 00067 memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); 00068 00069 if (limit_negative) 00070 { 00071 // Use a threshold for cutting out points which inside the interval 00072 if ((distance_value < max_distance) && (distance_value > min_distance)) 00073 continue; 00074 } 00075 else 00076 { 00077 // Use a threshold for cutting out points which are too close/far away 00078 if ((distance_value > max_distance) || (distance_value < min_distance)) 00079 continue; 00080 } 00081 // Create the point structure and get the min/max 00082 pcl::Array4fMapConst pt = cloud->points[i].getArray4fMap (); 00083 min_p = min_p.min (pt); 00084 max_p = max_p.max (pt); 00085 } 00086 } 00087 else 00088 { 00089 for (size_t i = 0; i < cloud->points.size (); ++i) 00090 { 00091 // Get the distance value 00092 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->points[i]); 00093 memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); 00094 00095 if (limit_negative) 00096 { 00097 // Use a threshold for cutting out points which inside the interval 00098 if ((distance_value < max_distance) && (distance_value > min_distance)) 00099 continue; 00100 } 00101 else 00102 { 00103 // Use a threshold for cutting out points which are too close/far away 00104 if ((distance_value > max_distance) || (distance_value < min_distance)) 00105 continue; 00106 } 00107 00108 // Check if the point is invalid 00109 if (!pcl_isfinite (cloud->points[i].x) || 00110 !pcl_isfinite (cloud->points[i].y) || 00111 !pcl_isfinite (cloud->points[i].z)) 00112 continue; 00113 // Create the point structure and get the min/max 00114 pcl::Array4fMapConst pt = cloud->points[i].getArray4fMap (); 00115 min_p = min_p.min (pt); 00116 max_p = max_p.max (pt); 00117 } 00118 } 00119 min_pt = min_p; 00120 max_pt = max_p; 00121 } 00122 00123 /////////////////////////////////////////////////////////////////////////////////////////// 00124 template <typename PointT> void 00125 pcl::getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00126 const std::vector<int> &indices, 00127 const std::string &distance_field_name, float min_distance, float max_distance, 00128 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative) 00129 { 00130 Eigen::Array4f min_p, max_p; 00131 min_p.setConstant (FLT_MAX); 00132 max_p.setConstant (-FLT_MAX); 00133 00134 // Get the fields list and the distance field index 00135 std::vector<pcl::PCLPointField> fields; 00136 int distance_idx = pcl::getFieldIndex (*cloud, distance_field_name, fields); 00137 00138 float distance_value; 00139 // If dense, no need to check for NaNs 00140 if (cloud->is_dense) 00141 { 00142 for (std::vector<int>::const_iterator it = indices.begin (); it != indices.end (); ++it) 00143 { 00144 // Get the distance value 00145 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->points[*it]); 00146 memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); 00147 00148 if (limit_negative) 00149 { 00150 // Use a threshold for cutting out points which inside the interval 00151 if ((distance_value < max_distance) && (distance_value > min_distance)) 00152 continue; 00153 } 00154 else 00155 { 00156 // Use a threshold for cutting out points which are too close/far away 00157 if ((distance_value > max_distance) || (distance_value < min_distance)) 00158 continue; 00159 } 00160 // Create the point structure and get the min/max 00161 pcl::Array4fMapConst pt = cloud->points[*it].getArray4fMap (); 00162 min_p = min_p.min (pt); 00163 max_p = max_p.max (pt); 00164 } 00165 } 00166 else 00167 { 00168 for (std::vector<int>::const_iterator it = indices.begin (); it != indices.end (); ++it) 00169 { 00170 // Get the distance value 00171 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud->points[*it]); 00172 memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); 00173 00174 if (limit_negative) 00175 { 00176 // Use a threshold for cutting out points which inside the interval 00177 if ((distance_value < max_distance) && (distance_value > min_distance)) 00178 continue; 00179 } 00180 else 00181 { 00182 // Use a threshold for cutting out points which are too close/far away 00183 if ((distance_value > max_distance) || (distance_value < min_distance)) 00184 continue; 00185 } 00186 00187 // Check if the point is invalid 00188 if (!pcl_isfinite (cloud->points[*it].x) || 00189 !pcl_isfinite (cloud->points[*it].y) || 00190 !pcl_isfinite (cloud->points[*it].z)) 00191 continue; 00192 // Create the point structure and get the min/max 00193 pcl::Array4fMapConst pt = cloud->points[*it].getArray4fMap (); 00194 min_p = min_p.min (pt); 00195 max_p = max_p.max (pt); 00196 } 00197 } 00198 min_pt = min_p; 00199 max_pt = max_p; 00200 } 00201 00202 struct cloud_point_index_idx 00203 { 00204 unsigned int idx; 00205 unsigned int cloud_point_index; 00206 00207 cloud_point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {} 00208 bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); } 00209 }; 00210 00211 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00212 template <typename PointT> void 00213 pcl::VoxelGrid<PointT>::applyFilter (PointCloud &output) 00214 { 00215 // Has the input dataset been set already? 00216 if (!input_) 00217 { 00218 PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); 00219 output.width = output.height = 0; 00220 output.points.clear (); 00221 return; 00222 } 00223 00224 // Copy the header (and thus the frame_id) + allocate enough space for points 00225 output.height = 1; // downsampling breaks the organized structure 00226 output.is_dense = true; // we filter out invalid points 00227 00228 Eigen::Vector4f min_p, max_p; 00229 // Get the minimum and maximum dimensions 00230 if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud... 00231 getMinMax3D<PointT> (input_, *indices_, filter_field_name_, static_cast<float> (filter_limit_min_), static_cast<float> (filter_limit_max_), min_p, max_p, filter_limit_negative_); 00232 else 00233 getMinMax3D<PointT> (*input_, *indices_, min_p, max_p); 00234 00235 // Check that the leaf size is not too small, given the size of the data 00236 int64_t dx = static_cast<int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1; 00237 int64_t dy = static_cast<int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1; 00238 int64_t dz = static_cast<int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1; 00239 00240 if( (dx*dy*dz) > static_cast<int64_t>(std::numeric_limits<int32_t>::max()) ) 00241 { 00242 PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str()); 00243 output = *input_; 00244 return; 00245 } 00246 00247 // Compute the minimum and maximum bounding box values 00248 min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0])); 00249 max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0])); 00250 min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1])); 00251 max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1])); 00252 min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2])); 00253 max_b_[2] = static_cast<int> (floor (max_p[2] * inverse_leaf_size_[2])); 00254 00255 // Compute the number of divisions needed along all axis 00256 div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones (); 00257 div_b_[3] = 0; 00258 00259 // Set up the division multiplier 00260 divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0); 00261 00262 int centroid_size = 4; 00263 if (downsample_all_data_) 00264 centroid_size = boost::mpl::size<FieldList>::value; 00265 00266 // ---[ RGB special case 00267 std::vector<pcl::PCLPointField> fields; 00268 int rgba_index = -1; 00269 rgba_index = pcl::getFieldIndex (*input_, "rgb", fields); 00270 if (rgba_index == -1) 00271 rgba_index = pcl::getFieldIndex (*input_, "rgba", fields); 00272 if (rgba_index >= 0) 00273 { 00274 rgba_index = fields[rgba_index].offset; 00275 centroid_size += 3; 00276 } 00277 00278 std::vector<cloud_point_index_idx> index_vector; 00279 index_vector.reserve (indices_->size ()); 00280 00281 // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first... 00282 if (!filter_field_name_.empty ()) 00283 { 00284 // Get the distance field index 00285 std::vector<pcl::PCLPointField> fields; 00286 int distance_idx = pcl::getFieldIndex (*input_, filter_field_name_, fields); 00287 if (distance_idx == -1) 00288 PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx); 00289 00290 // First pass: go over all points and insert them into the index_vector vector 00291 // with calculated idx. Points with the same idx value will contribute to the 00292 // same point of resulting CloudPoint 00293 for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it) 00294 { 00295 if (!input_->is_dense) 00296 // Check if the point is invalid 00297 if (!pcl_isfinite (input_->points[*it].x) || 00298 !pcl_isfinite (input_->points[*it].y) || 00299 !pcl_isfinite (input_->points[*it].z)) 00300 continue; 00301 00302 // Get the distance value 00303 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&input_->points[*it]); 00304 float distance_value = 0; 00305 memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); 00306 00307 if (filter_limit_negative_) 00308 { 00309 // Use a threshold for cutting out points which inside the interval 00310 if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) 00311 continue; 00312 } 00313 else 00314 { 00315 // Use a threshold for cutting out points which are too close/far away 00316 if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) 00317 continue; 00318 } 00319 00320 int ijk0 = static_cast<int> (floor (input_->points[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0])); 00321 int ijk1 = static_cast<int> (floor (input_->points[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1])); 00322 int ijk2 = static_cast<int> (floor (input_->points[*it].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2])); 00323 00324 // Compute the centroid leaf index 00325 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 00326 index_vector.push_back (cloud_point_index_idx (static_cast<unsigned int> (idx), *it)); 00327 } 00328 } 00329 // No distance filtering, process all data 00330 else 00331 { 00332 // First pass: go over all points and insert them into the index_vector vector 00333 // with calculated idx. Points with the same idx value will contribute to the 00334 // same point of resulting CloudPoint 00335 for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it) 00336 { 00337 if (!input_->is_dense) 00338 // Check if the point is invalid 00339 if (!pcl_isfinite (input_->points[*it].x) || 00340 !pcl_isfinite (input_->points[*it].y) || 00341 !pcl_isfinite (input_->points[*it].z)) 00342 continue; 00343 00344 int ijk0 = static_cast<int> (floor (input_->points[*it].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0])); 00345 int ijk1 = static_cast<int> (floor (input_->points[*it].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1])); 00346 int ijk2 = static_cast<int> (floor (input_->points[*it].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2])); 00347 00348 // Compute the centroid leaf index 00349 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 00350 index_vector.push_back (cloud_point_index_idx (static_cast<unsigned int> (idx), *it)); 00351 } 00352 } 00353 00354 // Second pass: sort the index_vector vector using value representing target cell as index 00355 // in effect all points belonging to the same output cell will be next to each other 00356 std::sort (index_vector.begin (), index_vector.end (), std::less<cloud_point_index_idx> ()); 00357 00358 // Third pass: count output cells 00359 // we need to skip all the same, adjacenent idx values 00360 unsigned int total = 0; 00361 unsigned int index = 0; 00362 while (index < index_vector.size ()) 00363 { 00364 unsigned int i = index + 1; 00365 while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx) 00366 ++i; 00367 ++total; 00368 index = i; 00369 } 00370 00371 // Fourth pass: compute centroids, insert them into their final position 00372 output.points.resize (total); 00373 if (save_leaf_layout_) 00374 { 00375 try 00376 { 00377 // Resizing won't reset old elements to -1. If leaf_layout_ has been used previously, it needs to be re-initialized to -1 00378 uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2]; 00379 //This is the number of elements that need to be re-initialized to -1 00380 uint32_t reinit_size = std::min (static_cast<unsigned int> (new_layout_size), static_cast<unsigned int> (leaf_layout_.size())); 00381 for (uint32_t i = 0; i < reinit_size; i++) 00382 { 00383 leaf_layout_[i] = -1; 00384 } 00385 leaf_layout_.resize (new_layout_size, -1); 00386 } 00387 catch (std::bad_alloc&) 00388 { 00389 throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout", 00390 "voxel_grid.hpp", "applyFilter"); 00391 } 00392 catch (std::length_error&) 00393 { 00394 throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout", 00395 "voxel_grid.hpp", "applyFilter"); 00396 } 00397 } 00398 00399 index = 0; 00400 Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size); 00401 Eigen::VectorXf temporary = Eigen::VectorXf::Zero (centroid_size); 00402 00403 for (unsigned int cp = 0; cp < index_vector.size ();) 00404 { 00405 // calculate centroid - sum values from all input points, that have the same idx value in index_vector array 00406 if (!downsample_all_data_) 00407 { 00408 centroid[0] = input_->points[index_vector[cp].cloud_point_index].x; 00409 centroid[1] = input_->points[index_vector[cp].cloud_point_index].y; 00410 centroid[2] = input_->points[index_vector[cp].cloud_point_index].z; 00411 } 00412 else 00413 { 00414 // ---[ RGB special case 00415 if (rgba_index >= 0) 00416 { 00417 // Fill r/g/b data, assuming that the order is BGRA 00418 pcl::RGB rgb; 00419 memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[index_vector[cp].cloud_point_index]) + rgba_index, sizeof (RGB)); 00420 centroid[centroid_size-3] = rgb.r; 00421 centroid[centroid_size-2] = rgb.g; 00422 centroid[centroid_size-1] = rgb.b; 00423 } 00424 pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <PointT> (input_->points[index_vector[cp].cloud_point_index], centroid)); 00425 } 00426 00427 unsigned int i = cp + 1; 00428 while (i < index_vector.size () && index_vector[i].idx == index_vector[cp].idx) 00429 { 00430 if (!downsample_all_data_) 00431 { 00432 centroid[0] += input_->points[index_vector[i].cloud_point_index].x; 00433 centroid[1] += input_->points[index_vector[i].cloud_point_index].y; 00434 centroid[2] += input_->points[index_vector[i].cloud_point_index].z; 00435 } 00436 else 00437 { 00438 // ---[ RGB special case 00439 if (rgba_index >= 0) 00440 { 00441 // Fill r/g/b data, assuming that the order is BGRA 00442 pcl::RGB rgb; 00443 memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[index_vector[i].cloud_point_index]) + rgba_index, sizeof (RGB)); 00444 temporary[centroid_size-3] = rgb.r; 00445 temporary[centroid_size-2] = rgb.g; 00446 temporary[centroid_size-1] = rgb.b; 00447 } 00448 pcl::for_each_type <FieldList> (NdCopyPointEigenFunctor <PointT> (input_->points[index_vector[i].cloud_point_index], temporary)); 00449 centroid += temporary; 00450 } 00451 ++i; 00452 } 00453 00454 // index is centroid final position in resulting PointCloud 00455 if (save_leaf_layout_) 00456 leaf_layout_[index_vector[cp].idx] = index; 00457 00458 centroid /= static_cast<float> (i - cp); 00459 00460 // store centroid 00461 // Do we need to process all the fields? 00462 if (!downsample_all_data_) 00463 { 00464 output.points[index].x = centroid[0]; 00465 output.points[index].y = centroid[1]; 00466 output.points[index].z = centroid[2]; 00467 } 00468 else 00469 { 00470 pcl::for_each_type<FieldList> (pcl::NdCopyEigenPointFunctor <PointT> (centroid, output.points[index])); 00471 // ---[ RGB special case 00472 if (rgba_index >= 0) 00473 { 00474 // pack r/g/b into rgb 00475 float r = centroid[centroid_size-3], g = centroid[centroid_size-2], b = centroid[centroid_size-1]; 00476 int rgb = (static_cast<int> (r) << 16) | (static_cast<int> (g) << 8) | static_cast<int> (b); 00477 memcpy (reinterpret_cast<char*> (&output.points[index]) + rgba_index, &rgb, sizeof (float)); 00478 } 00479 } 00480 cp = i; 00481 ++index; 00482 } 00483 output.width = static_cast<uint32_t> (output.points.size ()); 00484 } 00485 00486 #define PCL_INSTANTIATE_VoxelGrid(T) template class PCL_EXPORTS pcl::VoxelGrid<T>; 00487 #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); 00488 00489 #endif // PCL_FILTERS_IMPL_VOXEL_GRID_H_ 00490