Point Cloud Library (PCL)  1.12.1-dev
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 ///////////////////////////////////////////////////////////////////////////////
49 template<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 
77  average_norm /= double (scaled_points_.size ());
78  for (std::size_t p_i = 0; p_i < scaled_points_.size (); ++p_i)
79  scaled_points_[p_i] /= float (average_norm);
80 
81  return (true);
82 }
83 
84 ///////////////////////////////////////////////////////////////////////////////
85 template<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 ///////////////////////////////////////////////////////////////////////////////
97 template<typename PointT, typename PointNT> double
98 pcl::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 ///////////////////////////////////////////////////////////////////////////////
108 template<typename PointT, typename PointNT> bool
109 pcl::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 ///////////////////////////////////////////////////////////////////////////////
130 template<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 ///////////////////////////////////////////////////////////////////////////////
212 template<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