Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/filters/include/pcl/filters/covariance_sampling.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Point Cloud Library (PCL) - www.pointclouds.org
00005  * Copyright (c) 2009-2012, Willow Garage, Inc.
00006  * Copyright (c) 2012-, Open Perception, Inc.
00007 
00008  * All rights reserved.
00009 
00010  * Redistribution and use in source and binary forms, with or without
00011  * modification, are permitted provided that the following conditions
00012  * are met:
00013  *
00014  *  * Redistributions of source code must retain the above copyright
00015  *    notice, this list of conditions and the following disclaimer.
00016  *  * Redistributions in binary form must reproduce the above
00017  *    copyright notice, this list of conditions and the following
00018  *    disclaimer in the documentation and/or other materials provided
00019  *    with the distribution.
00020  * * Neither the name of the copyright holder(s) nor the names of its
00021  *    contributors may be used to endorse or promote products derived
00022  *    from this software without specific prior written permission.
00023  *
00024  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  * POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id$
00038  *
00039  */
00040 
00041 
00042 #ifndef PCL_FILTERS_COVARIANCE_SAMPLING_H_
00043 #define PCL_FILTERS_COVARIANCE_SAMPLING_H_
00044 
00045 #include <pcl/filters/filter_indices.h>
00046 
00047 namespace pcl
00048 {
00049   /** \brief Point Cloud sampling based on the 6D covariances. It selects the points such that the resulting cloud is
00050     * as stable as possible for being registered (against a copy of itself) with ICP. The algorithm adds points to the
00051     * resulting cloud incrementally, while trying to keep all the 6 eigenvalues of the covariance matrix as close to each
00052     * other as possible.
00053     * This class also comes with the \a computeConditionNumber method that returns a number which shows how stable a point
00054     * cloud will be when used as input for ICP (the closer the value it is to 1.0, the better).
00055     *
00056     * Based on the following publication:
00057     *    * "Geometrically Stable Sampling for the ICP Algorithm" - N. Gelfand, L. Ikemoto, S. Rusinkiewicz, M. Levoy
00058     *
00059     * \author Alexandru E. Ichim, alex.e.ichim@gmail.com
00060     */
00061   template <typename PointT, typename PointNT>
00062   class CovarianceSampling : public FilterIndices<PointT>
00063   {
00064       using FilterIndices<PointT>::filter_name_;
00065       using FilterIndices<PointT>::getClassName;
00066       using FilterIndices<PointT>::indices_;
00067       using FilterIndices<PointT>::input_;
00068       using FilterIndices<PointT>::initCompute;
00069 
00070       typedef typename FilterIndices<PointT>::PointCloud Cloud;
00071       typedef typename Cloud::Ptr CloudPtr;
00072       typedef typename Cloud::ConstPtr CloudConstPtr;
00073       typedef typename pcl::PointCloud<PointNT>::ConstPtr NormalsConstPtr;
00074 
00075     public:
00076       typedef boost::shared_ptr< CovarianceSampling<PointT, PointNT> > Ptr;
00077       typedef boost::shared_ptr< const CovarianceSampling<PointT, PointNT> > ConstPtr;
00078  
00079       /** \brief Empty constructor. */
00080       CovarianceSampling ()
00081       { filter_name_ = "CovarianceSampling"; }
00082 
00083       /** \brief Set number of indices to be sampled.
00084         * \param[in] sample the number of sample indices
00085         */
00086       inline void
00087       setNumberOfSamples (unsigned int samples)
00088       { num_samples_ = samples; }
00089 
00090       /** \brief Get the value of the internal \a num_samples_ parameter. */
00091       inline unsigned int
00092       getNumberOfSamples () const
00093       { return (num_samples_); }
00094 
00095       /** \brief Set the normals computed on the input point cloud
00096         * \param[in] normals the normals computed for the input cloud
00097         */
00098       inline void
00099       setNormals (const NormalsConstPtr &normals)
00100       { input_normals_ = normals; }
00101 
00102       /** \brief Get the normals computed on the input point cloud */
00103       inline NormalsConstPtr
00104       getNormals () const
00105       { return (input_normals_); }
00106 
00107 
00108 
00109       /** \brief Compute the condition number of the input point cloud. The condition number is the ratio between the
00110         * largest and smallest eigenvalues of the 6x6 covariance matrix of the cloud. The closer this number is to 1.0,
00111         * the more stable the cloud is for ICP registration.
00112         * \return the condition number
00113         */
00114       double
00115       computeConditionNumber ();
00116 
00117       /** \brief Compute the condition number of the input point cloud. The condition number is the ratio between the
00118         * largest and smallest eigenvalues of the 6x6 covariance matrix of the cloud. The closer this number is to 1.0,
00119         * the more stable the cloud is for ICP registration.
00120         * \param[in] covariance_matrix user given covariance matrix
00121         * \return the condition number
00122         */
00123       static double
00124       computeConditionNumber (const Eigen::Matrix<double, 6, 6> &covariance_matrix);
00125 
00126       /** \brief Computes the covariance matrix of the input cloud.
00127         * \param[out] covariance_matrix the computed covariance matrix.
00128         * \return whether the computation succeeded or not
00129         */
00130       bool
00131       computeCovarianceMatrix (Eigen::Matrix<double, 6, 6> &covariance_matrix);
00132 
00133     protected:
00134       /** \brief Number of indices that will be returned. */
00135       unsigned int num_samples_;
00136 
00137       /** \brief The normals computed at each point in the input cloud */
00138       NormalsConstPtr input_normals_;
00139 
00140       std::vector<Eigen::Vector3f> scaled_points_;
00141 
00142       bool
00143       initCompute ();
00144 
00145       /** \brief Sample of point indices into a separate PointCloud
00146         * \param[out] output the resultant point cloud
00147         */
00148       void
00149       applyFilter (Cloud &output);
00150 
00151       /** \brief Sample of point indices
00152         * \param[out] indices the resultant point cloud indices
00153         */
00154       void
00155       applyFilter (std::vector<int> &indices);
00156 
00157       static bool
00158       sort_dot_list_function (std::pair<int, double> a,
00159                               std::pair<int, double> b)
00160       { return (a.second > b.second); }
00161 
00162     public:
00163       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00164   };
00165 }
00166 
00167 #ifdef PCL_NO_PRECOMPILE
00168 #include <pcl/filters/impl/covariance_sampling.hpp>
00169 #endif
00170 
00171 
00172 #endif /* PCL_FILTERS_COVARIANCE_SAMPLING_H_ */