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); }