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 (
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 ();
76 double demean_xy, demean_xz, demean_yz;
78 for (
size_t idx = 0; idx < indices.size (); ++idx)
80 demean_ = projected_normals_[idx] - xyz_centroid_;
82 demean_xy = demean_[0] * demean_[1];
83 demean_xz = demean_[0] * demean_[2];
84 demean_yz = demean_[1] * demean_[2];
86 covariance_matrix_(0, 0) += demean_[0] * demean_[0];
87 covariance_matrix_(0, 1) +=
static_cast<float> (demean_xy);
88 covariance_matrix_(0, 2) +=
static_cast<float> (demean_xz);
90 covariance_matrix_(1, 0) +=
static_cast<float> (demean_xy);
91 covariance_matrix_(1, 1) += demean_[1] * demean_[1];
92 covariance_matrix_(1, 2) +=
static_cast<float> (demean_yz);
94 covariance_matrix_(2, 0) +=
static_cast<float> (demean_xz);
95 covariance_matrix_(2, 1) +=
static_cast<float> (demean_yz);
96 covariance_matrix_(2, 2) += demean_[2] * demean_[2];
103 pcx = eigenvector_ [0];
104 pcy = eigenvector_ [1];
105 pcz = eigenvector_ [2];
106 float indices_size = 1.0f /
static_cast<float> (indices.size ());
107 pc1 = eigenvalues_ [2] * indices_size;
108 pc2 = eigenvalues_ [1] * indices_size;
113 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
118 std::vector<int> nn_indices (k_);
119 std::vector<float> nn_dists (k_);
123 if (input_->is_dense)
126 for (
size_t idx = 0; idx < indices_->size (); ++idx)
128 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
130 output.
points[idx].principal_curvature[0] = output.
points[idx].principal_curvature[1] = output.
points[idx].principal_curvature[2] =
131 output.
points[idx].pc1 = output.
points[idx].pc2 = std::numeric_limits<float>::quiet_NaN ();
137 computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
138 output.
points[idx].principal_curvature[0], output.
points[idx].principal_curvature[1], output.
points[idx].principal_curvature[2],
145 for (
size_t idx = 0; idx < indices_->size (); ++idx)
147 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
148 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
150 output.
points[idx].principal_curvature[0] = output.
points[idx].principal_curvature[1] = output.
points[idx].principal_curvature[2] =
151 output.
points[idx].pc1 = output.
points[idx].pc2 = std::numeric_limits<float>::quiet_NaN ();
157 computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
158 output.
points[idx].principal_curvature[0], output.
points[idx].principal_curvature[1], output.
points[idx].principal_curvature[2],
164 #define PCL_INSTANTIATE_PrincipalCurvaturesEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PrincipalCurvaturesEstimation<T,NT,OutT>;
166 #endif // PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_