41 #include <Eigen/Eigenvalues>
44 #include <pcl/common/eigen.h>
45 #include <pcl/common/pca.h>
46 #include <pcl/common/transforms.h>
48 #include <pcl/exceptions.h>
54 template<
typename Po
intT>
bool
55 PCA<PointT>::initCompute ()
57 if(!Base::initCompute ())
59 PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::PCA::initCompute] failed");
61 if(indices_->size () < 3)
63 PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::PCA::initCompute] number of points < 3");
67 mean_ = Eigen::Vector4f::Zero ();
70 Eigen::MatrixXf cloud_demean;
72 assert (cloud_demean.cols () == int (indices_->size ()));
74 const Eigen::Matrix3f alpha = (1.f / (float (indices_->size ()) - 1.f))
75 * cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ();
78 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
80 for (
int i = 0; i < 3; ++i)
82 eigenvalues_[i] = evd.eigenvalues () [2-i];
83 eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
86 eigenvectors_.col(2) = eigenvectors_.col(0).cross(eigenvectors_.col(1));
89 coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
95 template<
typename Po
intT>
inline void
103 Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
104 const std::size_t n = eigenvectors_.cols ();
105 Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) /
float(n + 1);
106 Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
107 Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
108 Eigen::VectorXf h = y - input;
113 float gamma = h.dot(input - mean_.head<3>());
114 Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
115 D.block(0,0,n,n) = a * a.transpose();
116 D /= float(n)/float((n+1) * (n+1));
117 for(std::size_t i=0; i < a.size(); i++) {
118 D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
119 D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
120 D(i,D.cols()-1) = D(D.rows()-1,i);
121 D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
124 Eigen::MatrixXf R(D.rows(), D.cols());
125 Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D,
false);
126 Eigen::VectorXf alphap = D_evd.eigenvalues().real();
127 eigenvalues_.resize(eigenvalues_.size() +1);
128 for(std::size_t i=0;i<eigenvalues_.size();i++) {
129 eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
130 R.col(i) = D.col(D.cols()-i-1);
132 Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
133 Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
134 Up.rightCols<1>() = h;
135 eigenvectors_ = Up*R;
137 Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
138 coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
139 for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
140 coefficients_(coefficients_.rows()-1,i) = 0;
141 coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
143 a.resize(a.size()+1);
145 coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
147 mean_.head<3>() = meanp;
151 if (eigenvectors_.rows() >= eigenvectors_.cols())
155 coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
156 eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
157 eigenvalues_.resize(eigenvalues_.size()-1);
160 PCL_ERROR(
"[pcl::PCA] unknown flag\n");
165 template<
typename Po
intT>
inline void
173 Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
174 projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
178 template<
typename Po
intT>
inline void
187 projection.resize (input.size ());
188 for (std::size_t i = 0; i < input.size (); ++i)
189 project (input[i], projection[i]);
194 for (
const auto& pt: input)
196 if (!std::isfinite (pt.x) ||
197 !std::isfinite (pt.y) ||
198 !std::isfinite (pt.z))
201 projection.push_back (p);
207 template<
typename Po
intT>
inline void
213 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
215 input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
216 input.getVector3fMap ()+= mean_.head<3> ();
220 template<
typename Po
intT>
inline void
226 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
229 input.resize (projection.size ());
230 for (std::size_t i = 0; i < projection.size (); ++i)
231 reconstruct (projection[i], input[i]);
236 for (std::size_t i = 0; i < input.size (); ++i)
238 if (!std::isfinite (input[i].x) ||
239 !std::isfinite (input[i].y) ||
240 !std::isfinite (input[i].z))
242 reconstruct (projection[i], p);