Point Cloud Library (PCL)
1.7.0
|
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 the copyright holder(s) 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$ 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 00048 ///////////////////////////////////////////////////////////////////////////////////////// 00049 /** \brief Constructor with direct computation 00050 * \param[in] X input m*n matrix (ie n vectors of R(m)) 00051 * \param[in] basis_only flag to compute only the PCA basis 00052 */ 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 00062 ///////////////////////////////////////////////////////////////////////////////////////// 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 00103 ///////////////////////////////////////////////////////////////////////////////////////// 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 00173 ///////////////////////////////////////////////////////////////////////////////////////// 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 00186 ///////////////////////////////////////////////////////////////////////////////////////// 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 00215 ///////////////////////////////////////////////////////////////////////////////////////// 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 00228 ///////////////////////////////////////////////////////////////////////////////////////// 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