Point Cloud Library (PCL) 1.12.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 * \author Alexandru E. Ichim, alex.e.ichim@gmail.com
60 */
61 template <typename PointT, typename PointNT>
62 class CovarianceSampling : public FilterIndices<PointT>
63 {
69
70 using Cloud = typename FilterIndices<PointT>::PointCloud;
71 using CloudPtr = typename Cloud::Ptr;
72 using CloudConstPtr = typename Cloud::ConstPtr;
73 using NormalsConstPtr = typename pcl::PointCloud<PointNT>::ConstPtr;
74
75 public:
78
79 /** \brief Empty constructor. */
81 { filter_name_ = "CovarianceSampling"; }
82
83 /** \brief Set number of indices to be sampled.
84 * \param[in] samples the number of sample indices
85 */
86 inline void
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. Assumed to be self adjoint/symmetric.
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, Eigen::aligned_allocator<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) override;
150
151 /** \brief Sample of point indices
152 * \param[out] indices the resultant point cloud indices
153 */
154 void
155 applyFilter (Indices &indices) override;
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:
164 };
165}
166
167#ifdef PCL_NO_PRECOMPILE
168#include <pcl/filters/impl/covariance_sampling.hpp>
169#endif
Iterator class for point clouds with or without given indices.
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.
double computeConditionNumber()
Compute the condition number of the input point cloud.
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:63
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.