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);
81 eigenvectors_.col(2) = eigenvectors_.col(0).cross(eigenvectors_.col(1));
84 coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
90 template<
typename Po
intT>
inline void
98 Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
99 const std::size_t n = eigenvectors_.cols ();
100 Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) /
float(n + 1);
101 Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
102 Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
103 Eigen::VectorXf h = y - input;
108 float gamma = h.dot(input - mean_.head<3>());
109 Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
110 D.block(0,0,n,n) = a * a.transpose();
111 D /= float(n)/float((n+1) * (n+1));
112 for(std::size_t i=0; i < a.size(); i++) {
113 D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
114 D(D.rows()-1,i) =
float(n) / float((n+1) * (n+1)) * gamma * a(i);
115 D(i,D.cols()-1) = D(D.rows()-1,i);
116 D(D.rows()-1,D.cols()-1) =
float(n)/float((n+1) * (n+1)) * gamma * gamma;
119 Eigen::MatrixXf R(D.rows(), D.cols());
120 Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D,
false);
121 Eigen::VectorXf alphap = D_evd.eigenvalues().real();
122 eigenvalues_.resize(eigenvalues_.size() +1);
123 for(std::size_t i=0;i<eigenvalues_.size();i++) {
124 eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
125 R.col(i) = D.col(D.cols()-i-1);
127 Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
128 Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
129 Up.rightCols<1>() = h;
130 eigenvectors_ = Up*R;
132 Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
133 coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
134 for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
135 coefficients_(coefficients_.rows()-1,i) = 0;
136 coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
138 a.resize(a.size()+1);
140 coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
142 mean_.head<3>() = meanp;
146 if (eigenvectors_.rows() >= eigenvectors_.cols())
150 coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
151 eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
152 eigenvalues_.resize(eigenvalues_.size()-1);
155 PCL_ERROR(
"[pcl::PCA] unknown flag\n");
160 template<
typename Po
intT>
inline void
168 Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
169 projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
173 template<
typename Po
intT>
inline void
182 projection.resize (input.size ());
183 for (std::size_t i = 0; i < input.size (); ++i)
184 project (input[i], projection[i]);
189 for (
const auto& pt: input)
191 if (!std::isfinite (pt.x) ||
192 !std::isfinite (pt.y) ||
193 !std::isfinite (pt.z))
196 projection.push_back (p);
202 template<
typename Po
intT>
inline void
208 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
210 input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
211 input.getVector3fMap ()+= mean_.head<3> ();
215 template<
typename Po
intT>
inline void
221 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
224 input.resize (projection.size ());
225 for (std::size_t i = 0; i < projection.size (); ++i)
226 reconstruct (projection[i], input[i]);
231 for (std::size_t i = 0; i < input.size (); ++i)
233 if (!std::isfinite (input[i].x) ||
234 !std::isfinite (input[i].y) ||
235 !std::isfinite (input[i].z))
237 reconstruct (projection[i], p);