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>
53 template<
typename Po
intT>
57 basis_only_ = basis_only;
59 compute_done_ = initCompute ();
63 template<
typename Po
intT>
bool
66 if(!Base::initCompute ())
71 if(indices_->size () < 3)
73 PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::PCA::initCompute] number of points < 3");
78 mean_ = Eigen::Vector4f::Zero ();
81 Eigen::MatrixXf cloud_demean;
83 assert (cloud_demean.cols () == int (indices_->size ()));
85 Eigen::Matrix3f alpha =
static_cast<Eigen::Matrix3f
> (cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ());
88 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
90 for (
int i = 0; i < 3; ++i)
92 eigenvalues_[i] = evd.eigenvalues () [2-i];
93 eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
98 coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
104 template<
typename Po
intT>
inline void
112 Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
113 const size_t n = eigenvectors_.cols ();
114 Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) /
float(n + 1);
115 Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
116 Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
117 Eigen::VectorXf h = y - input;
122 float gamma = h.dot(input - mean_.head<3>());
123 Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
124 D.block(0,0,n,n) = a * a.transpose();
125 D /= float(n)/float((n+1) * (n+1));
126 for(std::size_t i=0; i < a.size(); i++) {
127 D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
128 D(D.rows()-1,i) =
float(n) / float((n+1) * (n+1)) * gamma * a(i);
129 D(i,D.cols()-1) = D(D.rows()-1,i);
130 D(D.rows()-1,D.cols()-1) =
float(n)/float((n+1) * (n+1)) * gamma * gamma;
133 Eigen::MatrixXf R(D.rows(), D.cols());
134 Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D,
false);
135 Eigen::VectorXf alphap = D_evd.eigenvalues().real();
136 eigenvalues_.resize(eigenvalues_.size() +1);
137 for(std::size_t i=0;i<eigenvalues_.size();i++) {
138 eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
139 R.col(i) = D.col(D.cols()-i-1);
141 Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
142 Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
143 Up.rightCols<1>() = h;
144 eigenvectors_ = Up*R;
146 Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
147 coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
148 for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
149 coefficients_(coefficients_.rows()-1,i) = 0;
150 coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
152 a.resize(a.size()+1);
154 coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
156 mean_.head<3>() = meanp;
160 if (eigenvectors_.rows() >= eigenvectors_.cols())
164 coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
165 eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
166 eigenvalues_.resize(eigenvalues_.size()-1);
169 PCL_ERROR(
"[pcl::PCA] unknown flag\n");
174 template<
typename Po
intT>
inline void
182 Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
183 projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
187 template<
typename Po
intT>
inline void
197 for (
size_t i = 0; i < input.
size (); ++i)
198 project (input[i], projection[i]);
203 for (
size_t i = 0; i < input.
size (); ++i)
205 if (!pcl_isfinite (input[i].x) ||
206 !pcl_isfinite (input[i].y) ||
207 !pcl_isfinite (input[i].z))
209 project (input[i], p);
216 template<
typename Po
intT>
inline void
222 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
224 input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
225 input.getVector3fMap ()+= mean_.head<3> ();
229 template<
typename Po
intT>
inline void
235 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
239 for (
size_t i = 0; i < projection.
size (); ++i)
240 reconstruct (projection[i], input[i]);
245 for (
size_t i = 0; i < input.
size (); ++i)
247 if (!pcl_isfinite (input[i].x) ||
248 !pcl_isfinite (input[i].y) ||
249 !pcl_isfinite (input[i].z))
251 reconstruct (projection[i], p);
Principal Component analysis (PCA) class.
FLAG
Updating method flag.
void update(const PointT &input, FLAG flag=preserve)
update PCA with a new point
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
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 resize(size_t n)
Resize the cloud.
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed, so avoid using this function on non-empty clouds.
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
PCA(bool basis_only=false)
Default Constructor.
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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).