Point Cloud Library (PCL)  1.9.1-dev
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 #pragma once
42 
43 #include <pcl/pcl_macros.h>
44 #include <pcl/filters/filter_indices.h>
45 
46 namespace pcl
47 {
48  /** \brief Point Cloud sampling based on the 6D covariances. It selects the points such that the resulting cloud is
49  * as stable as possible for being registered (against a copy of itself) with ICP. The algorithm adds points to the
50  * resulting cloud incrementally, while trying to keep all the 6 eigenvalues of the covariance matrix as close to each
51  * other as possible.
52  * This class also comes with the \a computeConditionNumber method that returns a number which shows how stable a point
53  * cloud will be when used as input for ICP (the closer the value it is to 1.0, the better).
54  *
55  * Based on the following publication:
56  * * "Geometrically Stable Sampling for the ICP Algorithm" - N. Gelfand, L. Ikemoto, S. Rusinkiewicz, M. Levoy
57  *
58  * \author Alexandru E. Ichim, alex.e.ichim@gmail.com
59  */
60  template <typename PointT, typename PointNT>
61  class CovarianceSampling : public FilterIndices<PointT>
62  {
68 
69  using Cloud = typename FilterIndices<PointT>::PointCloud;
70  using CloudPtr = typename Cloud::Ptr;
71  using CloudConstPtr = typename Cloud::ConstPtr;
72  using NormalsConstPtr = typename pcl::PointCloud<PointNT>::ConstPtr;
73 
74  public:
75  using Ptr = boost::shared_ptr< CovarianceSampling<PointT, PointNT> >;
76  using ConstPtr = boost::shared_ptr< const CovarianceSampling<PointT, PointNT> >;
77 
78  /** \brief Empty constructor. */
80  { filter_name_ = "CovarianceSampling"; }
81 
82  /** \brief Set number of indices to be sampled.
83  * \param[in] samples the number of sample indices
84  */
85  inline void
86  setNumberOfSamples (unsigned int samples)
87  { num_samples_ = samples; }
88 
89  /** \brief Get the value of the internal \a num_samples_ parameter. */
90  inline unsigned int
92  { return (num_samples_); }
93 
94  /** \brief Set the normals computed on the input point cloud
95  * \param[in] normals the normals computed for the input cloud
96  */
97  inline void
98  setNormals (const NormalsConstPtr &normals)
99  { input_normals_ = normals; }
100 
101  /** \brief Get the normals computed on the input point cloud */
102  inline NormalsConstPtr
103  getNormals () const
104  { return (input_normals_); }
105 
106 
107 
108  /** \brief Compute the condition number of the input point cloud. The condition number is the ratio between the
109  * largest and smallest eigenvalues of the 6x6 covariance matrix of the cloud. The closer this number is to 1.0,
110  * the more stable the cloud is for ICP registration.
111  * \return the condition number
112  */
113  double
115 
116  /** \brief Compute the condition number of the input point cloud. The condition number is the ratio between the
117  * largest and smallest eigenvalues of the 6x6 covariance matrix of the cloud. The closer this number is to 1.0,
118  * the more stable the cloud is for ICP registration.
119  * \param[in] covariance_matrix user given covariance matrix. Assumed to be self adjoint/symmetric.
120  * \return the condition number
121  */
122  static double
123  computeConditionNumber (const Eigen::Matrix<double, 6, 6> &covariance_matrix);
124 
125  /** \brief Computes the covariance matrix of the input cloud.
126  * \param[out] covariance_matrix the computed covariance matrix.
127  * \return whether the computation succeeded or not
128  */
129  bool
130  computeCovarianceMatrix (Eigen::Matrix<double, 6, 6> &covariance_matrix);
131 
132  protected:
133  /** \brief Number of indices that will be returned. */
134  unsigned int num_samples_;
135 
136  /** \brief The normals computed at each point in the input cloud */
137  NormalsConstPtr input_normals_;
138 
139  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > scaled_points_;
140 
141  bool
142  initCompute ();
143 
144  /** \brief Sample of point indices into a separate PointCloud
145  * \param[out] output the resultant point cloud
146  */
147  void
148  applyFilter (Cloud &output) override;
149 
150  /** \brief Sample of point indices
151  * \param[out] indices the resultant point cloud indices
152  */
153  void
154  applyFilter (std::vector<int> &indices) override;
155 
156  static bool
157  sort_dot_list_function (std::pair<int, double> a,
158  std::pair<int, double> b)
159  { return (a.second > b.second); }
160 
161  public:
163  };
164 }
165 
166 #ifdef PCL_NO_PRECOMPILE
167 #include <pcl/filters/impl/covariance_sampling.hpp>
168 #endif
unsigned int num_samples_
Number of indices that will be returned.
Point Cloud sampling based on the 6D covariances.
CovarianceSampling()
Empty constructor.
void setNormals(const NormalsConstPtr &normals)
Set the normals computed on the input point cloud.
unsigned int getNumberOfSamples() const
Get the value of the internal num_samples_ parameter.
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > scaled_points_
NormalsConstPtr getNormals() const
Get the normals computed on the input point cloud.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
boost::shared_ptr< CovarianceSampling< PointT, PointNT > > Ptr
void setNumberOfSamples(unsigned int samples)
Set number of indices to be sampled.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:344
FilterIndices represents the base class for filters that are about binary point removal.
NormalsConstPtr input_normals_
The normals computed at each point in the input cloud.
static bool sort_dot_list_function(std::pair< int, double > a, std::pair< int, double > b)
boost::shared_ptr< const CovarianceSampling< PointT, PointNT > > ConstPtr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void applyFilter(Cloud &output) override
Sample of point indices into a separate PointCloud.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
std::string filter_name_
The filter name.
Definition: filter.h:164
bool computeCovarianceMatrix(Eigen::Matrix< double, 6, 6 > &covariance_matrix)
Computes the covariance matrix of the input cloud.
double computeConditionNumber()
Compute the condition number of the input point cloud.