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