Point Cloud Library (PCL)  1.7.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 /** \brief Constructor with direct computation
50  * \param[in] X input m*n matrix (ie n vectors of R(m))
51  * \param[in] basis_only flag to compute only the PCA basis
52  */
53 template<typename PointT>
54 pcl::PCA<PointT>::PCA (const pcl::PointCloud<PointT>& X, bool basis_only)
55 {
56  Base ();
57  basis_only_ = basis_only;
58  setInputCloud (X.makeShared ());
59  compute_done_ = initCompute ();
60 }
61 
62 /////////////////////////////////////////////////////////////////////////////////////////
63 template<typename PointT> bool
65 {
66  if(!Base::initCompute ())
67  {
68  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] failed");
69  return (false);
70  }
71  if(indices_->size () < 3)
72  {
73  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] number of points < 3");
74  return (false);
75  }
76 
77  // Compute mean
78  mean_ = Eigen::Vector4f::Zero ();
79  compute3DCentroid (*input_, *indices_, mean_);
80  // Compute demeanished cloud
81  Eigen::MatrixXf cloud_demean;
82  demeanPointCloud (*input_, *indices_, mean_, cloud_demean);
83  assert (cloud_demean.cols () == int (indices_->size ()));
84  // Compute the product cloud_demean * cloud_demean^T
85  Eigen::Matrix3f alpha = static_cast<Eigen::Matrix3f> (cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ());
86 
87  // Compute eigen vectors and values
88  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
89  // Organize eigenvectors and eigenvalues in ascendent order
90  for (int i = 0; i < 3; ++i)
91  {
92  eigenvalues_[i] = evd.eigenvalues () [2-i];
93  eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
94  }
95  // If not basis only then compute the coefficients
96 
97  if (!basis_only_)
98  coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
99  compute_done_ = true;
100  return (true);
101 }
102 
103 /////////////////////////////////////////////////////////////////////////////////////////
104 template<typename PointT> inline void
105 pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag)
106 {
107  if (!compute_done_)
108  initCompute ();
109  if (!compute_done_)
110  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");
111 
112  Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
113  const size_t n = eigenvectors_.cols ();// number of eigen vectors
114  Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
115  Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
116  Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
117  Eigen::VectorXf h = y - input;
118  if (h.norm() > 0)
119  h.normalize ();
120  else
121  h.setZero ();
122  float gamma = h.dot(input - mean_.head<3>());
123  Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
124  D.block(0,0,n,n) = a * a.transpose();
125  D /= float(n)/float((n+1) * (n+1));
126  for(std::size_t i=0; i < a.size(); i++) {
127  D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
128  D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
129  D(i,D.cols()-1) = D(D.rows()-1,i);
130  D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
131  }
132 
133  Eigen::MatrixXf R(D.rows(), D.cols());
134  Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
135  Eigen::VectorXf alphap = D_evd.eigenvalues().real();
136  eigenvalues_.resize(eigenvalues_.size() +1);
137  for(std::size_t i=0;i<eigenvalues_.size();i++) {
138  eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
139  R.col(i) = D.col(D.cols()-i-1);
140  }
141  Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
142  Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
143  Up.rightCols<1>() = h;
144  eigenvectors_ = Up*R;
145  if (!basis_only_) {
146  Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
147  coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
148  for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
149  coefficients_(coefficients_.rows()-1,i) = 0;
150  coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
151  }
152  a.resize(a.size()+1);
153  a(a.size()-1) = 0;
154  coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
155  }
156  mean_.head<3>() = meanp;
157  switch (flag)
158  {
159  case increase:
160  if (eigenvectors_.rows() >= eigenvectors_.cols())
161  break;
162  case preserve:
163  if (!basis_only_)
164  coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
165  eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
166  eigenvalues_.resize(eigenvalues_.size()-1);
167  break;
168  default:
169  PCL_ERROR("[pcl::PCA] unknown flag\n");
170  }
171 }
172 
173 /////////////////////////////////////////////////////////////////////////////////////////
174 template<typename PointT> inline void
175 pcl::PCA<PointT>::project (const PointT& input, PointT& projection)
176 {
177  if(!compute_done_)
178  initCompute ();
179  if (!compute_done_)
180  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
181 
182  Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
183  projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
184 }
185 
186 /////////////////////////////////////////////////////////////////////////////////////////
187 template<typename PointT> inline void
189 {
190  if(!compute_done_)
191  initCompute ();
192  if (!compute_done_)
193  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
194  if (input.is_dense)
195  {
196  projection.resize (input.size ());
197  for (size_t i = 0; i < input.size (); ++i)
198  project (input[i], projection[i]);
199  }
200  else
201  {
202  PointT p;
203  for (size_t i = 0; i < input.size (); ++i)
204  {
205  if (!pcl_isfinite (input[i].x) ||
206  !pcl_isfinite (input[i].y) ||
207  !pcl_isfinite (input[i].z))
208  continue;
209  project (input[i], p);
210  projection.push_back (p);
211  }
212  }
213 }
214 
215 /////////////////////////////////////////////////////////////////////////////////////////
216 template<typename PointT> inline void
217 pcl::PCA<PointT>::reconstruct (const PointT& projection, PointT& input)
218 {
219  if(!compute_done_)
220  initCompute ();
221  if (!compute_done_)
222  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
223 
224  input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
225  input.getVector3fMap ()+= mean_.head<3> ();
226 }
227 
228 /////////////////////////////////////////////////////////////////////////////////////////
229 template<typename PointT> inline void
231 {
232  if(!compute_done_)
233  initCompute ();
234  if (!compute_done_)
235  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
236  if (input.is_dense)
237  {
238  input.resize (projection.size ());
239  for (size_t i = 0; i < projection.size (); ++i)
240  reconstruct (projection[i], input[i]);
241  }
242  else
243  {
244  PointT p;
245  for (size_t i = 0; i < input.size (); ++i)
246  {
247  if (!pcl_isfinite (input[i].x) ||
248  !pcl_isfinite (input[i].y) ||
249  !pcl_isfinite (input[i].z))
250  continue;
251  reconstruct (projection[i], p);
252  input.push_back (p);
253  }
254  }
255 }
256 
257 #endif