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