Point Cloud Library (PCL)  1.7.1
covariance_sampling.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7 
8  * All rights reserved.
9 
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 
42 #ifndef PCL_FILTERS_COVARIANCE_SAMPLING_H_
43 #define PCL_FILTERS_COVARIANCE_SAMPLING_H_
44 
45 #include <pcl/filters/filter_indices.h>
46 
47 namespace pcl
48 {
49  /** \brief Point Cloud sampling based on the 6D covariances. It selects the points such that the resulting cloud is
50  * as stable as possible for being registered (against a copy of itself) with ICP. The algorithm adds points to the
51  * resulting cloud incrementally, while trying to keep all the 6 eigenvalues of the covariance matrix as close to each
52  * other as possible.
53  * This class also comes with the \a computeConditionNumber method that returns a number which shows how stable a point
54  * cloud will be when used as input for ICP (the closer the value it is to 1.0, the better).
55  *
56  * Based on the following publication:
57  * * "Geometrically Stable Sampling for the ICP Algorithm" - N. Gelfand, L. Ikemoto, S. Rusinkiewicz, M. Levoy
58  *
59  * \author Alexandru E. Ichim, alex.e.ichim@gmail.com
60  */
61  template <typename PointT, typename PointNT>
62  class CovarianceSampling : public FilterIndices<PointT>
63  {
69 
71  typedef typename Cloud::Ptr CloudPtr;
72  typedef typename Cloud::ConstPtr CloudConstPtr;
73  typedef typename pcl::PointCloud<PointNT>::ConstPtr NormalsConstPtr;
74 
75  public:
76  typedef boost::shared_ptr< CovarianceSampling<PointT, PointNT> > Ptr;
77  typedef boost::shared_ptr< const CovarianceSampling<PointT, PointNT> > ConstPtr;
78 
79  /** \brief Empty constructor. */
81  { filter_name_ = "CovarianceSampling"; }
82 
83  /** \brief Set number of indices to be sampled.
84  * \param[in] sample the number of sample indices
85  */
86  inline void
87  setNumberOfSamples (unsigned int samples)
88  { num_samples_ = samples; }
89 
90  /** \brief Get the value of the internal \a num_samples_ parameter. */
91  inline unsigned int
93  { return (num_samples_); }
94 
95  /** \brief Set the normals computed on the input point cloud
96  * \param[in] normals the normals computed for the input cloud
97  */
98  inline void
99  setNormals (const NormalsConstPtr &normals)
100  { input_normals_ = normals; }
101 
102  /** \brief Get the normals computed on the input point cloud */
103  inline NormalsConstPtr
104  getNormals () const
105  { return (input_normals_); }
106 
107 
108 
109  /** \brief Compute the condition number of the input point cloud. The condition number is the ratio between the
110  * largest and smallest eigenvalues of the 6x6 covariance matrix of the cloud. The closer this number is to 1.0,
111  * the more stable the cloud is for ICP registration.
112  * \return the condition number
113  */
114  double
116 
117  /** \brief Compute the condition number of the input point cloud. The condition number is the ratio between the
118  * largest and smallest eigenvalues of the 6x6 covariance matrix of the cloud. The closer this number is to 1.0,
119  * the more stable the cloud is for ICP registration.
120  * \param[in] covariance_matrix user given covariance matrix
121  * \return the condition number
122  */
123  static double
124  computeConditionNumber (const Eigen::Matrix<double, 6, 6> &covariance_matrix);
125 
126  /** \brief Computes the covariance matrix of the input cloud.
127  * \param[out] covariance_matrix the computed covariance matrix.
128  * \return whether the computation succeeded or not
129  */
130  bool
131  computeCovarianceMatrix (Eigen::Matrix<double, 6, 6> &covariance_matrix);
132 
133  protected:
134  /** \brief Number of indices that will be returned. */
135  unsigned int num_samples_;
136 
137  /** \brief The normals computed at each point in the input cloud */
138  NormalsConstPtr input_normals_;
139 
140  std::vector<Eigen::Vector3f> scaled_points_;
141 
142  bool
143  initCompute ();
144 
145  /** \brief Sample of point indices into a separate PointCloud
146  * \param[out] output the resultant point cloud
147  */
148  void
149  applyFilter (Cloud &output);
150 
151  /** \brief Sample of point indices
152  * \param[out] indices the resultant point cloud indices
153  */
154  void
155  applyFilter (std::vector<int> &indices);
156 
157  static bool
158  sort_dot_list_function (std::pair<int, double> a,
159  std::pair<int, double> b)
160  { return (a.second > b.second); }
161 
162  public:
163  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
164  };
165 }
166 
167 #ifdef PCL_NO_PRECOMPILE
168 #include <pcl/filters/impl/covariance_sampling.hpp>
169 #endif
170 
171 
172 #endif /* PCL_FILTERS_COVARIANCE_SAMPLING_H_ */
static bool sort_dot_list_function(std::pair< int, double > a, std::pair< int, double > b)
double computeConditionNumber()
Compute the condition number of the input point cloud.
void setNumberOfSamples(unsigned int samples)
Set number of indices to be sampled.
CovarianceSampling()
Empty constructor.
std::string filter_name_
The filter name.
Definition: filter.h:160
void setNormals(const NormalsConstPtr &normals)
Set the normals computed on the input point cloud.
boost::shared_ptr< CovarianceSampling< PointT, PointNT > > Ptr
bool computeCovarianceMatrix(Eigen::Matrix< double, 6, 6 > &covariance_matrix)
Computes the covariance matrix of the input cloud.
NormalsConstPtr getNormals() const
Get the normals computed on the input point cloud.
unsigned int getNumberOfSamples() const
Get the value of the internal num_samples_ parameter.
unsigned int num_samples_
Number of indices that will be returned.
FilterIndices represents the base class for filters that are about binary point removal.
void applyFilter(Cloud &output)
Sample of point indices into a separate PointCloud.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
std::vector< Eigen::Vector3f > scaled_points_
boost::shared_ptr< const CovarianceSampling< PointT, PointNT > > ConstPtr
NormalsConstPtr input_normals_
The normals computed at each point in the input cloud.
Point Cloud sampling based on the 6D covariances.