41 #ifndef PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_
42 #define PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_
44 #include <pcl/features/principal_curvatures.h>
47 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
50 float &pcx,
float &pcy,
float &pcz,
float &pc1,
float &pc2)
52 EIGEN_ALIGN16 Eigen::Matrix3f I = Eigen::Matrix3f::Identity ();
53 Eigen::Vector3f n_idx (normals.
points[p_idx].normal[0], normals.
points[p_idx].normal[1], normals.
points[p_idx].normal[2]);
54 EIGEN_ALIGN16 Eigen::Matrix3f M = I - n_idx * n_idx.transpose ();
57 Eigen::Vector3f normal;
58 projected_normals_.resize (indices.size ());
59 xyz_centroid_.setZero ();
60 for (std::size_t idx = 0; idx < indices.size(); ++idx)
62 normal[0] = normals.
points[indices[idx]].normal[0];
63 normal[1] = normals.
points[indices[idx]].normal[1];
64 normal[2] = normals.
points[indices[idx]].normal[2];
66 projected_normals_[idx] = M * normal;
67 xyz_centroid_ += projected_normals_[idx];
71 xyz_centroid_ /= static_cast<float> (indices.size ());
74 covariance_matrix_.setZero ();
77 for (std::size_t idx = 0; idx < indices.size (); ++idx)
79 demean_ = projected_normals_[idx] - xyz_centroid_;
81 double demean_xy = demean_[0] * demean_[1];
82 double demean_xz = demean_[0] * demean_[2];
83 double demean_yz = demean_[1] * demean_[2];
85 covariance_matrix_(0, 0) += demean_[0] * demean_[0];
86 covariance_matrix_(0, 1) += static_cast<float> (demean_xy);
87 covariance_matrix_(0, 2) += static_cast<float> (demean_xz);
89 covariance_matrix_(1, 0) += static_cast<float> (demean_xy);
90 covariance_matrix_(1, 1) += demean_[1] * demean_[1];
91 covariance_matrix_(1, 2) += static_cast<float> (demean_yz);
93 covariance_matrix_(2, 0) += static_cast<float> (demean_xz);
94 covariance_matrix_(2, 1) += static_cast<float> (demean_yz);
95 covariance_matrix_(2, 2) += demean_[2] * demean_[2];
102 pcx = eigenvector_ [0];
103 pcy = eigenvector_ [1];
104 pcz = eigenvector_ [2];
105 float indices_size = 1.0f / static_cast<float> (indices.size ());
106 pc1 = eigenvalues_ [2] * indices_size;
107 pc2 = eigenvalues_ [1] * indices_size;
112 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
117 std::vector<int> nn_indices (k_);
118 std::vector<float> nn_dists (k_);
122 if (input_->is_dense)
125 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
127 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
129 output.
points[idx].principal_curvature[0] = output.
points[idx].principal_curvature[1] = output.
points[idx].principal_curvature[2] =
130 output.
points[idx].pc1 = output.
points[idx].pc2 = std::numeric_limits<float>::quiet_NaN ();
136 computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
137 output.
points[idx].principal_curvature[0], output.
points[idx].principal_curvature[1], output.
points[idx].principal_curvature[2],
144 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
146 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
147 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
149 output.
points[idx].principal_curvature[0] = output.
points[idx].principal_curvature[1] = output.
points[idx].principal_curvature[2] =
150 output.
points[idx].pc1 = output.
points[idx].pc2 = std::numeric_limits<float>::quiet_NaN ();
156 computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
157 output.
points[idx].principal_curvature[0], output.
points[idx].principal_curvature[1], output.
points[idx].principal_curvature[2],
163 #define PCL_INSTANTIATE_PrincipalCurvaturesEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PrincipalCurvaturesEstimation<T,NT,OutT>;
165 #endif // PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_