QDomElement PCA_transformer::serialize() const{
QDomDocument doc;
QDomElement elem = doc.createElement("Transformer");
elem.setAttribute("type",this->classname().c_str());
elem.setAttribute("name",this->name().c_str());
QStringList qvariate_names;
for(int i=0; i< variate_names_.size(); ++i) {
qvariate_names.append(variate_names_[i].c_str());
}
elem.setAttribute("variableNames",qvariate_names.join(";"));
QStringList qstr_mean;
for(int i=0; i< means_.rows(); ++i) {
qstr_mean.append( QString("%1").arg(means_(i)) );
}
elem.setAttribute("means",qstr_mean.join(";"));
QStringList qstr_eigvalues;
for(int i=0; i< eigenvalues_.rows(); ++i) {
qstr_eigvalues.append( QString("%1").arg(eigenvalues_(i)) );
}
elem.setAttribute("EigenValues",qstr_eigvalues.join(";"));
QStringList qstr_eigvectors;
for(int i=0; i< eigenvectors_.rows(); ++i) {
for(int j=0; j< eigenvectors_.cols(); ++j) {
qstr_eigvectors.append( QString("%1").arg(eigenvectors_(i,j)) );
}
}
elem.setAttribute("EigenVectors",qstr_eigvectors.join(";"));
return elem;
}
template<typename PointT> inline void
pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag)
{
if (!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");
Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
const size_t n = eigenvectors_.cols ();// number of eigen vectors
Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
Eigen::VectorXf h = y - input;
if (h.norm() > 0)
h.normalize ();
else
h.setZero ();
float gamma = h.dot(input - mean_.head<3>());
Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
D.block(0,0,n,n) = a * a.transpose();
D /= float(n)/float((n+1) * (n+1));
for(std::size_t i=0; i < a.size(); i++) {
D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
D(i,D.cols()-1) = D(D.rows()-1,i);
D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
}
Eigen::MatrixXf R(D.rows(), D.cols());
Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
Eigen::VectorXf alphap = D_evd.eigenvalues().real();
eigenvalues_.resize(eigenvalues_.size() +1);
for(std::size_t i=0; i<eigenvalues_.size(); i++) {
eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
R.col(i) = D.col(D.cols()-i-1);
}
Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
Up.rightCols<1>() = h;
eigenvectors_ = Up*R;
if (!basis_only_) {
Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
coefficients_(coefficients_.rows()-1,i) = 0;
coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
}
a.resize(a.size()+1);
a(a.size()-1) = 0;
coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
}
mean_.head<3>() = meanp;
switch (flag)
{
case increase:
if (eigenvectors_.rows() >= eigenvectors_.cols())
break;
case preserve:
if (!basis_only_)
coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
eigenvalues_.resize(eigenvalues_.size()-1);
break;
default:
PCL_ERROR("[pcl::PCA] unknown flag\n");
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PrincipalCurvaturesEstimation<PointInT, PointNT, PointOutT>::computePointPrincipalCurvatures (
const pcl::PointCloud<PointNT> &normals, int p_idx, const std::vector<int> &indices,
float &pcx, float &pcy, float &pcz, float &pc1, float &pc2)
{
EIGEN_ALIGN16 Eigen::Matrix3f I = Eigen::Matrix3f::Identity ();
Eigen::Vector3f n_idx (normals.points[p_idx].normal[0], normals.points[p_idx].normal[1], normals.points[p_idx].normal[2]);
EIGEN_ALIGN16 Eigen::Matrix3f M = I - n_idx * n_idx.transpose (); // projection matrix (into tangent plane)
// Project normals into the tangent plane
Eigen::Vector3f normal;
projected_normals_.resize (indices.size ());
xyz_centroid_.setZero ();
for (size_t idx = 0; idx < indices.size(); ++idx)
{
normal[0] = normals.points[indices[idx]].normal[0];
normal[1] = normals.points[indices[idx]].normal[1];
normal[2] = normals.points[indices[idx]].normal[2];
projected_normals_[idx] = M * normal;
xyz_centroid_ += projected_normals_[idx];
}
// Estimate the XYZ centroid
xyz_centroid_ /= indices.size ();
// Initialize to 0
covariance_matrix_.setZero ();
double demean_xy, demean_xz, demean_yz;
// For each point in the cloud
for (size_t idx = 0; idx < indices.size (); ++idx)
{
demean_ = projected_normals_[idx] - xyz_centroid_;
demean_xy = demean_[0] * demean_[1];
demean_xz = demean_[0] * demean_[2];
demean_yz = demean_[1] * demean_[2];
covariance_matrix_(0, 0) += demean_[0] * demean_[0];
covariance_matrix_(0, 1) += demean_xy;
covariance_matrix_(0, 2) += demean_xz;
covariance_matrix_(1, 0) += demean_xy;
covariance_matrix_(1, 1) += demean_[1] * demean_[1];
covariance_matrix_(1, 2) += demean_yz;
covariance_matrix_(2, 0) += demean_xz;
covariance_matrix_(2, 1) += demean_yz;
covariance_matrix_(2, 2) += demean_[2] * demean_[2];
}
// Extract the eigenvalues and eigenvectors
//Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> ei_symm (covariance_matrix_);
//eigenvalues_ = ei_symm.eigenvalues ();
//eigenvectors_ = ei_symm.eigenvectors ();
pcl::eigen33 (covariance_matrix_, eigenvectors_, eigenvalues_);
pcx = eigenvectors_ (0, 2);
pcy = eigenvectors_ (1, 2);
pcz = eigenvectors_ (2, 2);
pc1 = eigenvalues_ (2);
pc2 = eigenvalues_ (1);
}
