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 */ 00037 00038 #ifndef PCL_VOXEL_GRID_COVARIANCE_IMPL_H_ 00039 #define PCL_VOXEL_GRID_COVARIANCE_IMPL_H_ 00040 00041 #include <pcl/common/common.h> 00042 #include <pcl/filters/boost.h> 00043 #include <pcl/filters/voxel_grid_covariance.h> 00044 #include <Eigen/Dense> 00045 #include <Eigen/Cholesky> 00046 00047 ////////////////////////////////////////////////////////////////////////////////////////// 00048 template<typename PointT> void 00049 pcl::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output) 00050 { 00051 voxel_centroids_leaf_indices_.clear (); 00052 00053 // Has the input dataset been set already? 00054 if (!input_) 00055 { 00056 PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); 00057 output.width = output.height = 0; 00058 output.points.clear (); 00059 return; 00060 } 00061 00062 // Copy the header (and thus the frame_id) + allocate enough space for points 00063 output.height = 1; // downsampling breaks the organized structure 00064 output.is_dense = true; // we filter out invalid points 00065 output.points.clear (); 00066 00067 Eigen::Vector4f min_p, max_p; 00068 // Get the minimum and maximum dimensions 00069 if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud... 00070 getMinMax3D<PointT> (input_, filter_field_name_, static_cast<float> (filter_limit_min_), static_cast<float> (filter_limit_max_), min_p, max_p, filter_limit_negative_); 00071 else 00072 getMinMax3D<PointT> (*input_, min_p, max_p); 00073 00074 // Check that the leaf size is not too small, given the size of the data 00075 int64_t dx = static_cast<int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1; 00076 int64_t dy = static_cast<int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1; 00077 int64_t dz = static_cast<int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1; 00078 00079 if((dx*dy*dz) > std::numeric_limits<int32_t>::max()) 00080 { 00081 PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str()); 00082 output.clear(); 00083 return; 00084 } 00085 00086 // Compute the minimum and maximum bounding box values 00087 min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0])); 00088 max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0])); 00089 min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1])); 00090 max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1])); 00091 min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2])); 00092 max_b_[2] = static_cast<int> (floor (max_p[2] * inverse_leaf_size_[2])); 00093 00094 // Compute the number of divisions needed along all axis 00095 div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones (); 00096 div_b_[3] = 0; 00097 00098 // Clear the leaves 00099 leaves_.clear (); 00100 00101 // Set up the division multiplier 00102 divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0); 00103 00104 int centroid_size = 4; 00105 00106 if (downsample_all_data_) 00107 centroid_size = boost::mpl::size<FieldList>::value; 00108 00109 // ---[ RGB special case 00110 std::vector<pcl::PCLPointField> fields; 00111 int rgba_index = -1; 00112 rgba_index = pcl::getFieldIndex (*input_, "rgb", fields); 00113 if (rgba_index == -1) 00114 rgba_index = pcl::getFieldIndex (*input_, "rgba", fields); 00115 if (rgba_index >= 0) 00116 { 00117 rgba_index = fields[rgba_index].offset; 00118 centroid_size += 3; 00119 } 00120 00121 // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first... 00122 if (!filter_field_name_.empty ()) 00123 { 00124 // Get the distance field index 00125 std::vector<pcl::PCLPointField> fields; 00126 int distance_idx = pcl::getFieldIndex (*input_, filter_field_name_, fields); 00127 if (distance_idx == -1) 00128 PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx); 00129 00130 // First pass: go over all points and insert them into the right leaf 00131 for (size_t cp = 0; cp < input_->points.size (); ++cp) 00132 { 00133 if (!input_->is_dense) 00134 // Check if the point is invalid 00135 if (!pcl_isfinite (input_->points[cp].x) || 00136 !pcl_isfinite (input_->points[cp].y) || 00137 !pcl_isfinite (input_->points[cp].z)) 00138 continue; 00139 00140 // Get the distance value 00141 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&input_->points[cp]); 00142 float distance_value = 0; 00143 memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float)); 00144 00145 if (filter_limit_negative_) 00146 { 00147 // Use a threshold for cutting out points which inside the interval 00148 if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) 00149 continue; 00150 } 00151 else 00152 { 00153 // Use a threshold for cutting out points which are too close/far away 00154 if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) 00155 continue; 00156 } 00157 00158 int ijk0 = static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0])); 00159 int ijk1 = static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1])); 00160 int ijk2 = static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2])); 00161 00162 // Compute the centroid leaf index 00163 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 00164 00165 Leaf& leaf = leaves_[idx]; 00166 if (leaf.nr_points == 0) 00167 { 00168 leaf.centroid.resize (centroid_size); 00169 leaf.centroid.setZero (); 00170 } 00171 00172 Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z); 00173 // Accumulate point sum for centroid calculation 00174 leaf.mean_ += pt3d; 00175 // Accumulate x*xT for single pass covariance calculation 00176 leaf.cov_ += pt3d * pt3d.transpose (); 00177 00178 // Do we need to process all the fields? 00179 if (!downsample_all_data_) 00180 { 00181 Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0); 00182 leaf.centroid.template head<4> () += pt; 00183 } 00184 else 00185 { 00186 // Copy all the fields 00187 Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size); 00188 // ---[ RGB special case 00189 if (rgba_index >= 0) 00190 { 00191 // fill r/g/b data 00192 int rgb; 00193 memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (int)); 00194 centroid[centroid_size - 3] = static_cast<float> ((rgb >> 16) & 0x0000ff); 00195 centroid[centroid_size - 2] = static_cast<float> ((rgb >> 8) & 0x0000ff); 00196 centroid[centroid_size - 1] = static_cast<float> ((rgb) & 0x0000ff); 00197 } 00198 pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid)); 00199 leaf.centroid += centroid; 00200 } 00201 ++leaf.nr_points; 00202 } 00203 } 00204 // No distance filtering, process all data 00205 else 00206 { 00207 // First pass: go over all points and insert them into the right leaf 00208 for (size_t cp = 0; cp < input_->points.size (); ++cp) 00209 { 00210 if (!input_->is_dense) 00211 // Check if the point is invalid 00212 if (!pcl_isfinite (input_->points[cp].x) || 00213 !pcl_isfinite (input_->points[cp].y) || 00214 !pcl_isfinite (input_->points[cp].z)) 00215 continue; 00216 00217 int ijk0 = static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0])); 00218 int ijk1 = static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1])); 00219 int ijk2 = static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2])); 00220 00221 // Compute the centroid leaf index 00222 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 00223 00224 //int idx = (((input_->points[cp].getArray4fmap () * inverse_leaf_size_).template cast<int> ()).matrix () - min_b_).dot (divb_mul_); 00225 Leaf& leaf = leaves_[idx]; 00226 if (leaf.nr_points == 0) 00227 { 00228 leaf.centroid.resize (centroid_size); 00229 leaf.centroid.setZero (); 00230 } 00231 00232 Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z); 00233 // Accumulate point sum for centroid calculation 00234 leaf.mean_ += pt3d; 00235 // Accumulate x*xT for single pass covariance calculation 00236 leaf.cov_ += pt3d * pt3d.transpose (); 00237 00238 // Do we need to process all the fields? 00239 if (!downsample_all_data_) 00240 { 00241 Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0); 00242 leaf.centroid.template head<4> () += pt; 00243 } 00244 else 00245 { 00246 // Copy all the fields 00247 Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size); 00248 // ---[ RGB special case 00249 if (rgba_index >= 0) 00250 { 00251 // Fill r/g/b data, assuming that the order is BGRA 00252 int rgb; 00253 memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (int)); 00254 centroid[centroid_size - 3] = static_cast<float> ((rgb >> 16) & 0x0000ff); 00255 centroid[centroid_size - 2] = static_cast<float> ((rgb >> 8) & 0x0000ff); 00256 centroid[centroid_size - 1] = static_cast<float> ((rgb) & 0x0000ff); 00257 } 00258 pcl::for_each_type<FieldList> (NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid)); 00259 leaf.centroid += centroid; 00260 } 00261 ++leaf.nr_points; 00262 } 00263 } 00264 00265 // Second pass: go over all leaves and compute centroids and covariance matrices 00266 output.points.reserve (leaves_.size ()); 00267 if (searchable_) 00268 voxel_centroids_leaf_indices_.reserve (leaves_.size ()); 00269 int cp = 0; 00270 if (save_leaf_layout_) 00271 leaf_layout_.resize (div_b_[0] * div_b_[1] * div_b_[2], -1); 00272 00273 // Eigen values and vectors calculated to prevent near singluar matrices 00274 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver; 00275 Eigen::Matrix3d eigen_val; 00276 Eigen::Vector3d pt_sum; 00277 00278 // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the max eigen value. 00279 double min_covar_eigvalue; 00280 00281 for (typename std::map<size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it) 00282 { 00283 00284 // Normalize the centroid 00285 Leaf& leaf = it->second; 00286 00287 // Normalize the centroid 00288 leaf.centroid /= static_cast<float> (leaf.nr_points); 00289 // Point sum used for single pass covariance calculation 00290 pt_sum = leaf.mean_; 00291 // Normalize mean 00292 leaf.mean_ /= leaf.nr_points; 00293 00294 // If the voxel contains sufficient points, its covariance is calculated and is added to the voxel centroids and output clouds. 00295 // Points with less than the minimum points will have a can not be accuratly approximated using a normal distribution. 00296 if (leaf.nr_points >= min_points_per_voxel_) 00297 { 00298 if (save_leaf_layout_) 00299 leaf_layout_[it->first] = cp++; 00300 00301 output.push_back (PointT ()); 00302 00303 // Do we need to process all the fields? 00304 if (!downsample_all_data_) 00305 { 00306 output.points.back ().x = leaf.centroid[0]; 00307 output.points.back ().y = leaf.centroid[1]; 00308 output.points.back ().z = leaf.centroid[2]; 00309 } 00310 else 00311 { 00312 pcl::for_each_type<FieldList> (pcl::NdCopyEigenPointFunctor<PointT> (leaf.centroid, output.back ())); 00313 // ---[ RGB special case 00314 if (rgba_index >= 0) 00315 { 00316 // pack r/g/b into rgb 00317 float r = leaf.centroid[centroid_size - 3], g = leaf.centroid[centroid_size - 2], b = leaf.centroid[centroid_size - 1]; 00318 int rgb = (static_cast<int> (r)) << 16 | (static_cast<int> (g)) << 8 | (static_cast<int> (b)); 00319 memcpy (reinterpret_cast<char*> (&output.points.back ()) + rgba_index, &rgb, sizeof (float)); 00320 } 00321 } 00322 00323 // Stores the voxel indice for fast access searching 00324 if (searchable_) 00325 voxel_centroids_leaf_indices_.push_back (static_cast<int> (it->first)); 00326 00327 // Single pass covariance calculation 00328 leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose ())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose (); 00329 leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points; 00330 00331 //Normalize Eigen Val such that max no more than 100x min. 00332 eigensolver.compute (leaf.cov_); 00333 eigen_val = eigensolver.eigenvalues ().asDiagonal (); 00334 leaf.evecs_ = eigensolver.eigenvectors (); 00335 00336 if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0) 00337 { 00338 leaf.nr_points = -1; 00339 continue; 00340 } 00341 00342 // Avoids matrices near singularities (eq 6.11)[Magnusson 2009] 00343 00344 min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val (2, 2); 00345 if (eigen_val (0, 0) < min_covar_eigvalue) 00346 { 00347 eigen_val (0, 0) = min_covar_eigvalue; 00348 00349 if (eigen_val (1, 1) < min_covar_eigvalue) 00350 { 00351 eigen_val (1, 1) = min_covar_eigvalue; 00352 } 00353 00354 leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse (); 00355 } 00356 leaf.evals_ = eigen_val.diagonal (); 00357 00358 leaf.icov_ = leaf.cov_.inverse (); 00359 if (leaf.icov_.maxCoeff () == std::numeric_limits<float>::infinity ( ) 00360 || leaf.icov_.minCoeff () == -std::numeric_limits<float>::infinity ( ) ) 00361 { 00362 leaf.nr_points = -1; 00363 } 00364 00365 } 00366 } 00367 00368 output.width = static_cast<uint32_t> (output.points.size ()); 00369 } 00370 00371 ////////////////////////////////////////////////////////////////////////////////////////// 00372 template<typename PointT> int 00373 pcl::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) 00374 { 00375 neighbors.clear (); 00376 00377 // Find displacement coordinates 00378 Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices (); 00379 Eigen::Vector4i ijk (static_cast<int> (floor (reference_point.x / leaf_size_[0])), 00380 static_cast<int> (floor (reference_point.y / leaf_size_[1])), 00381 static_cast<int> (floor (reference_point.z / leaf_size_[2])), 0); 00382 Eigen::Array4i diff2min = min_b_ - ijk; 00383 Eigen::Array4i diff2max = max_b_ - ijk; 00384 neighbors.reserve (relative_coordinates.cols ()); 00385 00386 // Check each neighbor to see if it is occupied and contains sufficient points 00387 // Slower than radius search because needs to check 26 indices 00388 for (int ni = 0; ni < relative_coordinates.cols (); ni++) 00389 { 00390 Eigen::Vector4i displacement = (Eigen::Vector4i () << relative_coordinates.col (ni), 0).finished (); 00391 // Checking if the specified cell is in the grid 00392 if ((diff2min <= displacement.array ()).all () && (diff2max >= displacement.array ()).all ()) 00393 { 00394 typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (((ijk + displacement - min_b_).dot (divb_mul_))); 00395 if (leaf_iter != leaves_.end () && leaf_iter->second.nr_points >= min_points_per_voxel_) 00396 { 00397 LeafConstPtr leaf = &(leaf_iter->second); 00398 neighbors.push_back (leaf); 00399 } 00400 } 00401 } 00402 00403 return (static_cast<int> (neighbors.size ())); 00404 } 00405 00406 ////////////////////////////////////////////////////////////////////////////////////////// 00407 template<typename PointT> void 00408 pcl::VoxelGridCovariance<PointT>::getDisplayCloud (pcl::PointCloud<PointXYZ>& cell_cloud) 00409 { 00410 cell_cloud.clear (); 00411 00412 int pnt_per_cell = 1000; 00413 boost::mt19937 rng; 00414 boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ()); 00415 boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd); 00416 00417 Eigen::LLT<Eigen::Matrix3d> llt_of_cov; 00418 Eigen::Matrix3d cholesky_decomp; 00419 Eigen::Vector3d cell_mean; 00420 Eigen::Vector3d rand_point; 00421 Eigen::Vector3d dist_point; 00422 00423 // Generate points for each occupied voxel with sufficient points. 00424 for (typename std::map<size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it) 00425 { 00426 Leaf& leaf = it->second; 00427 00428 if (leaf.nr_points >= min_points_per_voxel_) 00429 { 00430 cell_mean = leaf.mean_; 00431 llt_of_cov.compute (leaf.cov_); 00432 cholesky_decomp = llt_of_cov.matrixL (); 00433 00434 // Random points generated by sampling the normal distribution given by voxel mean and covariance matrix 00435 for (int i = 0; i < pnt_per_cell; i++) 00436 { 00437 rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ()); 00438 dist_point = cell_mean + cholesky_decomp * rand_point; 00439 cell_cloud.push_back (PointXYZ (static_cast<float> (dist_point (0)), static_cast<float> (dist_point (1)), static_cast<float> (dist_point (2)))); 00440 } 00441 } 00442 } 00443 } 00444 00445 #define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance<T>; 00446 00447 #endif // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_