Point Cloud Library (PCL)  1.12.1-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 #pragma once
40 
41 #include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
42 
43 #include <pcl/common/centroid.h>
44 #include <pcl/common/eigen.h>
45 #include <pcl/common/pca.h>
46 #include <pcl/common/transforms.h>
47 #include <pcl/point_types.h>
48 #include <pcl/exceptions.h>
49 
50 
51 namespace pcl
52 {
53 
54 template<typename PointT> bool
55 PCA<PointT>::initCompute ()
56 {
57  if(!Base::initCompute ())
58  {
59  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] failed");
60  }
61  if(indices_->size () < 3)
62  {
63  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] number of points < 3");
64  }
65 
66  // Compute mean
67  mean_ = Eigen::Vector4f::Zero ();
68  compute3DCentroid (*input_, *indices_, mean_);
69  // Compute demeanished cloud
70  Eigen::MatrixXf cloud_demean;
71  demeanPointCloud (*input_, *indices_, mean_, cloud_demean);
72  assert (cloud_demean.cols () == int (indices_->size ()));
73  // Compute the product cloud_demean * cloud_demean^T
74  const Eigen::Matrix3f alpha = (1.f / (float (indices_->size ()) - 1.f))
75  * cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ();
76 
77  // Compute eigen vectors and values
78  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
79  // Organize eigenvectors and eigenvalues in ascendent order
80  for (int i = 0; i < 3; ++i)
81  {
82  eigenvalues_[i] = evd.eigenvalues () [2-i];
83  eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
84  }
85  // Enforce right hand rule
86  eigenvectors_.col(2) = eigenvectors_.col(0).cross(eigenvectors_.col(1));
87  // If not basis only then compute the coefficients
88  if (!basis_only_)
89  coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
90  compute_done_ = true;
91  return (true);
92 }
93 
94 
95 template<typename PointT> inline void
96 PCA<PointT>::update (const PointT& input_point, FLAG flag)
97 {
98  if (!compute_done_)
99  initCompute ();
100  if (!compute_done_)
101  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");
102 
103  Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
104  const std::size_t n = eigenvectors_.cols ();// number of eigen vectors
105  Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
106  Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
107  Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
108  Eigen::VectorXf h = y - input;
109  if (h.norm() > 0)
110  h.normalize ();
111  else
112  h.setZero ();
113  float gamma = h.dot(input - mean_.head<3>());
114  Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
115  D.block(0,0,n,n) = a * a.transpose();
116  D /= float(n)/float((n+1) * (n+1));
117  for(std::size_t i=0; i < a.size(); i++) {
118  D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
119  D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
120  D(i,D.cols()-1) = D(D.rows()-1,i);
121  D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
122  }
123 
124  Eigen::MatrixXf R(D.rows(), D.cols());
125  Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
126  Eigen::VectorXf alphap = D_evd.eigenvalues().real();
127  eigenvalues_.resize(eigenvalues_.size() +1);
128  for(std::size_t i=0;i<eigenvalues_.size();i++) {
129  eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
130  R.col(i) = D.col(D.cols()-i-1);
131  }
132  Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
133  Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
134  Up.rightCols<1>() = h;
135  eigenvectors_ = Up*R;
136  if (!basis_only_) {
137  Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
138  coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
139  for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
140  coefficients_(coefficients_.rows()-1,i) = 0;
141  coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
142  }
143  a.resize(a.size()+1);
144  a(a.size()-1) = 0;
145  coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
146  }
147  mean_.head<3>() = meanp;
148  switch (flag)
149  {
150  case increase:
151  if (eigenvectors_.rows() >= eigenvectors_.cols())
152  break;
153  case preserve:
154  if (!basis_only_)
155  coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
156  eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
157  eigenvalues_.resize(eigenvalues_.size()-1);
158  break;
159  default:
160  PCL_ERROR("[pcl::PCA] unknown flag\n");
161  }
162 }
163 
164 
165 template<typename PointT> inline void
166 PCA<PointT>::project (const PointT& input, PointT& projection)
167 {
168  if(!compute_done_)
169  initCompute ();
170  if (!compute_done_)
171  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
172 
173  Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
174  projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
175 }
176 
177 
178 template<typename PointT> inline void
179 PCA<PointT>::project (const PointCloud& input, PointCloud& projection)
180 {
181  if(!compute_done_)
182  initCompute ();
183  if (!compute_done_)
184  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
185  if (input.is_dense)
186  {
187  projection.resize (input.size ());
188  for (std::size_t i = 0; i < input.size (); ++i)
189  project (input[i], projection[i]);
190  }
191  else
192  {
193  PointT p;
194  for (const auto& pt: input)
195  {
196  if (!std::isfinite (pt.x) ||
197  !std::isfinite (pt.y) ||
198  !std::isfinite (pt.z))
199  continue;
200  project (pt, p);
201  projection.push_back (p);
202  }
203  }
204 }
205 
206 
207 template<typename PointT> inline void
208 PCA<PointT>::reconstruct (const PointT& projection, PointT& input)
209 {
210  if(!compute_done_)
211  initCompute ();
212  if (!compute_done_)
213  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
214 
215  input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
216  input.getVector3fMap ()+= mean_.head<3> ();
217 }
218 
219 
220 template<typename PointT> inline void
221 PCA<PointT>::reconstruct (const PointCloud& projection, PointCloud& input)
222 {
223  if(!compute_done_)
224  initCompute ();
225  if (!compute_done_)
226  PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
227  if (input.is_dense)
228  {
229  input.resize (projection.size ());
230  for (std::size_t i = 0; i < projection.size (); ++i)
231  reconstruct (projection[i], input[i]);
232  }
233  else
234  {
235  PointT p;
236  for (std::size_t i = 0; i < input.size (); ++i)
237  {
238  if (!std::isfinite (input[i].x) ||
239  !std::isfinite (input[i].y) ||
240  !std::isfinite (input[i].z))
241  continue;
242  reconstruct (projection[i], p);
243  input.push_back (p);
244  }
245  }
246 }
247 
248 } // namespace pcl
249 
Define methods for centroid estimation and covariance matrix calculus.
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:194
FLAG
Updating method flag.
Definition: pca.h:78
void project(const PointT &input, PointT &projection)
Project point on the eigenspace.
Definition: pca.hpp:166
void update(const PointT &input, FLAG flag=preserve)
update PCA with a new point
Definition: pca.hpp:96
typename Base::PointCloud PointCloud
Definition: pca.h:65
void reconstruct(const PointT &projection, PointT &input)
Reconstruct point from its projection.
Definition: pca.hpp:208
Defines all the PCL implemented PointT point type structures.
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:663
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:56
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
Definition: geometry.h:81
A point structure representing Euclidean xyz coordinates, and the RGB color.