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_H_ 00039 #define PCL_VOXEL_GRID_COVARIANCE_H_ 00040 00041 #include <pcl/filters/boost.h> 00042 #include <pcl/filters/voxel_grid.h> 00043 #include <map> 00044 #include <pcl/point_types.h> 00045 #include <pcl/kdtree/kdtree_flann.h> 00046 00047 namespace pcl 00048 { 00049 /** \brief A searchable voxel strucure containing the mean and covariance of the data. 00050 * \note For more information please see 00051 * <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — 00052 * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. 00053 * PhD thesis, Orebro University. Orebro Studies in Technology 36</b> 00054 * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) 00055 */ 00056 template<typename PointT> 00057 class VoxelGridCovariance : public VoxelGrid<PointT> 00058 { 00059 protected: 00060 using VoxelGrid<PointT>::filter_name_; 00061 using VoxelGrid<PointT>::getClassName; 00062 using VoxelGrid<PointT>::input_; 00063 using VoxelGrid<PointT>::indices_; 00064 using VoxelGrid<PointT>::filter_limit_negative_; 00065 using VoxelGrid<PointT>::filter_limit_min_; 00066 using VoxelGrid<PointT>::filter_limit_max_; 00067 using VoxelGrid<PointT>::filter_field_name_; 00068 00069 using VoxelGrid<PointT>::downsample_all_data_; 00070 using VoxelGrid<PointT>::leaf_layout_; 00071 using VoxelGrid<PointT>::save_leaf_layout_; 00072 using VoxelGrid<PointT>::leaf_size_; 00073 using VoxelGrid<PointT>::min_b_; 00074 using VoxelGrid<PointT>::max_b_; 00075 using VoxelGrid<PointT>::inverse_leaf_size_; 00076 using VoxelGrid<PointT>::div_b_; 00077 using VoxelGrid<PointT>::divb_mul_; 00078 00079 00080 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00081 typedef typename Filter<PointT>::PointCloud PointCloud; 00082 typedef typename PointCloud::Ptr PointCloudPtr; 00083 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00084 00085 public: 00086 00087 typedef boost::shared_ptr< VoxelGrid<PointT> > Ptr; 00088 typedef boost::shared_ptr< const VoxelGrid<PointT> > ConstPtr; 00089 00090 00091 /** \brief Simple structure to hold a centroid, covarince and the number of points in a leaf. 00092 * Inverse covariance, eigen vectors and engen values are precomputed. */ 00093 struct Leaf 00094 { 00095 /** \brief Constructor. 00096 * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix 00097 */ 00098 Leaf () : 00099 nr_points (0), 00100 mean_ (Eigen::Vector3d::Zero ()), 00101 centroid (), 00102 cov_ (Eigen::Matrix3d::Identity ()), 00103 icov_ (Eigen::Matrix3d::Zero ()), 00104 evecs_ (Eigen::Matrix3d::Identity ()), 00105 evals_ (Eigen::Vector3d::Zero ()) 00106 { 00107 } 00108 00109 /** \brief Get the voxel covariance. 00110 * \return covariance matrix 00111 */ 00112 Eigen::Matrix3d 00113 getCov () const 00114 { 00115 return (cov_); 00116 } 00117 00118 /** \brief Get the inverse of the voxel covariance. 00119 * \return inverse covariance matrix 00120 */ 00121 Eigen::Matrix3d 00122 getInverseCov () const 00123 { 00124 return (icov_); 00125 } 00126 00127 /** \brief Get the voxel centroid. 00128 * \return centroid 00129 */ 00130 Eigen::Vector3d 00131 getMean () const 00132 { 00133 return (mean_); 00134 } 00135 00136 /** \brief Get the eigen vectors of the voxel covariance. 00137 * \note Order corresponds with \ref getEvals 00138 * \return matrix whose columns contain eigen vectors 00139 */ 00140 Eigen::Matrix3d 00141 getEvecs () const 00142 { 00143 return (evecs_); 00144 } 00145 00146 /** \brief Get the eigen values of the voxel covariance. 00147 * \note Order corresponds with \ref getEvecs 00148 * \return vector of eigen values 00149 */ 00150 Eigen::Vector3d 00151 getEvals () const 00152 { 00153 return (evals_); 00154 } 00155 00156 /** \brief Get the number of points contained by this voxel. 00157 * \return number of points 00158 */ 00159 int 00160 getPointCount () const 00161 { 00162 return (nr_points); 00163 } 00164 00165 /** \brief Number of points contained by voxel */ 00166 int nr_points; 00167 00168 /** \brief 3D voxel centroid */ 00169 Eigen::Vector3d mean_; 00170 00171 /** \brief Nd voxel centroid 00172 * \note Differs from \ref mean_ when color data is used 00173 */ 00174 Eigen::VectorXf centroid; 00175 00176 /** \brief Voxel covariance matrix */ 00177 Eigen::Matrix3d cov_; 00178 00179 /** \brief Inverse of voxel covariance matrix */ 00180 Eigen::Matrix3d icov_; 00181 00182 /** \brief Eigen vectors of voxel covariance matrix */ 00183 Eigen::Matrix3d evecs_; 00184 00185 /** \brief Eigen values of voxel covariance matrix */ 00186 Eigen::Vector3d evals_; 00187 00188 }; 00189 00190 /** \brief Pointer to VoxelGridCovariance leaf structure */ 00191 typedef Leaf* LeafPtr; 00192 00193 /** \brief Const pointer to VoxelGridCovariance leaf structure */ 00194 typedef const Leaf* LeafConstPtr; 00195 00196 public: 00197 00198 /** \brief Constructor. 00199 * Sets \ref leaf_size_ to 0 and \ref searchable_ to false. 00200 */ 00201 VoxelGridCovariance () : 00202 searchable_ (true), 00203 min_points_per_voxel_ (6), 00204 min_covar_eigvalue_mult_ (0.01), 00205 leaves_ (), 00206 voxel_centroids_ (), 00207 voxel_centroids_leaf_indices_ (), 00208 kdtree_ () 00209 { 00210 downsample_all_data_ = false; 00211 save_leaf_layout_ = false; 00212 leaf_size_.setZero (); 00213 min_b_.setZero (); 00214 max_b_.setZero (); 00215 filter_name_ = "VoxelGridCovariance"; 00216 } 00217 00218 /** \brief Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance calculation). 00219 * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used 00220 */ 00221 inline void 00222 setMinPointPerVoxel (int min_points_per_voxel) 00223 { 00224 if(min_points_per_voxel > 2) 00225 { 00226 min_points_per_voxel_ = min_points_per_voxel; 00227 } 00228 else 00229 { 00230 PCL_WARN ("%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->getClassName ().c_str ()); 00231 min_points_per_voxel_ = 3; 00232 } 00233 } 00234 00235 /** \brief Get the minimum number of points required for a cell to be used. 00236 * \return the minimum number of points for required for a voxel to be used 00237 */ 00238 inline int 00239 getMinPointPerVoxel () 00240 { 00241 return min_points_per_voxel_; 00242 } 00243 00244 /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices. 00245 * \param[in] min_points_per_voxel the minimum allowable ratio between eigenvalues 00246 */ 00247 inline void 00248 setCovEigValueInflationRatio (double min_covar_eigvalue_mult) 00249 { 00250 min_covar_eigvalue_mult_ = min_covar_eigvalue_mult; 00251 } 00252 00253 /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices. 00254 * \return the minimum allowable ratio between eigenvalues 00255 */ 00256 inline double 00257 getCovEigValueInflationRatio () 00258 { 00259 return min_covar_eigvalue_mult_; 00260 } 00261 00262 /** \brief Filter cloud and initializes voxel structure. 00263 * \param[out] output cloud containing centroids of voxels containing a sufficient number of points 00264 * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built 00265 */ 00266 inline void 00267 filter (PointCloud &output, bool searchable = false) 00268 { 00269 searchable_ = searchable; 00270 applyFilter (output); 00271 00272 voxel_centroids_ = PointCloudPtr (new PointCloud (output)); 00273 00274 if (searchable_ && voxel_centroids_->size() > 0) 00275 { 00276 // Initiates kdtree of the centroids of voxels containing a sufficient number of points 00277 kdtree_.setInputCloud (voxel_centroids_); 00278 } 00279 } 00280 00281 /** \brief Initializes voxel structure. 00282 * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built 00283 */ 00284 inline void 00285 filter (bool searchable = false) 00286 { 00287 searchable_ = searchable; 00288 voxel_centroids_ = PointCloudPtr (new PointCloud); 00289 applyFilter (*voxel_centroids_); 00290 00291 if (searchable_ && voxel_centroids_->size() > 0) 00292 { 00293 // Initiates kdtree of the centroids of voxels containing a sufficient number of points 00294 kdtree_.setInputCloud (voxel_centroids_); 00295 } 00296 } 00297 00298 /** \brief Get the voxel containing point p. 00299 * \param[in] index the index of the leaf structure node 00300 * \return const pointer to leaf structure 00301 */ 00302 inline LeafConstPtr 00303 getLeaf (int index) 00304 { 00305 typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (index); 00306 if (leaf_iter != leaves_.end ()) 00307 { 00308 LeafConstPtr ret (&(leaf_iter->second)); 00309 return ret; 00310 } 00311 else 00312 return NULL; 00313 } 00314 00315 /** \brief Get the voxel containing point p. 00316 * \param[in] p the point to get the leaf structure at 00317 * \return const pointer to leaf structure 00318 */ 00319 inline LeafConstPtr 00320 getLeaf (PointT &p) 00321 { 00322 // Generate index associated with p 00323 int ijk0 = static_cast<int> (floor (p.x * inverse_leaf_size_[0]) - min_b_[0]); 00324 int ijk1 = static_cast<int> (floor (p.y * inverse_leaf_size_[1]) - min_b_[1]); 00325 int ijk2 = static_cast<int> (floor (p.z * inverse_leaf_size_[2]) - min_b_[2]); 00326 00327 // Compute the centroid leaf index 00328 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 00329 00330 // Find leaf associated with index 00331 typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (idx); 00332 if (leaf_iter != leaves_.end ()) 00333 { 00334 // If such a leaf exists return the pointer to the leaf structure 00335 LeafConstPtr ret (&(leaf_iter->second)); 00336 return ret; 00337 } 00338 else 00339 return NULL; 00340 } 00341 00342 /** \brief Get the voxel containing point p. 00343 * \param[in] p the point to get the leaf structure at 00344 * \return const pointer to leaf structure 00345 */ 00346 inline LeafConstPtr 00347 getLeaf (Eigen::Vector3f &p) 00348 { 00349 // Generate index associated with p 00350 int ijk0 = static_cast<int> (floor (p[0] * inverse_leaf_size_[0]) - min_b_[0]); 00351 int ijk1 = static_cast<int> (floor (p[1] * inverse_leaf_size_[1]) - min_b_[1]); 00352 int ijk2 = static_cast<int> (floor (p[2] * inverse_leaf_size_[2]) - min_b_[2]); 00353 00354 // Compute the centroid leaf index 00355 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; 00356 00357 // Find leaf associated with index 00358 typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (idx); 00359 if (leaf_iter != leaves_.end ()) 00360 { 00361 // If such a leaf exists return the pointer to the leaf structure 00362 LeafConstPtr ret (&(leaf_iter->second)); 00363 return ret; 00364 } 00365 else 00366 return NULL; 00367 00368 } 00369 00370 /** \brief Get the voxels surrounding point p, not including the voxel contating point p. 00371 * \note Only voxels containing a sufficient number of points are used (slower than radius search in practice). 00372 * \param[in] reference_point the point to get the leaf structure at 00373 * \param[out] neighbors 00374 * \return number of neighbors found 00375 */ 00376 int 00377 getNeighborhoodAtPoint (const PointT& reference_point, std::vector<LeafConstPtr> &neighbors); 00378 00379 /** \brief Get the leaf structure map 00380 * \return a map contataining all leaves 00381 */ 00382 inline const std::map<size_t, Leaf>& 00383 getLeaves () 00384 { 00385 return leaves_; 00386 } 00387 00388 /** \brief Get a pointcloud containing the voxel centroids 00389 * \note Only voxels containing a sufficient number of points are used. 00390 * \return a map contataining all leaves 00391 */ 00392 inline PointCloudPtr 00393 getCentroids () 00394 { 00395 return voxel_centroids_; 00396 } 00397 00398 00399 /** \brief Get a cloud to visualize each voxels normal distribution. 00400 * \param[out] a cloud created by sampling the normal distributions of each voxel 00401 */ 00402 void 00403 getDisplayCloud (pcl::PointCloud<PointXYZ>& cell_cloud); 00404 00405 /** \brief Search for the k-nearest occupied voxels for the given query point. 00406 * \note Only voxels containing a sufficient number of points are used. 00407 * \param[in] point the given query point 00408 * \param[in] k the number of neighbors to search for 00409 * \param[out] k_leaves the resultant leaves of the neighboring points 00410 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 00411 * \return number of neighbors found 00412 */ 00413 int 00414 nearestKSearch (const PointT &point, int k, 00415 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances) 00416 { 00417 k_leaves.clear (); 00418 00419 // Check if kdtree has been built 00420 if (!searchable_) 00421 { 00422 PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ()); 00423 return 0; 00424 } 00425 00426 // Find k-nearest neighbors in the occupied voxel centroid cloud 00427 std::vector<int> k_indices; 00428 k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances); 00429 00430 // Find leaves corresponding to neighbors 00431 k_leaves.reserve (k); 00432 for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++) 00433 { 00434 k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[*iter]]); 00435 } 00436 return k; 00437 } 00438 00439 /** \brief Search for the k-nearest occupied voxels for the given query point. 00440 * \note Only voxels containing a sufficient number of points are used. 00441 * \param[in] point the given query point 00442 * \param[in] k the number of neighbors to search for 00443 * \param[out] k_leaves the resultant leaves of the neighboring points 00444 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 00445 * \return number of neighbors found 00446 */ 00447 inline int 00448 nearestKSearch (const PointCloud &cloud, int index, int k, 00449 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances) 00450 { 00451 if (index >= static_cast<int> (cloud.points.size ()) || index < 0) 00452 return (0); 00453 return (nearestKSearch (cloud.points[index], k, k_leaves, k_sqr_distances)); 00454 } 00455 00456 00457 /** \brief Search for all the nearest occupied voxels of the query point in a given radius. 00458 * \note Only voxels containing a sufficient number of points are used. 00459 * \param[in] point the given query point 00460 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 00461 * \param[out] k_leaves the resultant leaves of the neighboring points 00462 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 00463 * \return number of neighbors found 00464 */ 00465 int 00466 radiusSearch (const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves, 00467 std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) 00468 { 00469 k_leaves.clear (); 00470 00471 // Check if kdtree has been built 00472 if (!searchable_) 00473 { 00474 PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ()); 00475 return 0; 00476 } 00477 00478 // Find neighbors within radius in the occupied voxel centroid cloud 00479 std::vector<int> k_indices; 00480 int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn); 00481 00482 // Find leaves corresponding to neighbors 00483 k_leaves.reserve (k); 00484 for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++) 00485 { 00486 k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[*iter]]); 00487 } 00488 return k; 00489 } 00490 00491 /** \brief Search for all the nearest occupied voxels of the query point in a given radius. 00492 * \note Only voxels containing a sufficient number of points are used. 00493 * \param[in] cloud the given query point 00494 * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point 00495 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 00496 * \param[out] k_leaves the resultant leaves of the neighboring points 00497 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 00498 * \return number of neighbors found 00499 */ 00500 inline int 00501 radiusSearch (const PointCloud &cloud, int index, double radius, 00502 std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances, 00503 unsigned int max_nn = 0) 00504 { 00505 if (index >= static_cast<int> (cloud.points.size ()) || index < 0) 00506 return (0); 00507 return (radiusSearch (cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn)); 00508 } 00509 00510 protected: 00511 00512 /** \brief Filter cloud and initializes voxel structure. 00513 * \param[out] output cloud containing centroids of voxels containing a sufficient number of points 00514 */ 00515 void applyFilter (PointCloud &output); 00516 00517 /** \brief Flag to determine if voxel structure is searchable. */ 00518 bool searchable_; 00519 00520 /** \brief Minimum points contained with in a voxel to allow it to be useable. */ 00521 int min_points_per_voxel_; 00522 00523 /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */ 00524 double min_covar_eigvalue_mult_; 00525 00526 /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */ 00527 std::map<size_t, Leaf> leaves_; 00528 00529 /** \brief Point cloud containing centroids of voxels containing atleast minimum number of points. */ 00530 PointCloudPtr voxel_centroids_; 00531 00532 /** \brief Indices of leaf structurs associated with each point in \ref voxel_centroids_ (used for searching). */ 00533 std::vector<int> voxel_centroids_leaf_indices_; 00534 00535 /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */ 00536 KdTreeFLANN<PointT> kdtree_; 00537 }; 00538 } 00539 00540 #ifdef PCL_NO_PRECOMPILE 00541 #include <pcl/filters/impl/voxel_grid_covariance.hpp> 00542 #endif 00543 00544 #endif //#ifndef PCL_VOXEL_GRID_COVARIANCE_H_