Point Cloud Library (PCL)  1.11.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
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 
typename Base::PointCloud PointCloud
Definition: pca.h:65
void update(const PointT &input, FLAG flag=preserve)
update PCA with a new point
Definition: pca.hpp:94
void reconstruct(const PointT &projection, PointT &input)
Reconstruct point from its projection.
Definition: pca.hpp:206
void project(const PointT &input, PointT &projection)
Project point on the eigenspace.
Definition: pca.hpp:164
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
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:56
FLAG
Updating method flag.
Definition: pca.h:77