Point Cloud Library (PCL)  1.14.1-dev
vector_average.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-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 
38 #pragma once
39 
40 #include <pcl/common/eigen.h> // for computeRoots, eigen33
41 #include <pcl/common/vector_average.h>
42 
43 #include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
44 
45 namespace pcl
46 {
47  template <typename real, int dimension>
48  VectorAverage<real, dimension>::VectorAverage ()
49  {
50  reset();
51  }
52 
53  template <typename real, int dimension>
54  inline void VectorAverage<real, dimension>::reset()
55  {
56  noOfSamples_ = 0;
57  accumulatedWeight_ = 0.0;
58  mean_.fill(0);
59  covariance_.fill(0);
60  }
61 
62  template <typename real, int dimension>
63  inline void VectorAverage<real, dimension>::add(const Eigen::Matrix<real, dimension, 1>& sample, real weight) {
64  if (weight == 0.0f)
65  return;
66 
67  ++noOfSamples_;
68  accumulatedWeight_ += weight;
69  real alpha = weight/accumulatedWeight_;
70 
71  Eigen::Matrix<real, dimension, 1> diff = sample - mean_;
72  covariance_ = (covariance_ + (diff * diff.transpose())*alpha)*(1.0f-alpha);
73 
74  mean_ += (diff)*alpha;
75 
76  //if (std::isnan(covariance_(0,0)))
77  //{
78  //std::cout << PVARN(weight);
79  //exit(0);
80  //}
81  }
82 
83  template <typename real, int dimension>
84  inline void VectorAverage<real, dimension>::doPCA(Eigen::Matrix<real, dimension, 1>& eigen_values, Eigen::Matrix<real, dimension, 1>& eigen_vector1,
85  Eigen::Matrix<real, dimension, 1>& eigen_vector2, Eigen::Matrix<real, dimension, 1>& eigen_vector3) const
86  {
87  // The following step is necessary for cases where the values in the covariance matrix are small
88  // In this case float accuracy is nor enough to calculate the eigenvalues and eigenvectors.
89  //Eigen::Matrix<double, dimension, dimension> tmp_covariance = covariance_.template cast<double>();
90  //Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, dimension, dimension> > ei_symm(tmp_covariance);
91  //eigen_values = ei_symm.eigenvalues().template cast<real>();
92  //Eigen::Matrix<real, dimension, dimension> eigen_vectors = ei_symm.eigenvectors().template cast<real>();
93 
94  //std::cout << "My covariance is \n"<<covariance_<<"\n";
95  //std::cout << "My mean is \n"<<mean_<<"\n";
96  //std::cout << "My Eigenvectors \n"<<eigen_vectors<<"\n";
97 
98  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<real, dimension, dimension> > ei_symm(covariance_);
99  eigen_values = ei_symm.eigenvalues();
100  Eigen::Matrix<real, dimension, dimension> eigen_vectors = ei_symm.eigenvectors();
101 
102  eigen_vector1 = eigen_vectors.col(0);
103  eigen_vector2 = eigen_vectors.col(1);
104  eigen_vector3 = eigen_vectors.col(2);
105  }
106 
107  template <typename real, int dimension>
108  inline void VectorAverage<real, dimension>::doPCA(Eigen::Matrix<real, dimension, 1>& eigen_values) const
109  {
110  // The following step is necessary for cases where the values in the covariance matrix are small
111  // In this case float accuracy is nor enough to calculate the eigenvalues and eigenvectors.
112  //Eigen::Matrix<double, dimension, dimension> tmp_covariance = covariance_.template cast<double>();
113  //Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, dimension, dimension> > ei_symm(tmp_covariance, false);
114  //eigen_values = ei_symm.eigenvalues().template cast<real>();
115 
116  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<real, dimension, dimension> > ei_symm(covariance_, false);
117  eigen_values = ei_symm.eigenvalues();
118  }
119 
120  template <typename real, int dimension>
121  inline void VectorAverage<real, dimension>::getEigenVector1(Eigen::Matrix<real, dimension, 1>& eigen_vector1) const
122  {
123  // The following step is necessary for cases where the values in the covariance matrix are small
124  // In this case float accuracy is nor enough to calculate the eigenvalues and eigenvectors.
125  //Eigen::Matrix<double, dimension, dimension> tmp_covariance = covariance_.template cast<double>();
126  //Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, dimension, dimension> > ei_symm(tmp_covariance);
127  //eigen_values = ei_symm.eigenvalues().template cast<real>();
128  //Eigen::Matrix<real, dimension, dimension> eigen_vectors = ei_symm.eigenvectors().template cast<real>();
129 
130  //std::cout << "My covariance is \n"<<covariance_<<"\n";
131  //std::cout << "My mean is \n"<<mean_<<"\n";
132  //std::cout << "My Eigenvectors \n"<<eigen_vectors<<"\n";
133 
134  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<real, dimension, dimension> > ei_symm(covariance_);
135  Eigen::Matrix<real, dimension, dimension> eigen_vectors = ei_symm.eigenvectors();
136  eigen_vector1 = eigen_vectors.col(0);
137  }
138 
139 
140  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
141  // Special cases for real=float & dimension=3 -> Partial specialization does not work with class templates. :( //
142  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
143  ///////////
144  // float //
145  ///////////
146  template <>
147  inline void VectorAverage<float, 3>::doPCA(Eigen::Matrix<float, 3, 1>& eigen_values, Eigen::Matrix<float, 3, 1>& eigen_vector1,
148  Eigen::Matrix<float, 3, 1>& eigen_vector2, Eigen::Matrix<float, 3, 1>& eigen_vector3) const
149  {
150  //std::cout << "Using specialized 3x3 version of doPCA!\n";
151  Eigen::Matrix<float, 3, 3> eigen_vectors;
152  eigen33(covariance_, eigen_vectors, eigen_values);
153  eigen_vector1 = eigen_vectors.col(0);
154  eigen_vector2 = eigen_vectors.col(1);
155  eigen_vector3 = eigen_vectors.col(2);
156  }
157  template <>
158  inline void VectorAverage<float, 3>::doPCA(Eigen::Matrix<float, 3, 1>& eigen_values) const
159  {
160  //std::cout << "Using specialized 3x3 version of doPCA!\n";
161  computeRoots (covariance_, eigen_values);
162  }
163  template <>
164  inline void VectorAverage<float, 3>::getEigenVector1(Eigen::Matrix<float, 3, 1>& eigen_vector1) const
165  {
166  //std::cout << "Using specialized 3x3 version of doPCA!\n";
167  Eigen::Vector3f::Scalar eigen_value;
168  Eigen::Vector3f eigen_vector;
169  eigen33(covariance_, eigen_value, eigen_vector);
170  eigen_vector1 = eigen_vector;
171  }
172 
173  ////////////
174  // double //
175  ////////////
176  template <>
177  inline void VectorAverage<double, 3>::doPCA(Eigen::Matrix<double, 3, 1>& eigen_values, Eigen::Matrix<double, 3, 1>& eigen_vector1,
178  Eigen::Matrix<double, 3, 1>& eigen_vector2, Eigen::Matrix<double, 3, 1>& eigen_vector3) const
179  {
180  //std::cout << "Using specialized 3x3 version of doPCA!\n";
181  Eigen::Matrix<double, 3, 3> eigen_vectors;
182  eigen33(covariance_, eigen_vectors, eigen_values);
183  eigen_vector1 = eigen_vectors.col(0);
184  eigen_vector2 = eigen_vectors.col(1);
185  eigen_vector3 = eigen_vectors.col(2);
186  }
187  template <>
188  inline void VectorAverage<double, 3>::doPCA(Eigen::Matrix<double, 3, 1>& eigen_values) const
189  {
190  //std::cout << "Using specialized 3x3 version of doPCA!\n";
191  computeRoots (covariance_, eigen_values);
192  }
193  template <>
194  inline void VectorAverage<double, 3>::getEigenVector1(Eigen::Matrix<double, 3, 1>& eigen_vector1) const
195  {
196  //std::cout << "Using specialized 3x3 version of doPCA!\n";
197  Eigen::Vector3d::Scalar eigen_value;
198  Eigen::Vector3d eigen_vector;
199  eigen33(covariance_, eigen_value, eigen_vector);
200  eigen_vector1 = eigen_vector;
201  }
202 } // namespace pcl
pcl::VectorAverage::add
void add(const VectorType &sample, real weight=1.0)
Add a new sample.
Definition: vector_average.hpp:63
pcl::VectorAverage::reset
void reset()
Reset the object to work with a new data set.
Definition: vector_average.hpp:54
pcl::VectorAverage::VectorAverage
VectorAverage()
Constructor - dimension gives the size of the vectors to work with.
Definition: vector_average.hpp:48
pcl::VectorAverage::doPCA
void doPCA(VectorType &eigen_values, VectorType &eigen_vector1, VectorType &eigen_vector2, VectorType &eigen_vector3) const
Do Principal component analysis.
Definition: vector_average.hpp:84
pcl::VectorAverage::getEigenVector1
void getEigenVector1(VectorType &eigen_vector1) const
Get the eigenvector corresponding to the smallest eigenvalue.
Definition: vector_average.hpp:121
pcl::eigen33
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition: eigen.hpp:295
pcl::computeRoots
void computeRoots(const Matrix &m, Roots &roots)
computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues
Definition: eigen.hpp:68