39 #ifndef PCL_PCA_IMPL_HPP
40 #define PCL_PCA_IMPL_HPP
42 #include <pcl/point_types.h>
43 #include <pcl/common/centroid.h>
44 #include <pcl/common/eigen.h>
45 #include <pcl/common/transforms.h>
46 #include <pcl/exceptions.h>
49 template<
typename Po
intT>
bool
52 if(!Base::initCompute ())
54 PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::PCA::initCompute] failed");
56 if(indices_->size () < 3)
58 PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::PCA::initCompute] number of points < 3");
62 mean_ = Eigen::Vector4f::Zero ();
65 Eigen::MatrixXf cloud_demean;
67 assert (cloud_demean.cols () == int (indices_->size ()));
69 const Eigen::Matrix3f alpha = (1.f / (float (indices_->size ()) - 1.f))
70 * cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ();
73 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
75 for (
int i = 0; i < 3; ++i)
77 eigenvalues_[i] = evd.eigenvalues () [2-i];
78 eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
82 coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
88 template<
typename Po
intT>
inline void
96 Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
97 const size_t n = eigenvectors_.cols ();
98 Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) /
float(n + 1);
99 Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
100 Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
101 Eigen::VectorXf h = y - input;
106 float gamma = h.dot(input - mean_.head<3>());
107 Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
108 D.block(0,0,n,n) = a * a.transpose();
109 D /= float(n)/float((n+1) * (n+1));
110 for(std::size_t i=0; i < a.size(); i++) {
111 D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
112 D(D.rows()-1,i) =
float(n) / float((n+1) * (n+1)) * gamma * a(i);
113 D(i,D.cols()-1) = D(D.rows()-1,i);
114 D(D.rows()-1,D.cols()-1) =
float(n)/float((n+1) * (n+1)) * gamma * gamma;
117 Eigen::MatrixXf R(D.rows(), D.cols());
118 Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D,
false);
119 Eigen::VectorXf alphap = D_evd.eigenvalues().real();
120 eigenvalues_.resize(eigenvalues_.size() +1);
121 for(std::size_t i=0;i<eigenvalues_.size();i++) {
122 eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
123 R.col(i) = D.col(D.cols()-i-1);
125 Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
126 Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
127 Up.rightCols<1>() = h;
128 eigenvectors_ = Up*R;
130 Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
131 coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
132 for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
133 coefficients_(coefficients_.rows()-1,i) = 0;
134 coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
136 a.resize(a.size()+1);
138 coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
140 mean_.head<3>() = meanp;
144 if (eigenvectors_.rows() >= eigenvectors_.cols())
148 coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
149 eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
150 eigenvalues_.resize(eigenvalues_.size()-1);
153 PCL_ERROR(
"[pcl::PCA] unknown flag\n");
158 template<
typename Po
intT>
inline void
166 Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
167 projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
171 template<
typename Po
intT>
inline void
181 for (
size_t i = 0; i < input.
size (); ++i)
182 project (input[i], projection[i]);
187 for (
size_t i = 0; i < input.
size (); ++i)
189 if (!pcl_isfinite (input[i].x) ||
190 !pcl_isfinite (input[i].y) ||
191 !pcl_isfinite (input[i].z))
193 project (input[i], p);
200 template<
typename Po
intT>
inline void
206 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
208 input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
209 input.getVector3fMap ()+= mean_.head<3> ();
213 template<
typename Po
intT>
inline void
219 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
223 for (
size_t i = 0; i < projection.
size (); ++i)
224 reconstruct (projection[i], input[i]);
229 for (
size_t i = 0; i < input.
size (); ++i)
231 if (!pcl_isfinite (input[i].x) ||
232 !pcl_isfinite (input[i].y) ||
233 !pcl_isfinite (input[i].z))
235 reconstruct (projection[i], p);
Principal Component analysis (PCA) class.
void update(const PointT &input, FLAG flag=preserve)
update PCA with a new point
void reconstruct(const PointT &projection, PointT &input)
Reconstruct point from its projection.
void project(const PointT &input, PointT &projection)
Project point on the eigenspace.
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.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
A point structure representing Euclidean xyz coordinates, and the RGB color.
void resize(size_t n)
Resize the cloud.
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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
FLAG
Updating method flag.