43 #include <pcl/features/principal_curvatures.h>
45 #include <pcl/common/point_tests.h>
49 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
52 float &pcx,
float &pcy,
float &pcz,
float &pc1,
float &pc2)
54 EIGEN_ALIGN16 Eigen::Matrix3f I = Eigen::Matrix3f::Identity ();
55 Eigen::Vector3f n_idx (normals[p_idx].normal[0], normals[p_idx].normal[1], normals[p_idx].normal[2]);
56 EIGEN_ALIGN16 Eigen::Matrix3f M = I - n_idx * n_idx.transpose ();
59 Eigen::Vector3f normal;
60 projected_normals_.resize (indices.size ());
61 xyz_centroid_.setZero ();
62 for (std::size_t idx = 0; idx < indices.size(); ++idx)
64 normal[0] = normals[indices[idx]].normal[0];
65 normal[1] = normals[indices[idx]].normal[1];
66 normal[2] = normals[indices[idx]].normal[2];
68 projected_normals_[idx] = M * normal;
69 xyz_centroid_ += projected_normals_[idx];
73 xyz_centroid_ /=
static_cast<float> (indices.size ());
76 covariance_matrix_.setZero ();
79 for (std::size_t idx = 0; idx < indices.size (); ++idx)
81 demean_ = projected_normals_[idx] - xyz_centroid_;
83 double demean_xy = demean_[0] * demean_[1];
84 double demean_xz = demean_[0] * demean_[2];
85 double demean_yz = demean_[1] * demean_[2];
87 covariance_matrix_(0, 0) += demean_[0] * demean_[0];
88 covariance_matrix_(0, 1) +=
static_cast<float> (demean_xy);
89 covariance_matrix_(0, 2) +=
static_cast<float> (demean_xz);
91 covariance_matrix_(1, 0) +=
static_cast<float> (demean_xy);
92 covariance_matrix_(1, 1) += demean_[1] * demean_[1];
93 covariance_matrix_(1, 2) +=
static_cast<float> (demean_yz);
95 covariance_matrix_(2, 0) +=
static_cast<float> (demean_xz);
96 covariance_matrix_(2, 1) +=
static_cast<float> (demean_yz);
97 covariance_matrix_(2, 2) += demean_[2] * demean_[2];
104 pcx = eigenvector_ [0];
105 pcy = eigenvector_ [1];
106 pcz = eigenvector_ [2];
107 float indices_size = 1.0f /
static_cast<float> (indices.size ());
108 pc1 = eigenvalues_ [2] * indices_size;
109 pc2 = eigenvalues_ [1] * indices_size;
114 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
120 std::vector<float> nn_dists (k_);
122 output.is_dense =
true;
124 if (input_->is_dense)
127 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
129 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
131 output[idx].principal_curvature[0] = output[idx].principal_curvature[1] = output[idx].principal_curvature[2] =
132 output[idx].pc1 = output[idx].pc2 = std::numeric_limits<float>::quiet_NaN ();
133 output.is_dense =
false;
138 computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
139 output[idx].principal_curvature[0], output[idx].principal_curvature[1], output[idx].principal_curvature[2],
140 output[idx].pc1, output[idx].pc2);
146 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
148 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
149 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
151 output[idx].principal_curvature[0] = output[idx].principal_curvature[1] = output[idx].principal_curvature[2] =
152 output[idx].pc1 = output[idx].pc2 = std::numeric_limits<float>::quiet_NaN ();
153 output.is_dense =
false;
158 computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
159 output[idx].principal_curvature[0], output[idx].principal_curvature[1], output[idx].principal_curvature[2],
160 output[idx].pc1, output[idx].pc2);
165 #define PCL_INSTANTIATE_PrincipalCurvaturesEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PrincipalCurvaturesEstimation<T,NT,OutT>;