Point Cloud Library (PCL)  1.3.1
pca.hpp
Go to the documentation of this file.
00001 #include <Eigen/Eigenvalues>
00002 #include "pcl/point_types.h"
00003 #include "pcl/common/centroid.h"
00004 #include <pcl/console/print.h>
00005 
00007 template<typename PointT> inline void 
00008 pcl::PCA<PointT>::compute (const pcl::PointCloud<PointT>& cloud) 
00009 {
00010   // Compute mean and covariance
00011   mean_ = Eigen::Vector4f::Zero ();
00012   compute3DCentroid (cloud, mean_);
00013   Eigen::Matrix3f covariance;
00014   pcl::computeCovarianceMatrixNormalized (cloud, mean_, covariance);
00015   
00016   // Compute demeanished cloud
00017   Eigen::MatrixXf cloud_demean;
00018   demeanPointCloud (cloud, mean_, cloud_demean);
00019 
00020   // Compute eigen vectors and values
00021   Eigen::EigenSolver<Eigen::Matrix3f> evd (covariance, true);
00022   eigenvalues_ = evd.eigenvalues ().real ();
00023   eigenvectors_ = evd.eigenvectors ().real ();
00024   if (!basis_only_)
00025     coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3>();
00026   compute_done_ = true;
00027 }
00028 
00030 template<typename PointT> inline void 
00031 pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag) 
00032 {
00033   if (!compute_done_)
00034     PCL_ERROR ("[pcl::PCA::update] PCA computing still not done.\n");
00035 
00036   Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
00037   const size_t n = eigenvectors_.cols ();// number of eigen vectors
00038   Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
00039   Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
00040   Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
00041   Eigen::VectorXf h = y - input;
00042   if (h.norm() > 0) 
00043     h.normalize ();
00044   else
00045     h.setZero ();
00046   float gamma = h.dot(input - mean_.head<3>());
00047   Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
00048   D.block(0,0,n,n) = a * a.transpose();
00049   D /=  float(n)/float((n+1) * (n+1));
00050   for(std::size_t i=0; i < a.size(); i++) {
00051     D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
00052     D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
00053     D(i,D.cols()-1) = D(D.rows()-1,i);
00054     D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
00055   }
00056 
00057   Eigen::MatrixXf R(D.rows(), D.cols());
00058   Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
00059   Eigen::VectorXf alphap = D_evd.eigenvalues().real();
00060   eigenvalues_.resize(eigenvalues_.size() +1);
00061   for(std::size_t i=0;i<eigenvalues_.size();i++) {
00062     eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
00063     R.col(i) = D.col(D.cols()-i-1);
00064   }
00065   Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
00066   Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
00067   Up.rightCols<1>() = h;
00068   eigenvectors_ = Up*R;
00069   if (!basis_only_) {
00070     Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
00071     coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
00072     for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
00073       coefficients_(coefficients_.rows()-1,i) = 0;
00074       coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
00075     }
00076     a.resize(a.size()+1);
00077     a(a.size()-1) = 0;
00078     coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
00079   }
00080   mean_.head<3>() = meanp;
00081   switch (flag) 
00082   {
00083     case increase:
00084       if (eigenvectors_.rows() >= eigenvectors_.cols())
00085         break;
00086     case preserve:
00087       if (!basis_only_)
00088         coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
00089       eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
00090       eigenvalues_.resize(eigenvalues_.size()-1);
00091       break;
00092     default:
00093       PCL_ERROR("[pcl::PCA] unknown flag\n");
00094   }
00095 }
00096 
00097 template<typename PointT>
00098 inline void pcl::PCA<PointT>::project(const PointT& input, PointT& projection) const 
00099 {
00100   if(!compute_done_)
00101     PCL_ERROR("[pcl::PCA::project] PCA computing still not done\n");
00102   Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3>();
00103   projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
00104 }
00105 
00106 template<typename PointT>
00107 inline void pcl::PCA<PointT>::reconstruct(const PointT& projection, PointT& input) const 
00108 {
00109   if(!compute_done_)
00110     PCL_ERROR("[pcl::PCA::reconstruct] PCA computing still not done\n");
00111   input.getVector3fMap () = (eigenvectors_ * projection.getVector3fMap ()) + mean_.head<3>();
00112 }
00113 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines