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 = (
static_cast<float>(n) * (mean_.head<3>() + input)) /
static_cast<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 /=
static_cast<float>(n)/
static_cast<float>((n+1) * (n+1));
117 for(std::size_t i=0; i < a.size(); i++) {
118 D(i,i)+=
static_cast<float>(n)/
static_cast<float>(n+1)*eigenvalues_(i);
119 D(D.rows()-1,i) =
static_cast<float>(n) /
static_cast<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) =
static_cast<float>(n)/
static_cast<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");
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);
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.