pca.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: pca.hpp 5294 2012-03-25 18:10:50Z rusu $
00037  */
00038 
00039 #ifndef PCL_PCA_IMPL_HPP
00040 #define PCL_PCA_IMPL_HPP
00041 
00042 #include <pcl/point_types.h>
00043 #include <pcl/common/centroid.h>
00044 #include <pcl/common/eigen.h>
00045 #include <pcl/common/transforms.h>
00046 #include <pcl/exceptions.h>
00047 
00049 
00053 template<typename PointT>
00054 pcl::PCA<PointT>::PCA (const pcl::PointCloud<PointT>& X, bool basis_only)
00055 {
00056   Base ();
00057   basis_only_ = basis_only;
00058   setInputCloud (X.makeShared ());
00059   compute_done_ = initCompute ();
00060 }
00061 
00063 template<typename PointT> bool
00064 pcl::PCA<PointT>::initCompute () 
00065 {
00066   if(!Base::initCompute ())
00067   {
00068     PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] failed");
00069     return (false);
00070   }
00071   if(indices_->size () < 3)
00072   {
00073     PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::initCompute] number of points < 3");
00074     return (false);
00075   }
00076   
00077   // Compute mean
00078   mean_ = Eigen::Vector4f::Zero ();
00079   compute3DCentroid (*input_, *indices_, mean_);  
00080   // Compute demeanished cloud
00081   Eigen::MatrixXf cloud_demean;
00082   demeanPointCloud (*input_, *indices_, mean_, cloud_demean);
00083   assert (cloud_demean.cols () == int (indices_->size ()));
00084   // Compute the product cloud_demean * cloud_demean^T
00085   Eigen::Matrix3f alpha = static_cast<Eigen::Matrix3f> (cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ());
00086   
00087   // Compute eigen vectors and values
00088   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
00089   // Organize eigenvectors and eigenvalues in ascendent order
00090   for (int i = 0; i < 3; ++i)
00091   {
00092     eigenvalues_[i] = evd.eigenvalues () [2-i];
00093     eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
00094   }
00095   // If not basis only then compute the coefficients
00096 
00097   if (!basis_only_)
00098     coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
00099   compute_done_ = true;
00100   return (true);
00101 }
00102 
00104 template<typename PointT> inline void 
00105 pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag) 
00106 {
00107   if (!compute_done_)
00108     initCompute ();
00109   if (!compute_done_)
00110     PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");
00111 
00112   Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
00113   const size_t n = eigenvectors_.cols ();// number of eigen vectors
00114   Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
00115   Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
00116   Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
00117   Eigen::VectorXf h = y - input;
00118   if (h.norm() > 0) 
00119     h.normalize ();
00120   else
00121     h.setZero ();
00122   float gamma = h.dot(input - mean_.head<3>());
00123   Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
00124   D.block(0,0,n,n) = a * a.transpose();
00125   D /=  float(n)/float((n+1) * (n+1));
00126   for(std::size_t i=0; i < a.size(); i++) {
00127     D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
00128     D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
00129     D(i,D.cols()-1) = D(D.rows()-1,i);
00130     D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
00131   }
00132 
00133   Eigen::MatrixXf R(D.rows(), D.cols());
00134   Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
00135   Eigen::VectorXf alphap = D_evd.eigenvalues().real();
00136   eigenvalues_.resize(eigenvalues_.size() +1);
00137   for(std::size_t i=0;i<eigenvalues_.size();i++) {
00138     eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
00139     R.col(i) = D.col(D.cols()-i-1);
00140   }
00141   Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
00142   Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
00143   Up.rightCols<1>() = h;
00144   eigenvectors_ = Up*R;
00145   if (!basis_only_) {
00146     Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
00147     coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
00148     for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
00149       coefficients_(coefficients_.rows()-1,i) = 0;
00150       coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
00151     }
00152     a.resize(a.size()+1);
00153     a(a.size()-1) = 0;
00154     coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
00155   }
00156   mean_.head<3>() = meanp;
00157   switch (flag) 
00158   {
00159     case increase:
00160       if (eigenvectors_.rows() >= eigenvectors_.cols())
00161         break;
00162     case preserve:
00163       if (!basis_only_)
00164         coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
00165       eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
00166       eigenvalues_.resize(eigenvalues_.size()-1);
00167       break;
00168     default:
00169       PCL_ERROR("[pcl::PCA] unknown flag\n");
00170   }
00171 }
00172 
00174 template<typename PointT> inline void
00175 pcl::PCA<PointT>::project (const PointT& input, PointT& projection)
00176 {
00177   if(!compute_done_)
00178     initCompute ();
00179   if (!compute_done_)
00180     PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
00181   
00182   Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
00183   projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
00184 }
00185 
00187 template<typename PointT> inline void
00188 pcl::PCA<PointT>::project (const PointCloud& input, PointCloud& projection)
00189 {
00190   if(!compute_done_)
00191     initCompute ();
00192   if (!compute_done_)
00193     PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");
00194   if (input.is_dense)
00195   {
00196     projection.resize (input.size ());
00197     for (size_t i = 0; i < input.size (); ++i)
00198       project (input[i], projection[i]);
00199   }
00200   else
00201   {
00202     PointT p;
00203     for (size_t i = 0; i < input.size (); ++i)
00204     {
00205       if (!pcl_isfinite (input[i].x) || 
00206           !pcl_isfinite (input[i].y) ||
00207           !pcl_isfinite (input[i].z))
00208         continue;
00209       project (input[i], p);
00210       projection.push_back (p);
00211     }
00212   }
00213 }
00214 
00216 template<typename PointT> inline void
00217 pcl::PCA<PointT>::reconstruct (const PointT& projection, PointT& input)
00218 {
00219   if(!compute_done_)
00220     initCompute ();
00221   if (!compute_done_)
00222     PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
00223 
00224   input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
00225   input.getVector3fMap ()+= mean_.head<3> ();
00226 }
00227 
00229 template<typename PointT> inline void
00230 pcl::PCA<PointT>::reconstruct (const PointCloud& projection, PointCloud& input)
00231 {
00232   if(!compute_done_)
00233     initCompute ();
00234   if (!compute_done_)
00235     PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::reconstruct] PCA initCompute failed");
00236   if (input.is_dense)
00237   {
00238     input.resize (projection.size ());
00239     for (size_t i = 0; i < projection.size (); ++i)
00240       reconstruct (projection[i], input[i]);
00241   }
00242   else
00243   {
00244     PointT p;
00245     for (size_t i = 0; i < input.size (); ++i)
00246     {
00247       if (!pcl_isfinite (input[i].x) || 
00248           !pcl_isfinite (input[i].y) ||
00249           !pcl_isfinite (input[i].z))
00250         continue;
00251       reconstruct (projection[i], p);
00252       input.push_back (p);
00253     }
00254   }
00255 }
00256 
00257 #endif


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:23