Point Cloud Library (PCL)  1.7.1
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/common/eigen.h>
45 #include <pcl/filters/covariance_sampling.h>
46 #include <list>
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 (%zu)\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 (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 (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 (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 ///////////////////////////////////////////////////////////////////////////////
84 template<typename PointT, typename PointNT> double
86 {
87  Eigen::Matrix<double, 6, 6> covariance_matrix;
88  if (!computeCovarianceMatrix (covariance_matrix))
89  return (-1.);
90 
91  Eigen::EigenSolver<Eigen::Matrix<double, 6, 6> > eigen_solver;
92  eigen_solver.compute (covariance_matrix, true);
93 
94  Eigen::MatrixXcd complex_eigenvalues = eigen_solver.eigenvalues ();
95 
96  double max_ev = std::numeric_limits<double>::min (),
97  min_ev = std::numeric_limits<double>::max ();
98  for (size_t i = 0; i < 6; ++i)
99  {
100  if (real (complex_eigenvalues (i, 0)) > max_ev)
101  max_ev = real (complex_eigenvalues (i, 0));
102 
103  if (real (complex_eigenvalues (i, 0)) < min_ev)
104  min_ev = real (complex_eigenvalues (i, 0));
105  }
106 
107  return (max_ev / min_ev);
108 }
109 
110 
111 ///////////////////////////////////////////////////////////////////////////////
112 template<typename PointT, typename PointNT> double
113 pcl::CovarianceSampling<PointT, PointNT>::computeConditionNumber (const Eigen::Matrix<double, 6, 6> &covariance_matrix)
114 {
115  Eigen::EigenSolver<Eigen::Matrix<double, 6, 6> > eigen_solver;
116  eigen_solver.compute (covariance_matrix, true);
117 
118  Eigen::MatrixXcd complex_eigenvalues = eigen_solver.eigenvalues ();
119 
120  double max_ev = std::numeric_limits<double>::min (),
121  min_ev = std::numeric_limits<double>::max ();
122  for (size_t i = 0; i < 6; ++i)
123  {
124  if (real (complex_eigenvalues (i, 0)) > max_ev)
125  max_ev = real (complex_eigenvalues (i, 0));
126 
127  if (real (complex_eigenvalues (i, 0)) < min_ev)
128  min_ev = real (complex_eigenvalues (i, 0));
129  }
130 
131  return (max_ev / min_ev);
132 }
133 
134 
135 ///////////////////////////////////////////////////////////////////////////////
136 template<typename PointT, typename PointNT> bool
137 pcl::CovarianceSampling<PointT, PointNT>::computeCovarianceMatrix (Eigen::Matrix<double, 6, 6> &covariance_matrix)
138 {
139  if (!initCompute ())
140  return false;
141 
142  //--- Part A from the paper
143  // Set up matrix F
144  Eigen::Matrix<double, 6, Eigen::Dynamic> f_mat = Eigen::Matrix<double, 6, Eigen::Dynamic> (6, indices_->size ());
145  for (size_t p_i = 0; p_i < scaled_points_.size (); ++p_i)
146  {
147  f_mat.block<3, 1> (0, p_i) = scaled_points_[p_i].cross (
148  (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ()).template cast<double> ();
149  f_mat.block<3, 1> (3, p_i) = (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ().template cast<double> ();
150  }
151 
152  // Compute the covariance matrix C and its 6 eigenvectors (initially complex, move them to a double matrix)
153  covariance_matrix = f_mat * f_mat.transpose ();
154  return true;
155 }
156 
157 ///////////////////////////////////////////////////////////////////////////////
158 template<typename PointT, typename PointNT> void
159 pcl::CovarianceSampling<PointT, PointNT>::applyFilter (std::vector<int> &sampled_indices)
160 {
161  if (!initCompute ())
162  return;
163 
164  //--- Part A from the paper
165  // Set up matrix F
166  Eigen::Matrix<double, 6, Eigen::Dynamic> f_mat = Eigen::Matrix<double, 6, Eigen::Dynamic> (6, indices_->size ());
167  for (size_t p_i = 0; p_i < scaled_points_.size (); ++p_i)
168  {
169  f_mat.block<3, 1> (0, p_i) = scaled_points_[p_i].cross (
170  (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ()).template cast<double> ();
171  f_mat.block<3, 1> (3, p_i) = (*input_normals_)[(*indices_)[p_i]].getNormalVector3fMap ().template cast<double> ();
172  }
173 
174  // Compute the covariance matrix C and its 6 eigenvectors (initially complex, move them to a double matrix)
175  Eigen::Matrix<double, 6, 6> c_mat (f_mat * f_mat.transpose ());
176 
177  Eigen::EigenSolver<Eigen::Matrix<double, 6, 6> > eigen_solver;
178  eigen_solver.compute (c_mat, true);
179  Eigen::MatrixXcd complex_eigenvectors = eigen_solver.eigenvectors ();
180 
181  Eigen::Matrix<double, 6, 6> x;
182  for (size_t i = 0; i < 6; ++i)
183  for (size_t j = 0; j < 6; ++j)
184  x (i, j) = real (complex_eigenvectors (i, j));
185 
186 
187  //--- Part B from the paper
188  /// TODO figure out how to fill the candidate_indices - see subsequent paper paragraphs
189  std::vector<size_t> candidate_indices;
190  candidate_indices.resize (indices_->size ());
191  for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
192  candidate_indices[p_i] = p_i;
193 
194  // Compute the v 6-vectors
195  typedef Eigen::Matrix<double, 6, 1> Vector6d;
196  std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > v;
197  v.resize (candidate_indices.size ());
198  for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
199  {
200  v[p_i].block<3, 1> (0, 0) = scaled_points_[p_i].cross (
201  (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ()).template cast<double> ();
202  v[p_i].block<3, 1> (3, 0) = (*input_normals_)[(*indices_)[candidate_indices[p_i]]].getNormalVector3fMap ().template cast<double> ();
203  }
204 
205 
206  // Set up the lists to be sorted
207  std::vector<std::list<std::pair<int, double> > > L;
208  L.resize (6);
209 
210  for (size_t i = 0; i < 6; ++i)
211  {
212  for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
213  L[i].push_back (std::make_pair (p_i, fabs (v[p_i].dot (x.block<6, 1> (0, i)))));
214 
215  // Sort in decreasing order
216  L[i].sort (sort_dot_list_function);
217  }
218 
219  // Initialize the 6 t's
220  std::vector<double> t (6, 0.0);
221 
222  sampled_indices.resize (num_samples_);
223  std::vector<bool> point_sampled (candidate_indices.size (), false);
224  // Now select the actual points
225  for (size_t sample_i = 0; sample_i < num_samples_; ++sample_i)
226  {
227  // Find the most unconstrained dimension, i.e., the minimum t
228  size_t min_t_i = 0;
229  for (size_t i = 0; i < 6; ++i)
230  {
231  if (t[min_t_i] > t[i])
232  min_t_i = i;
233  }
234 
235  // Add the point from the top of the list corresponding to the dimension to the set of samples
236  while (point_sampled [L[min_t_i].front ().first])
237  L[min_t_i].pop_front ();
238 
239  sampled_indices[sample_i] = L[min_t_i].front ().first;
240  point_sampled[L[min_t_i].front ().first] = true;
241  L[min_t_i].pop_front ();
242 
243  // Update the running totals
244  for (size_t i = 0; i < 6; ++i)
245  {
246  double val = v[sampled_indices[sample_i]].dot (x.block<6, 1> (0, i));
247  t[i] += val * val;
248  }
249  }
250 
251  // Remap the sampled_indices to the input_ cloud
252  for (size_t i = 0; i < sampled_indices.size (); ++i)
253  sampled_indices[i] = (*indices_)[candidate_indices[sampled_indices[i]]];
254 }
255 
256 
257 ///////////////////////////////////////////////////////////////////////////////
258 template<typename PointT, typename PointNT> void
260 {
261  std::vector<int> sampled_indices;
262  applyFilter (sampled_indices);
263 
264  output.resize (sampled_indices.size ());
265  output.header = input_->header;
266  output.height = 1;
267  output.width = uint32_t (output.size ());
268  output.is_dense = true;
269  for (size_t i = 0; i < sampled_indices.size (); ++i)
270  output[i] = (*input_)[sampled_indices[i]];
271 }
272 
273 
274 #define PCL_INSTANTIATE_CovarianceSampling(T,NT) template class PCL_EXPORTS pcl::CovarianceSampling<T,NT>;
275 
276 #endif /* PCL_FILTERS_IMPL_COVARIANCE_SAMPLING_H_ */