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);
Define methods for centroid estimation and covariance matrix calculus.
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
FLAG
Updating method flag.
void project(const PointT &input, PointT &projection)
Project point on the eigenspace.
void update(const PointT &input, FLAG flag=preserve)
update PCA with a new point
typename Base::PointCloud PointCloud
void reconstruct(const PointT &projection, PointT &input)
Reconstruct point from its projection.
Defines all the PCL implemented PointT point type structures.
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
A point structure representing Euclidean xyz coordinates, and the RGB color.