39 #ifndef PCL_PCA_IMPL_HPP
40 #define PCL_PCA_IMPL_HPP
44 #include <pcl/common/eigen.h>
45 #include <pcl/common/transforms.h>
46 #include <pcl/exceptions.h>
49 template<
typename Po
intT>
bool
52 if(!Base::initCompute ())
54 PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::PCA::initCompute] failed");
56 if(indices_->size () < 3)
58 PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::PCA::initCompute] number of points < 3");
62 mean_ = Eigen::Vector4f::Zero ();
65 Eigen::MatrixXf cloud_demean;
67 assert (cloud_demean.cols () == int (indices_->size ()));
69 const Eigen::Matrix3f alpha = (1.f / (float (indices_->size ()) - 1.f))
70 * cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ();
73 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
75 for (
int i = 0; i < 3; ++i)
77 eigenvalues_[i] = evd.eigenvalues () [2-i];
78 eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
82 coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
88 template<
typename Po
intT>
inline void
96 Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
97 const size_t n = eigenvectors_.cols ();
98 Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) /
float(n + 1);
99 Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
100 Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
101 Eigen::VectorXf h = y - input;
106 float gamma = h.dot(input - mean_.head<3>());
107 Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
108 D.block(0,0,n,n) = a * a.transpose();
109 D /= float(n)/float((n+1) * (n+1));
110 for(std::size_t i=0; i < a.size(); i++) {
111 D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
112 D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
113 D(i,D.cols()-1) = D(D.rows()-1,i);
114 D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
117 Eigen::MatrixXf R(D.rows(), D.cols());
118 Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D,
false);
119 Eigen::VectorXf alphap = D_evd.eigenvalues().real();
120 eigenvalues_.resize(eigenvalues_.size() +1);
121 for(std::size_t i=0;i<eigenvalues_.size();i++) {
122 eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
123 R.col(i) = D.col(D.cols()-i-1);
125 Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
126 Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
127 Up.rightCols<1>() = h;
128 eigenvectors_ = Up*R;
130 Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
131 coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
132 for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
133 coefficients_(coefficients_.rows()-1,i) = 0;
134 coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
136 a.resize(a.size()+1);
138 coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
140 mean_.head<3>() = meanp;
144 if (eigenvectors_.rows() >= eigenvectors_.cols())
148 coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
149 eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
150 eigenvalues_.resize(eigenvalues_.size()-1);
153 PCL_ERROR(
"[pcl::PCA] unknown flag\n");
158 template<
typename Po
intT>
inline void
166 Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
167 projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
171 template<
typename Po
intT>
inline void
181 for (
size_t i = 0; i < input.
size (); ++i)
182 project (input[i], projection[i]);
187 for (
size_t i = 0; i < input.
size (); ++i)
189 if (!pcl_isfinite (input[i].x) ||
190 !pcl_isfinite (input[i].y) ||
191 !pcl_isfinite (input[i].z))
200 template<
typename Po
intT>
inline void
206 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
208 input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
209 input.getVector3fMap ()+= mean_.head<3> ();
213 template<
typename Po
intT>
inline void
219 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
223 for (
size_t i = 0; i < projection.
size (); ++i)
224 reconstruct (projection[i], input[i]);
229 for (
size_t i = 0; i < input.
size (); ++i)
231 if (!pcl_isfinite (input[i].x) ||
232 !pcl_isfinite (input[i].y) ||
233 !pcl_isfinite (input[i].z))
235 reconstruct (projection[i], p);