Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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/memory.h>
44#include <pcl/pcl_macros.h>
45#include <pcl/filters/filter_indices.h>
46
47namespace 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 * \ingroup filters
60 * \author Alexandru E. Ichim, alex.e.ichim@gmail.com
61 */
62 template <typename PointT, typename PointNT>
63 class CovarianceSampling : public FilterIndices<PointT>
64 {
70
71 using Cloud = typename FilterIndices<PointT>::PointCloud;
72 using CloudPtr = typename Cloud::Ptr;
73 using CloudConstPtr = typename Cloud::ConstPtr;
74 using NormalsConstPtr = typename pcl::PointCloud<PointNT>::ConstPtr;
75
76 public:
77 using Ptr = shared_ptr< CovarianceSampling<PointT, PointNT> >;
78 using ConstPtr = shared_ptr< const CovarianceSampling<PointT, PointNT> >;
79
80 /** \brief Empty constructor. */
82 { filter_name_ = "CovarianceSampling"; }
83
84 /** \brief Set number of indices to be sampled.
85 * \param[in] samples the number of sample indices
86 */
87 inline void
88 setNumberOfSamples (unsigned int samples)
89 { num_samples_ = samples; }
90
91 /** \brief Get the value of the internal \a num_samples_ parameter. */
92 inline unsigned int
94 { return (num_samples_); }
95
96 /** \brief Set the normals computed on the input point cloud
97 * \param[in] normals the normals computed for the input cloud
98 */
99 inline void
100 setNormals (const NormalsConstPtr &normals)
101 { input_normals_ = normals; }
102
103 /** \brief Get the normals computed on the input point cloud */
104 inline NormalsConstPtr
105 getNormals () const
106 { return (input_normals_); }
107
108
109
110 /** \brief Compute the condition number of the input point cloud. The condition number is the ratio between the
111 * largest and smallest eigenvalues of the 6x6 covariance matrix of the cloud. The closer this number is to 1.0,
112 * the more stable the cloud is for ICP registration.
113 * \return the condition number
114 */
115 double
117
118 /** \brief Compute the condition number of the input point cloud. The condition number is the ratio between the
119 * largest and smallest eigenvalues of the 6x6 covariance matrix of the cloud. The closer this number is to 1.0,
120 * the more stable the cloud is for ICP registration.
121 * \param[in] covariance_matrix user given covariance matrix. Assumed to be self adjoint/symmetric.
122 * \return the condition number
123 */
124 static double
125 computeConditionNumber (const Eigen::Matrix<double, 6, 6> &covariance_matrix);
126
127 /** \brief Computes the covariance matrix of the input cloud.
128 * \param[out] covariance_matrix the computed covariance matrix.
129 * \return whether the computation succeeded or not
130 */
131 bool
132 computeCovarianceMatrix (Eigen::Matrix<double, 6, 6> &covariance_matrix);
133
134 protected:
135 /** \brief Number of indices that will be returned. */
136 unsigned int num_samples_;
137
138 /** \brief The normals computed at each point in the input cloud */
139 NormalsConstPtr input_normals_;
140
141 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > scaled_points_;
142
143 bool
144 initCompute ();
145
146 /** \brief Sample of point indices into a separate PointCloud
147 * \param[out] output the resultant point cloud
148 */
149 void
150 applyFilter (Cloud &output) override;
151
152 /** \brief Sample of point indices
153 * \param[out] indices the resultant point cloud indices
154 */
155 void
156 applyFilter (Indices &indices) override;
157
158 static bool
159 sort_dot_list_function (std::pair<int, double> a,
160 std::pair<int, double> b)
161 { return (a.second > b.second); }
162
163 public:
165 };
166}
167
168#ifdef PCL_NO_PRECOMPILE
169#include <pcl/filters/impl/covariance_sampling.hpp>
170#endif
Point Cloud sampling based on the 6D covariances.
NormalsConstPtr input_normals_
The normals computed at each point in the input cloud.
CovarianceSampling()
Empty constructor.
void setNormals(const NormalsConstPtr &normals)
Set the normals computed on the input point cloud.
shared_ptr< const CovarianceSampling< PointT, PointNT > > ConstPtr
double computeConditionNumber()
Compute the condition number of the input point cloud.
shared_ptr< CovarianceSampling< PointT, PointNT > > Ptr
NormalsConstPtr getNormals() const
Get the normals computed on the input point cloud.
bool computeCovarianceMatrix(Eigen::Matrix< double, 6, 6 > &covariance_matrix)
Computes the covariance matrix of the input cloud.
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > scaled_points_
void setNumberOfSamples(unsigned int samples)
Set number of indices to be sampled.
static bool sort_dot_list_function(std::pair< int, double > a, std::pair< int, double > b)
unsigned int num_samples_
Number of indices that will be returned.
unsigned int getNumberOfSamples() const
Get the value of the internal num_samples_ parameter.
void applyFilter(Cloud &output) override
Sample of point indices into a separate PointCloud.
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition filter.h:174
std::string filter_name_
The filter name.
Definition filter.h:158
FilterIndices represents the base class for filters that are about binary point removal.
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< const PointCloud< PointT > > ConstPtr
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:86
Defines functions, macros and traits for allocating and using memory.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, and the RGB color.