41 #ifndef PCL_FEATURES_IMPL_MOMENT_INVARIANTS_H_ 42 #define PCL_FEATURES_IMPL_MOMENT_INVARIANTS_H_ 44 #include <pcl/features/moment_invariants.h> 45 #include <pcl/common/centroid.h> 48 template <
typename Po
intInT,
typename Po
intOutT>
void 51 float &j1,
float &j2,
float &j3)
57 float mu200 = 0, mu020 = 0, mu002 = 0, mu110 = 0, mu101 = 0, mu011 = 0;
60 for (
size_t nn_idx = 0; nn_idx < indices.size (); ++nn_idx)
63 temp_pt_[0] = cloud.
points[indices[nn_idx]].x - xyz_centroid_[0];
64 temp_pt_[1] = cloud.
points[indices[nn_idx]].y - xyz_centroid_[1];
65 temp_pt_[2] = cloud.
points[indices[nn_idx]].z - xyz_centroid_[2];
67 mu200 += temp_pt_[0] * temp_pt_[0];
68 mu020 += temp_pt_[1] * temp_pt_[1];
69 mu002 += temp_pt_[2] * temp_pt_[2];
70 mu110 += temp_pt_[0] * temp_pt_[1];
71 mu101 += temp_pt_[0] * temp_pt_[2];
72 mu011 += temp_pt_[1] * temp_pt_[2];
76 j1 = mu200 + mu020 + mu002;
77 j2 = mu200*mu020 + mu200*mu002 + mu020*mu002 - mu110*mu110 - mu101*mu101 - mu011*mu011;
78 j3 = mu200*mu020*mu002 + 2*mu110*mu101*mu011 - mu002*mu110*mu110 - mu020*mu101*mu101 - mu200*mu011*mu011;
82 template <
typename Po
intInT,
typename Po
intOutT>
void 90 float mu200 = 0, mu020 = 0, mu002 = 0, mu110 = 0, mu101 = 0, mu011 = 0;
93 for (
size_t nn_idx = 0; nn_idx < cloud.
points.size (); ++nn_idx )
96 temp_pt_[0] = cloud.
points[nn_idx].x - xyz_centroid_[0];
97 temp_pt_[1] = cloud.
points[nn_idx].y - xyz_centroid_[1];
98 temp_pt_[2] = cloud.
points[nn_idx].z - xyz_centroid_[2];
100 mu200 += temp_pt_[0] * temp_pt_[0];
101 mu020 += temp_pt_[1] * temp_pt_[1];
102 mu002 += temp_pt_[2] * temp_pt_[2];
103 mu110 += temp_pt_[0] * temp_pt_[1];
104 mu101 += temp_pt_[0] * temp_pt_[2];
105 mu011 += temp_pt_[1] * temp_pt_[2];
109 j1 = mu200 + mu020 + mu002;
110 j2 = mu200*mu020 + mu200*mu002 + mu020*mu002 - mu110*mu110 - mu101*mu101 - mu011*mu011;
111 j3 = mu200*mu020*mu002 + 2*mu110*mu101*mu011 - mu002*mu110*mu110 - mu020*mu101*mu101 - mu200*mu011*mu011;
115 template <
typename Po
intInT,
typename Po
intOutT>
void 120 std::vector<int> nn_indices (k_);
121 std::vector<float> nn_dists (k_);
125 if (input_->is_dense)
128 for (
size_t idx = 0; idx < indices_->size (); ++idx)
130 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
132 output.
points[idx].j1 = output.
points[idx].j2 = output.
points[idx].j3 = std::numeric_limits<float>::quiet_NaN ();
137 computePointMomentInvariants (*surface_, nn_indices,
144 for (
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].j1 = output.
points[idx].j2 = output.
points[idx].j3 = std::numeric_limits<float>::quiet_NaN ();
154 computePointMomentInvariants (*surface_, nn_indices,
160 #define PCL_INSTANTIATE_MomentInvariantsEstimation(T,NT) template class PCL_EXPORTS pcl::MomentInvariantsEstimation<T,NT>; 162 #endif // PCL_FEATURES_IMPL_MOMENT_INVARIANTS_H_ bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void computePointMomentInvariants(const pcl::PointCloud< PointInT > &cloud, const std::vector< int > &indices, float &j1, float &j2, float &j3)
Compute the 3 moment invariants (j1, j2, j3) for a given set of points, using their indices...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void computeFeature(PointCloudOut &output)
Estimate moment invariants for all points given in <setInputCloud (), setIndices ()> using the surfac...
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.