Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
51namespace pcl
52{
53
54template<typename PointT> bool
55PCA<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
95template<typename PointT> inline void
96PCA<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
165template<typename PointT> inline void
166PCA<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
178template<typename PointT> inline void
179PCA<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
207template<typename PointT> inline void
208PCA<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
220template<typename PointT> inline void
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: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
A point structure representing Euclidean xyz coordinates, and the RGB color.