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_ /= static_cast<float> (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) += static_cast<float> (demean_xy);
covariance_matrix_(0, 2) += static_cast<float> (demean_xz);
covariance_matrix_(1, 0) += static_cast<float> (demean_xy);
covariance_matrix_(1, 1) += demean_[1] * demean_[1];
covariance_matrix_(1, 2) += static_cast<float> (demean_yz);
covariance_matrix_(2, 0) += static_cast<float> (demean_xz);
covariance_matrix_(2, 1) += static_cast<float> (demean_yz);
covariance_matrix_(2, 2) += demean_[2] * demean_[2];
}
// Extract the eigenvalues and eigenvectors
pcl::eigen33 (covariance_matrix_, eigenvalues_);
pcl::computeCorrespondingEigenVector (covariance_matrix_, eigenvalues_ [2], eigenvector_);
pcx = eigenvector_ [0];
pcy = eigenvector_ [1];
pcz = eigenvector_ [2];
float indices_size = 1.0f / static_cast<float> (indices.size ());
pc1 = eigenvalues_ [2] * indices_size;
pc2 = eigenvalues_ [1] * indices_size;
}
template <typename PointInT, typename PointNT, typename PointLabelT, typename PointOutT> void
cob_3d_features::OrganizedCurvatureEstimation<PointInT,PointNT,PointLabelT,PointOutT>::computePointCurvatures (
const NormalCloudIn &normals,
const int index,
const std::vector<int> &indices,
const std::vector<float> &sqr_distances,
float &pcx, float &pcy, float &pcz, float &pc1, float &pc2,
int &label_out)
{
Eigen::Vector3f n_idx(normals.points[index].normal);
Eigen::Matrix3f M = Eigen::Matrix3f::Identity() - n_idx * n_idx.transpose(); // projection matrix
std::vector<Eigen::Vector3f> normals_projected;
normals_projected.reserve(n_points_);
Eigen::Vector3f centroid = Eigen::Vector3f::Zero();
Eigen::Vector3f p_idx = surface_->points[index].getVector3fMap();
float angle = 0.0; // to discriminate convex and concave surface
int prob_concave = 0, prob_convex = 0;
for (std::vector<int>::const_iterator it = indices.begin(); it != indices.end(); ++it)
{
PointNT const* n_i = &(normals.points[*it]);
if ( pcl_isnan(n_i->normal[2]) ) continue;
normals_projected.push_back( M * Eigen::Vector3f(n_i->normal) );
centroid += normals_projected.back();
if ( (surface_->points[*it].getVector3fMap() - p_idx).normalized().dot(n_idx) > 0.0) ++prob_concave;
else ++prob_convex;
}
if (normals_projected.size() <=1) return;
float num_p_inv = 1.0f / normals_projected.size();
centroid *= num_p_inv;
Eigen::Matrix3f cov = Eigen::Matrix3f::Zero();
{
std::vector<Eigen::Vector3f>::iterator n_it = normals_projected.begin();
std::vector<float>::const_iterator d_it = sqr_distances.begin();
for (; n_it != normals_projected.end(); ++n_it, ++d_it)
{
Eigen::Vector3f demean = *n_it - centroid;
//cov += 1.0f / sqrt(*d_it) * demean * demean.transpose();
cov += demean * demean.transpose();
}
}
Eigen::Vector3f eigenvalues;
Eigen::Matrix3f eigenvectors;
pcl::eigen33(cov, eigenvectors, eigenvalues);
pcx = eigenvectors (0,2);
pcy = eigenvectors (1,2);
pcz = eigenvectors (2,2);
if (prob_concave < prob_convex) // if convex, make eigenvalues negative
num_p_inv *= surface_->points[index].z * (-1);
//num_p_inv *= 1.0 / (10.0*centroid.norm()) * surface_->points[index].z * (-1);
else
num_p_inv *= surface_->points[index].z;
pc1 = eigenvalues (2) * num_p_inv;
pc2 = eigenvalues (1) * num_p_inv;
//normals_->points[index].curvature = curvatures_->points[index].pc1;
if (std::abs(pc1) >= edge_curvature_threshold_ && label_out == 0)
label_out = I_EDGE;
}
