Point Cloud Library (PCL)  1.10.0
pca.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  */
38 
39 #ifndef PCL_PCA_IMPL_HPP
40 #define PCL_PCA_IMPL_HPP
41 
42 #include <pcl/point_types.h>
43 #include <pcl/common/centroid.h>
44 #include <pcl/common/eigen.h>
45 #include <pcl/common/transforms.h>
46 #include <pcl/exceptions.h>
47 
48 /////////////////////////////////////////////////////////////////////////////////////////
49 template<typename PointT> bool
51 {
52  if(!Base::initCompute ())
53  {
54  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] failed");
55  }
56  if(indices_->size () < 3)
57  {
58  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] number of points < 3");
59  }
60 
61  // Compute mean
62  mean_ = Eigen::Vector4f::Zero ();
63  compute3DCentroid (*input_, *indices_, mean_);
64  // Compute demeanished cloud
65  Eigen::MatrixXf cloud_demean;
66  demeanPointCloud (*input_, *indices_, mean_, cloud_demean);
67  assert (cloud_demean.cols () == int (indices_->size ()));
68  // Compute the product cloud_demean * cloud_demean^T
69  const Eigen::Matrix3f alpha = (1.f / (float (indices_->size ()) - 1.f))
70  * cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ();
71 
72  // Compute eigen vectors and values
73  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
74  // Organize eigenvectors and eigenvalues in ascendent order
75  for (int i = 0; i < 3; ++i)
76  {
77  eigenvalues_[i] = evd.eigenvalues () [2-i];
78  eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
79  }
80  // Enforce right hand rule
81  eigenvectors_.col(2) = eigenvectors_.col(0).cross(eigenvectors_.col(1));
82  // If not basis only then compute the coefficients
83  if (!basis_only_)
84  coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
85  compute_done_ = true;
86  return (true);
87 }
88 
89 /////////////////////////////////////////////////////////////////////////////////////////
90 template<typename PointT> inline void
91 pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag)
92 {
93  if (!compute_done_)
94  initCompute ();
95  if (!compute_done_)
96  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");
97 
98  Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
99  const std::size_t n = eigenvectors_.cols ();// number of eigen vectors
100  Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
101  Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
102  Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
103  Eigen::VectorXf h = y - input;
104  if (h.norm() > 0)
105  h.normalize ();
106  else
107  h.setZero ();
108  float gamma = h.dot(input - mean_.head<3>());
109  Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
110  D.block(0,0,n,n) = a * a.transpose();
111  D /= float(n)/float((n+1) * (n+1));
112  for(std::size_t i=0; i < a.size(); i++) {
113  D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
114  D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
115  D(i,D.cols()-1) = D(D.rows()-1,i);
116  D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
117  }
118 
119  Eigen::MatrixXf R(D.rows(), D.cols());
120  Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
121  Eigen::VectorXf alphap = D_evd.eigenvalues().real();
122  eigenvalues_.resize(eigenvalues_.size() +1);
123  for(std::size_t i=0;i<eigenvalues_.size();i++) {
124  eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
125  R.col(i) = D.col(D.cols()-i-1);
126  }
127  Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
128  Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
129  Up.rightCols<1>() = h;
130  eigenvectors_ = Up*R;
131  if (!basis_only_) {
132  Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
133  coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
134  for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
135  coefficients_(coefficients_.rows()-1,i) = 0;
136  coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
137  }
138  a.resize(a.size()+1);
139  a(a.size()-1) = 0;
140  coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
141  }
142  mean_.head<3>() = meanp;
143  switch (flag)
144  {
145  case increase:
146  if (eigenvectors_.rows() >= eigenvectors_.cols())
147  break;
148  case preserve:
149  if (!basis_only_)
150  coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
151  eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
152  eigenvalues_.resize(eigenvalues_.size()-1);
153  break;
154  default:
155  PCL_ERROR("[pcl::PCA] unknown flag\n");
156  }
157 }
158 
159 /////////////////////////////////////////////////////////////////////////////////////////
160 template<typename PointT> inline void
161 pcl::PCA<PointT>::project (const PointT& input, PointT& projection)
162 {
163  if(!compute_done_)
164  initCompute ();
165  if (!compute_done_)
166  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
167 
168  Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
169  projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
170 }
171 
172 /////////////////////////////////////////////////////////////////////////////////////////
173 template<typename PointT> inline void
175 {
176  if(!compute_done_)
177  initCompute ();
178  if (!compute_done_)
179  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
180  if (input.is_dense)
181  {
182  projection.resize (input.size ());
183  for (std::size_t i = 0; i < input.size (); ++i)
184  project (input[i], projection[i]);
185  }
186  else
187  {
188  PointT p;
189  for (const auto& pt: input)
190  {
191  if (!std::isfinite (pt.x) ||
192  !std::isfinite (pt.y) ||
193  !std::isfinite (pt.z))
194  continue;
195  project (pt, p);
196  projection.push_back (p);
197  }
198  }
199 }
200 
201 /////////////////////////////////////////////////////////////////////////////////////////
202 template<typename PointT> inline void
203 pcl::PCA<PointT>::reconstruct (const PointT& projection, PointT& input)
204 {
205  if(!compute_done_)
206  initCompute ();
207  if (!compute_done_)
208  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
209 
210  input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
211  input.getVector3fMap ()+= mean_.head<3> ();
212 }
213 
214 /////////////////////////////////////////////////////////////////////////////////////////
215 template<typename PointT> inline void
217 {
218  if(!compute_done_)
219  initCompute ();
220  if (!compute_done_)
221  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
222  if (input.is_dense)
223  {
224  input.resize (projection.size ());
225  for (std::size_t i = 0; i < projection.size (); ++i)
226  reconstruct (projection[i], input[i]);
227  }
228  else
229  {
230  PointT p;
231  for (std::size_t i = 0; i < input.size (); ++i)
232  {
233  if (!std::isfinite (input[i].x) ||
234  !std::isfinite (input[i].y) ||
235  !std::isfinite (input[i].z))
236  continue;
237  reconstruct (projection[i], p);
238  input.push_back (p);
239  }
240  }
241 }
242 
243 #endif
point_types.h
pcl::PCA::PointCloud
typename Base::PointCloud PointCloud
Definition: pca.h:65
pcl::PCA
Principal Component analysis (PCA) class.
Definition: pca.h:61
pcl::demeanPointCloud
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > &centroid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
Definition: centroid.hpp:625
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:623
pcl::PCA::FLAG
FLAG
Updating method flag.
Definition: pca.h:77
pcl::geometry::project
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
Definition: geometry.h:81
pcl::PCA::project
void project(const PointT &input, PointT &projection)
Project point on the eigenspace.
Definition: pca.hpp:161
pcl::InitFailedException
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:194
pcl::PCA::reconstruct
void reconstruct(const PointT &projection, PointT &input)
Reconstruct point from its projection.
Definition: pca.hpp:203
pcl::compute3DCentroid
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition: centroid.hpp:50
pcl::PCA::update
void update(const PointT &input, FLAG flag=preserve)
update PCA with a new point
Definition: pca.hpp:91
centroid.h