Point Cloud Library (PCL) 1.13.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:
76 using Ptr = shared_ptr< CovarianceSampling<PointT, PointNT> >;
77 using ConstPtr = shared_ptr< const CovarianceSampling<PointT, PointNT> >;
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
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. 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
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: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.