Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/filters/include/pcl/filters/voxel_grid_covariance.h
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_