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) 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_ */