39 #ifndef PCL_PCA_IMPL_HPP 40 #define PCL_PCA_IMPL_HPP 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))
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 project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
FLAG
Updating method flag.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
void update(const PointT &input, FLAG flag=preserve)
update PCA with a new point
Defines all the PCL implemented PointT point type structures.
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.
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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(size_t n)
Resize the cloud.
A point structure representing Euclidean xyz coordinates, and the RGB color.
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.
Define methods for centroid estimation and covariance matrix calculus.