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> 53 template<
typename Po
intT>
57 basis_only_ = basis_only;
59 compute_done_ = initCompute ();
63 template<
typename Po
intT>
bool 66 if(!Base::initCompute ())
71 if(indices_->size () < 3)
73 PCL_THROW_EXCEPTION (InitFailedException,
"[pcl::PCA::initCompute] number of points < 3");
78 mean_ = Eigen::Vector4f::Zero ();
81 Eigen::MatrixXf cloud_demean;
83 assert (cloud_demean.cols () == int (indices_->size ()));
85 Eigen::Matrix3f alpha =
static_cast<Eigen::Matrix3f
> (cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ());
88 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
90 for (
int i = 0; i < 3; ++i)
92 eigenvalues_[i] = evd.eigenvalues () [2-i];
93 eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
98 coefficients_ = eigenvectors_.transpose() * cloud_demean.topRows<3> ();
104 template<
typename Po
intT>
inline void 112 Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
113 const size_t n = eigenvectors_.cols ();
114 Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) /
float(n + 1);
115 Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
116 Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
117 Eigen::VectorXf h = y - input;
122 float gamma = h.dot(input - mean_.head<3>());
123 Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
124 D.block(0,0,n,n) = a * a.transpose();
125 D /= float(n)/float((n+1) * (n+1));
126 for(std::size_t i=0; i < a.size(); i++) {
127 D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
128 D(D.rows()-1,i) =
float(n) / float((n+1) * (n+1)) * gamma * a(i);
129 D(i,D.cols()-1) = D(D.rows()-1,i);
130 D(D.rows()-1,D.cols()-1) =
float(n)/float((n+1) * (n+1)) * gamma * gamma;
133 Eigen::MatrixXf R(D.rows(), D.cols());
134 Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D,
false);
135 Eigen::VectorXf alphap = D_evd.eigenvalues().real();
136 eigenvalues_.resize(eigenvalues_.size() +1);
137 for(std::size_t i=0;i<eigenvalues_.size();i++) {
138 eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
139 R.col(i) = D.col(D.cols()-i-1);
141 Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
142 Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
143 Up.rightCols<1>() = h;
144 eigenvectors_ = Up*R;
146 Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
147 coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
148 for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
149 coefficients_(coefficients_.rows()-1,i) = 0;
150 coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
152 a.resize(a.size()+1);
154 coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
156 mean_.head<3>() = meanp;
160 if (eigenvectors_.rows() >= eigenvectors_.cols())
164 coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
165 eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
166 eigenvalues_.resize(eigenvalues_.size()-1);
169 PCL_ERROR(
"[pcl::PCA] unknown flag\n");
174 template<
typename Po
intT>
inline void 182 Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
183 projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
187 template<
typename Po
intT>
inline void 197 for (
size_t i = 0; i < input.
size (); ++i)
198 project (input[i], projection[i]);
203 for (
size_t i = 0; i < input.
size (); ++i)
205 if (!pcl_isfinite (input[i].x) ||
206 !pcl_isfinite (input[i].y) ||
207 !pcl_isfinite (input[i].z))
216 template<
typename Po
intT>
inline void 222 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
224 input.getVector3fMap ()= eigenvectors_ * projection.getVector3fMap ();
225 input.getVector3fMap ()+= mean_.head<3> ();
229 template<
typename Po
intT>
inline void 235 PCL_THROW_EXCEPTION (
InitFailedException,
"[pcl::PCA::reconstruct] PCA initCompute failed");
239 for (
size_t i = 0; i < projection.
size (); ++i)
240 reconstruct (projection[i], input[i]);
245 for (
size_t i = 0; i < input.
size (); ++i)
247 if (!pcl_isfinite (input[i].x) ||
248 !pcl_isfinite (input[i].y) ||
249 !pcl_isfinite (input[i].z))
251 reconstruct (projection[i], p);
Principal Component analysis (PCA) class.
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
FLAG
Updating method flag.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
void update(const PointT &input, FLAG flag=preserve)
update PCA with a new point
Defines all the PCL implemented PointT point type structures.
void reconstruct(const PointT &projection, PointT &input)
Reconstruct point from its projection.
void project(const PointT &input, PointT &projection)
Project point on the eigenspace.
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.
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
PCA(bool basis_only=false)
Default Constructor.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed, so avoid using this function on non-empty clouds.
void resize(size_t n)
Resize the cloud.
A point structure representing Euclidean xyz coordinates, and the RGB color.
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.
Define methods for centroid estimation and covariance matrix calculus.