42 #include <pcl/common/eigen.h>
43 #include <pcl/common/pca.h>
44 #include <pcl/common/transforms.h>
46 #include <pcl/exceptions.h>
52 template<
typename Po
intT>
bool
53 PCA<PointT>::initCompute ()
55 if(!Base::initCompute ())
57 PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::PCA::initCompute] failed");
59 if(indices_->size () < 3)
61 PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::PCA::initCompute] number of points < 3");
65 mean_ = Eigen::Vector4f::Zero ();
68 Eigen::MatrixXf cloud_demean;
70 assert (cloud_demean.cols () == int (indices_->size ()));
72 const Eigen::Matrix3f alpha = (1.f / (float (indices_->size ()) - 1.f))
73 * cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ();
76 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
78 for (
int i = 0; i < 3; ++i)
80 eigenvalues_[i] = evd.eigenvalues () [2-i];
81 eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
84 eigenvectors_.col(2) = eigenvectors_.col(0).cross(eigenvectors_.col(1));
87 coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
93 template<
typename Po
intT>
inline void
101 Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
102 const std::size_t n = eigenvectors_.cols ();
103 Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) /
float(n + 1);
104 Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
105 Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
106 Eigen::VectorXf h = y - input;
111 float gamma = h.dot(input - mean_.head<3>());
112 Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
113 D.block(0,0,n,n) = a * a.transpose();
114 D /= float(n)/float((n+1) * (n+1));
115 for(std::size_t i=0; i < a.size(); i++) {
116 D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
117 D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
118 D(i,D.cols()-1) = D(D.rows()-1,i);
119 D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
122 Eigen::MatrixXf R(D.rows(), D.cols());
123 Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D,
false);
124 Eigen::VectorXf alphap = D_evd.eigenvalues().real();
125 eigenvalues_.resize(eigenvalues_.size() +1);
126 for(std::size_t i=0;i<eigenvalues_.size();i++) {
127 eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
128 R.col(i) = D.col(D.cols()-i-1);
130 Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
131 Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
132 Up.rightCols<1>() = h;
133 eigenvectors_ = Up*R;
135 Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
136 coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
137 for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
138 coefficients_(coefficients_.rows()-1,i) = 0;
139 coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
141 a.resize(a.size()+1);
143 coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
145 mean_.head<3>() = meanp;
149 if (eigenvectors_.rows() >= eigenvectors_.cols())
153 coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
154 eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
155 eigenvalues_.resize(eigenvalues_.size()-1);
158 PCL_ERROR(
"[pcl::PCA] unknown flag\n");
163 template<
typename Po
intT>
inline void
171 Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
172 projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
176 template<
typename Po
intT>
inline void
185 projection.resize (input.size ());
186 for (std::size_t i = 0; i < input.size (); ++i)
187 project (input[i], projection[i]);
192 for (
const auto& pt: input)
194 if (!std::isfinite (pt.x) ||
195 !std::isfinite (pt.y) ||
196 !std::isfinite (pt.z))
199 projection.push_back (p);
205 template<
typename Po
intT>
inline void
211 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
213 input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
214 input.getVector3fMap ()+= mean_.head<3> ();
218 template<
typename Po
intT>
inline void
224 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
227 input.resize (projection.size ());
228 for (std::size_t i = 0; i < projection.size (); ++i)
229 reconstruct (projection[i], input[i]);
234 for (std::size_t i = 0; i < input.size (); ++i)
236 if (!std::isfinite (input[i].x) ||
237 !std::isfinite (input[i].y) ||
238 !std::isfinite (input[i].z))
240 reconstruct (projection[i], p);