Point Cloud Library (PCL)  1.9.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/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 (%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 (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  return computeConditionNumber (covariance_matrix);
92 }
93 
94 
95 ///////////////////////////////////////////////////////////////////////////////
96 template<typename PointT, typename PointNT> double
97 pcl::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 ///////////////////////////////////////////////////////////////////////////////
107 template<typename PointT, typename PointNT> bool
108 pcl::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 (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 ///////////////////////////////////////////////////////////////////////////////
129 template<typename PointT, typename PointNT> void
130 pcl::CovarianceSampling<PointT, PointNT>::applyFilter (std::vector<int> &sampled_indices)
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<size_t> candidate_indices;
143  candidate_indices.resize (indices_->size ());
144  for (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  typedef Eigen::Matrix<double, 6, 1> Vector6d;
149  std::vector<Vector6d, Eigen::aligned_allocator<Vector6d> > v;
150  v.resize (candidate_indices.size ());
151  for (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 (size_t i = 0; i < 6; ++i)
164  {
165  for (size_t p_i = 0; p_i < candidate_indices.size (); ++p_i)
166  L[i].push_back (std::make_pair (p_i, fabs (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 (size_t sample_i = 0; sample_i < num_samples_; ++sample_i)
179  {
180  // Find the most unconstrained dimension, i.e., the minimum t
181  size_t min_t_i = 0;
182  for (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 (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 (int &sampled_index : sampled_indices)
206  sampled_index = (*indices_)[candidate_indices[sampled_index]];
207 }
208 
209 
210 ///////////////////////////////////////////////////////////////////////////////
211 template<typename PointT, typename PointNT> void
213 {
214  std::vector<int> 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 = uint32_t (output.size ());
221  output.is_dense = true;
222  for (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_ */
size_t size() const
Definition: point_cloud.h:447
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:414
FilterIndices represents the base class for filters that are about binary point removal.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:412
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.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:406
void applyFilter(Cloud &output) override
Sample of point indices into a separate PointCloud.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:417
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:454
bool computeCovarianceMatrix(Eigen::Matrix< double, 6, 6 > &covariance_matrix)
Computes the covariance matrix of the input cloud.
double computeConditionNumber()
Compute the condition number of the input point cloud.