Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
covariance_sampling.hpp
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#ifndef PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_
42#define PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_
43
44#include <pcl/filters/covariance_sampling.h>
45#include <list>
46#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
47
48///////////////////////////////////////////////////////////////////////////////
49template<typename PointT, typename PointNT> bool
51{
53 return false;
54
55 if (num_samples_ > indices_->size ())
56 {
57 PCL_ERROR ("[pcl::CovarianceSampling::initCompute] The number of samples you asked for (%d) is larger than the number of input indices (%lu)\n",
58 num_samples_, indices_->size ());
59 return false;
60 }
61
62 // Prepare the point cloud by centering at the origin and then scaling the points such that the average distance from
63 // the origin is 1.0 => rotations and translations will have the same magnitude
64 Eigen::Vector3f centroid (0.f, 0.f, 0.f);
65 for (std::size_t p_i = 0; p_i < indices_->size (); ++p_i)
66 centroid += (*input_)[(*indices_)[p_i]].getVector3fMap ();
67 centroid /= static_cast<float>(indices_->size ());
68
69 scaled_points_.resize (indices_->size ());
70 double average_norm = 0.0;
71 for (std::size_t p_i = 0; p_i < indices_->size (); ++p_i)
72 {
73 scaled_points_[p_i] = (*input_)[(*indices_)[p_i]].getVector3fMap () - centroid;
74 average_norm += scaled_points_[p_i].norm ();
75 }
76
77 average_norm /= static_cast<double>(scaled_points_.size ());
78 for (auto & scaled_point : scaled_points_)
79 scaled_point /= static_cast<float>(average_norm);
80
81 return (true);
82}
83
84///////////////////////////////////////////////////////////////////////////////
85template<typename PointT, typename PointNT> double
87{
88 Eigen::Matrix<double, 6, 6> covariance_matrix;
89 if (!computeCovarianceMatrix (covariance_matrix))
90 return (-1.);
91
92 return computeConditionNumber (covariance_matrix);
93}
94
95
96///////////////////////////////////////////////////////////////////////////////
97template<typename PointT, typename PointNT> double
98pcl::CovarianceSampling<PointT, PointNT>::computeConditionNumber (const Eigen::Matrix<double, 6, 6> &covariance_matrix)
99{
100 const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6> > solver (covariance_matrix, Eigen::EigenvaluesOnly);
101 const double max_ev = solver.eigenvalues (). maxCoeff ();
102 const double min_ev = solver.eigenvalues (). minCoeff ();
103 return (max_ev / min_ev);
104}
105
106
107///////////////////////////////////////////////////////////////////////////////
108template<typename PointT, typename PointNT> bool
109pcl::CovarianceSampling<PointT, PointNT>::computeCovarianceMatrix (Eigen::Matrix<double, 6, 6> &covariance_matrix)
110{
111 if (!initCompute ())
112 return false;
113
114 //--- Part A from the paper
115 // Set up matrix F
116 Eigen::Matrix<double, 6, Eigen::Dynamic> f_mat = Eigen::Matrix<double, 6, Eigen::Dynamic> (6, indices_->size ());
117 for (std::size_t p_i = 0; p_i < scaled_points_.size (); ++p_i)
118 {
119 f_mat.block<3, 1> (0, p_i) = scaled_points_[p_i].cross (
120 (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ()).template cast<double> ();
121 f_mat.block<3, 1> (3, p_i) = (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ().template cast<double> ();
122 }
123
124 // Compute the covariance matrix C and its 6 eigenvectors (initially complex, move them to a double matrix)
125 covariance_matrix = f_mat * f_mat.transpose ();
126 return true;
127}
128
129///////////////////////////////////////////////////////////////////////////////
130template<typename PointT, typename PointNT> void
132{
133 Eigen::Matrix<double, 6, 6> c_mat;
134 // Invokes initCompute()
135 if (!computeCovarianceMatrix (c_mat))
136 return;
137
138 const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6> > solver (c_mat);
139 const Eigen::Matrix<double, 6, 6> x = solver.eigenvectors ();
140
141 //--- Part B from the paper
142 /// TODO figure out how to fill the candidate_indices - see subsequent paper paragraphs
143 std::vector<std::size_t> candidate_indices;
144 candidate_indices.resize (indices_->size ());
145 for (std::size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
146 candidate_indices[p_i] = p_i;
147
148 // Compute the v 6-vectors
149 using Vector6d = Eigen::Matrix<double, 6, 1>;
150 std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > v;
151 v.resize (candidate_indices.size ());
152 for (std::size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
153 {
154 v[p_i].block<3, 1> (0, 0) = scaled_points_[p_i].cross (
155 (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ()).template cast<double> ();
156 v[p_i].block<3, 1> (3, 0) = (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ().template cast<double> ();
157 }
158
159
160 // Set up the lists to be sorted
161 std::vector<std::list<std::pair<int, double> > > L;
162 L.resize (6);
163
164 for (std::size_t i = 0; i < 6; ++i)
165 {
166 for (std::size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
167 L[i].push_back (std::make_pair (p_i, std::abs (v[p_i].dot (x.block<6, 1> (0, i)))));
168
169 // Sort in decreasing order
170 L[i].sort (sort_dot_list_function);
171 }
172
173 // Initialize the 6 t's
174 std::vector<double> t (6, 0.0);
175
176 sampled_indices.resize (num_samples_);
177 std::vector<bool> point_sampled (candidate_indices.size (), false);
178 // Now select the actual points
179 for (std::size_t sample_i = 0; sample_i < num_samples_; ++sample_i)
180 {
181 // Find the most unconstrained dimension, i.e., the minimum t
182 std::size_t min_t_i = 0;
183 for (std::size_t i = 0; i < 6; ++i)
184 {
185 if (t[min_t_i] > t[i])
186 min_t_i = i;
187 }
188
189 // Add the point from the top of the list corresponding to the dimension to the set of samples
190 while (point_sampled [L[min_t_i].front ().first])
191 L[min_t_i].pop_front ();
192
193 sampled_indices[sample_i] = L[min_t_i].front ().first;
194 point_sampled[L[min_t_i].front ().first] = true;
195 L[min_t_i].pop_front ();
196
197 // Update the running totals
198 for (std::size_t i = 0; i < 6; ++i)
199 {
200 double val = v[sampled_indices[sample_i]].dot (x.block<6, 1> (0, i));
201 t[i] += val * val;
202 }
203 }
204
205 // Remap the sampled_indices to the input_ cloud
206 for (auto &sampled_index : sampled_indices)
207 sampled_index = (*indices_)[candidate_indices[sampled_index]];
208}
209
210
211///////////////////////////////////////////////////////////////////////////////
212template<typename PointT, typename PointNT> void
214{
215 Indices sampled_indices;
216 applyFilter (sampled_indices);
217
218 output.resize (sampled_indices.size ());
219 output.header = input_->header;
220 output.height = 1;
221 output.width = output.size ();
222 output.is_dense = true;
223 for (std::size_t i = 0; i < sampled_indices.size (); ++i)
224 output[i] = (*input_)[sampled_indices[i]];
225}
226
227
228#define PCL_INSTANTIATE_CovarianceSampling(T,NT) template class PCL_EXPORTS pcl::CovarianceSampling<T,NT>;
229
230#endif /* PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_ */
double computeConditionNumber()
Compute the condition number of the input point cloud.
bool computeCovarianceMatrix(Eigen::Matrix< double, 6, 6 > &covariance_matrix)
Computes the covariance matrix of the input cloud.
void applyFilter(Cloud &output) override
Sample of point indices into a separate PointCloud.
FilterIndices represents the base class for filters that are about binary point removal.
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
Definition centroid.hpp:191
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133