Point Cloud Library (PCL) 1.15.1
Loading...
Searching...
No Matches
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
45namespace pcl
46{
47 template <typename real, int dimension>
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::VectorAverage::noOfSamples_
unsigned int noOfSamples_
Definition vector_average.h:110
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