Point Cloud Library (PCL)
1.3.1
|
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