Point Cloud Library (PCL)  1.9.0-dev
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  // If not basis only then compute the coefficients
81  if (!basis_only_)
82  coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
83  compute_done_ = true;
84  return (true);
85 }
86 
87 /////////////////////////////////////////////////////////////////////////////////////////
88 template<typename PointT> inline void
89 pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag)
90 {
91  if (!compute_done_)
92  initCompute ();
93  if (!compute_done_)
94  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");
95 
96  Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
97  const size_t n = eigenvectors_.cols ();// number of eigen vectors
98  Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
99  Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
100  Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
101  Eigen::VectorXf h = y - input;
102  if (h.norm() > 0)
103  h.normalize ();
104  else
105  h.setZero ();
106  float gamma = h.dot(input - mean_.head<3>());
107  Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
108  D.block(0,0,n,n) = a * a.transpose();
109  D /= float(n)/float((n+1) * (n+1));
110  for(std::size_t i=0; i < a.size(); i++) {
111  D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
112  D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
113  D(i,D.cols()-1) = D(D.rows()-1,i);
114  D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
115  }
116 
117  Eigen::MatrixXf R(D.rows(), D.cols());
118  Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
119  Eigen::VectorXf alphap = D_evd.eigenvalues().real();
120  eigenvalues_.resize(eigenvalues_.size() +1);
121  for(std::size_t i=0;i<eigenvalues_.size();i++) {
122  eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
123  R.col(i) = D.col(D.cols()-i-1);
124  }
125  Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
126  Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
127  Up.rightCols<1>() = h;
128  eigenvectors_ = Up*R;
129  if (!basis_only_) {
130  Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
131  coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
132  for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
133  coefficients_(coefficients_.rows()-1,i) = 0;
134  coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
135  }
136  a.resize(a.size()+1);
137  a(a.size()-1) = 0;
138  coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
139  }
140  mean_.head<3>() = meanp;
141  switch (flag)
142  {
143  case increase:
144  if (eigenvectors_.rows() >= eigenvectors_.cols())
145  break;
146  case preserve:
147  if (!basis_only_)
148  coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
149  eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
150  eigenvalues_.resize(eigenvalues_.size()-1);
151  break;
152  default:
153  PCL_ERROR("[pcl::PCA] unknown flag\n");
154  }
155 }
156 
157 /////////////////////////////////////////////////////////////////////////////////////////
158 template<typename PointT> inline void
159 pcl::PCA<PointT>::project (const PointT& input, PointT& projection)
160 {
161  if(!compute_done_)
162  initCompute ();
163  if (!compute_done_)
164  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
165 
166  Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
167  projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
168 }
169 
170 /////////////////////////////////////////////////////////////////////////////////////////
171 template<typename PointT> inline void
173 {
174  if(!compute_done_)
175  initCompute ();
176  if (!compute_done_)
177  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
178  if (input.is_dense)
179  {
180  projection.resize (input.size ());
181  for (size_t i = 0; i < input.size (); ++i)
182  project (input[i], projection[i]);
183  }
184  else
185  {
186  PointT p;
187  for (size_t i = 0; i < input.size (); ++i)
188  {
189  if (!pcl_isfinite (input[i].x) ||
190  !pcl_isfinite (input[i].y) ||
191  !pcl_isfinite (input[i].z))
192  continue;
193  project (input[i], p);
194  projection.push_back (p);
195  }
196  }
197 }
198 
199 /////////////////////////////////////////////////////////////////////////////////////////
200 template<typename PointT> inline void
201 pcl::PCA<PointT>::reconstruct (const PointT& projection, PointT& input)
202 {
203  if(!compute_done_)
204  initCompute ();
205  if (!compute_done_)
206  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
207 
208  input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
209  input.getVector3fMap ()+= mean_.head<3> ();
210 }
211 
212 /////////////////////////////////////////////////////////////////////////////////////////
213 template<typename PointT> inline void
215 {
216  if(!compute_done_)
217  initCompute ();
218  if (!compute_done_)
219  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
220  if (input.is_dense)
221  {
222  input.resize (projection.size ());
223  for (size_t i = 0; i < projection.size (); ++i)
224  reconstruct (projection[i], input[i]);
225  }
226  else
227  {
228  PointT p;
229  for (size_t i = 0; i < input.size (); ++i)
230  {
231  if (!pcl_isfinite (input[i].x) ||
232  !pcl_isfinite (input[i].y) ||
233  !pcl_isfinite (input[i].z))
234  continue;
235  reconstruct (projection[i], p);
236  input.push_back (p);
237  }
238  }
239 }
240 
241 #endif
Principal Component analysis (PCA) class.
Definition: pca.h:60
size_t size() const
Definition: point_cloud.h:448
FLAG
Updating method flag.
Definition: pca.h:76
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:480
void update(const PointT &input, FLAG flag=preserve)
update PCA with a new point
Definition: pca.hpp:89
void reconstruct(const PointT &projection, PointT &input)
Reconstruct point from its projection.
Definition: pca.hpp:201
void project(const PointT &input, PointT &projection)
Project point on the eigenspace.
Definition: pca.hpp:159
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:631
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:194
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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:418
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:455
A point structure representing Euclidean xyz coordinates, and the RGB color.
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