41#include <Eigen/Eigenvalues>
44#include <pcl/common/eigen.h>
45#include <pcl/common/pca.h>
46#include <pcl/common/transforms.h>
48#include <pcl/exceptions.h>
54template<
typename Po
intT>
bool
55PCA<PointT>::initCompute ()
57 if(!Base::initCompute ())
59 PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::PCA::initCompute] failed");
61 if(indices_->size () < 3)
63 PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::PCA::initCompute] number of points < 3");
67 mean_ = Eigen::Vector4f::Zero ();
74 const Eigen::Matrix3f
alpha = (1.f / (
float (indices_->size ()) - 1.f))
78 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f>
evd (
alpha);
80 for (
int i = 0; i < 3; ++i)
82 eigenvalues_[i] =
evd.eigenvalues () [2-i];
83 eigenvectors_.col (i) =
evd.eigenvectors ().col (2-i);
86 eigenvectors_.col(2) = eigenvectors_.col(0).cross(eigenvectors_.col(1));
89 coefficients_ = eigenvectors_.transpose() *
cloud_demean.topRows<3> ();
95template<
typename Po
intT>
inline void
104 const std::size_t n = eigenvectors_.cols ();
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;
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();
117 for(std::size_t i=0; i < a.size(); 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;
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);
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;
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;
143 a.resize(a.size()+1);
145 coefficients_.col(coefficients_.cols()-1) = (
R.transpose() * a) +
etha;
147 mean_.head<3>() =
meanp;
151 if (eigenvectors_.rows() >= eigenvectors_.cols())
155 coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
156 eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
157 eigenvalues_.resize(eigenvalues_.size()-1);
160 PCL_ERROR(
"[pcl::PCA] unknown flag\n");
165template<
typename Po
intT>
inline void
178template<
typename Po
intT>
inline void
188 for (std::size_t i = 0; i <
input.
size (); ++i)
196 if (!std::isfinite (
pt.x) ||
197 !std::isfinite (
pt.y) ||
198 !std::isfinite (
pt.z))
207template<
typename Po
intT>
inline void
213 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
216 input.getVector3fMap ()+= mean_.head<3> ();
220template<
typename Po
intT>
inline void
226 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
236 for (std::size_t i = 0; i <
input.
size (); ++i)
238 if (!std::isfinite (
input[i].x) ||
239 !std::isfinite (
input[i].y) ||
240 !std::isfinite (
input[i].z))
Define methods for centroid estimation and covariance matrix calculus.
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
FLAG
Updating method flag.
void project(const PointT &input, PointT &projection)
Project point on the eigenspace.
void update(const PointT &input, FLAG flag=preserve)
update PCA with a new point
typename Base::PointCloud PointCloud
void reconstruct(const PointT &projection, PointT &input)
Reconstruct point from its projection.
Defines all the PCL implemented PointT point type structures.
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
A point structure representing Euclidean xyz coordinates, and the RGB color.