Point Cloud Library (PCL) 1.12.0
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 /= 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 average_norm /= double (scaled_points_.size ());
77 for (std::size_t p_i = 0; p_i < scaled_points_.size (); ++p_i)
78 scaled_points_[p_i] /= float (average_norm);
79
80 return (true);
81}
82
83///////////////////////////////////////////////////////////////////////////////
84template<typename PointT, typename PointNT> double
86{
87 Eigen::Matrix<double, 6, 6> covariance_matrix;
88 if (!computeCovarianceMatrix (covariance_matrix))
89 return (-1.);
90
91 return computeConditionNumber (covariance_matrix);
92}
93
94
95///////////////////////////////////////////////////////////////////////////////
96template<typename PointT, typename PointNT> double
97pcl::CovarianceSampling<PointT, PointNT>::computeConditionNumber (const Eigen::Matrix<double, 6, 6> &covariance_matrix)
98{
99 const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6> > solver (covariance_matrix, Eigen::EigenvaluesOnly);
100 const double max_ev = solver.eigenvalues (). maxCoeff ();
101 const double min_ev = solver.eigenvalues (). minCoeff ();
102 return (max_ev / min_ev);
103}
104
105
106///////////////////////////////////////////////////////////////////////////////
107template<typename PointT, typename PointNT> bool
108pcl::CovarianceSampling<PointT, PointNT>::computeCovarianceMatrix (Eigen::Matrix<double, 6, 6> &covariance_matrix)
109{
110 if (!initCompute ())
111 return false;
112
113 //--- Part A from the paper
114 // Set up matrix F
115 Eigen::Matrix<double, 6, Eigen::Dynamic> f_mat = Eigen::Matrix<double, 6, Eigen::Dynamic> (6, indices_->size ());
116 for (std::size_t p_i = 0; p_i < scaled_points_.size (); ++p_i)
117 {
118 f_mat.block<3, 1> (0, p_i) = scaled_points_[p_i].cross (
119 (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ()).template cast<double> ();
120 f_mat.block<3, 1> (3, p_i) = (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ().template cast<double> ();
121 }
122
123 // Compute the covariance matrix C and its 6 eigenvectors (initially complex, move them to a double matrix)
124 covariance_matrix = f_mat * f_mat.transpose ();
125 return true;
126}
127
128///////////////////////////////////////////////////////////////////////////////
129template<typename PointT, typename PointNT> void
131{
132 Eigen::Matrix<double, 6, 6> c_mat;
133 // Invokes initCompute()
134 if (!computeCovarianceMatrix (c_mat))
135 return;
136
137 const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6> > solver (c_mat);
138 const Eigen::Matrix<double, 6, 6> x = solver.eigenvectors ();
139
140 //--- Part B from the paper
141 /// TODO figure out how to fill the candidate_indices - see subsequent paper paragraphs
142 std::vector<std::size_t> candidate_indices;
143 candidate_indices.resize (indices_->size ());
144 for (std::size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
145 candidate_indices[p_i] = p_i;
146
147 // Compute the v 6-vectors
148 using Vector6d = Eigen::Matrix<double, 6, 1>;
149 std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > v;
150 v.resize (candidate_indices.size ());
151 for (std::size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
152 {
153 v[p_i].block<3, 1> (0, 0) = scaled_points_[p_i].cross (
154 (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ()).template cast<double> ();
155 v[p_i].block<3, 1> (3, 0) = (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ().template cast<double> ();
156 }
157
158
159 // Set up the lists to be sorted
160 std::vector<std::list<std::pair<int, double> > > L;
161 L.resize (6);
162
163 for (std::size_t i = 0; i < 6; ++i)
164 {
165 for (std::size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
166 L[i].push_back (std::make_pair (p_i, std::abs (v[p_i].dot (x.block<6, 1> (0, i)))));
167
168 // Sort in decreasing order
169 L[i].sort (sort_dot_list_function);
170 }
171
172 // Initialize the 6 t's
173 std::vector<double> t (6, 0.0);
174
175 sampled_indices.resize (num_samples_);
176 std::vector<bool> point_sampled (candidate_indices.size (), false);
177 // Now select the actual points
178 for (std::size_t sample_i = 0; sample_i < num_samples_; ++sample_i)
179 {
180 // Find the most unconstrained dimension, i.e., the minimum t
181 std::size_t min_t_i = 0;
182 for (std::size_t i = 0; i < 6; ++i)
183 {
184 if (t[min_t_i] > t[i])
185 min_t_i = i;
186 }
187
188 // Add the point from the top of the list corresponding to the dimension to the set of samples
189 while (point_sampled [L[min_t_i].front ().first])
190 L[min_t_i].pop_front ();
191
192 sampled_indices[sample_i] = L[min_t_i].front ().first;
193 point_sampled[L[min_t_i].front ().first] = true;
194 L[min_t_i].pop_front ();
195
196 // Update the running totals
197 for (std::size_t i = 0; i < 6; ++i)
198 {
199 double val = v[sampled_indices[sample_i]].dot (x.block<6, 1> (0, i));
200 t[i] += val * val;
201 }
202 }
203
204 // Remap the sampled_indices to the input_ cloud
205 for (auto &sampled_index : sampled_indices)
206 sampled_index = (*indices_)[candidate_indices[sampled_index]];
207}
208
209
210///////////////////////////////////////////////////////////////////////////////
211template<typename PointT, typename PointNT> void
213{
214 Indices sampled_indices;
215 applyFilter (sampled_indices);
216
217 output.resize (sampled_indices.size ());
218 output.header = input_->header;
219 output.height = 1;
220 output.width = output.size ();
221 output.is_dense = true;
222 for (std::size_t i = 0; i < sampled_indices.size (); ++i)
223 output[i] = (*input_)[sampled_indices[i]];
224}
225
226
227#define PCL_INSTANTIATE_CovarianceSampling(T,NT) template class PCL_EXPORTS pcl::CovarianceSampling<T,NT>;
228
229#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:180
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133