40 #ifndef PCL_ROPS_ESTIMATION_HPP_ 41 #define PCL_ROPS_ESTIMATION_HPP_ 43 #include <pcl/features/rops_estimation.h> 46 template <
typename Po
intInT,
typename Po
intOutT>
49 number_of_rotations_ (3),
50 support_radius_ (1.0f),
51 sqr_support_radius_ (1.0f),
54 triangles_of_the_point_ (0)
59 template <
typename Po
intInT,
typename Po
intOutT>
63 triangles_of_the_point_.clear ();
67 template <
typename Po
intInT,
typename Po
intOutT>
void 70 if (number_of_bins != 0)
71 number_of_bins_ = number_of_bins;
75 template <
typename Po
intInT,
typename Po
intOutT>
unsigned int 78 return (number_of_bins_);
82 template <
typename Po
intInT,
typename Po
intOutT>
void 85 if (number_of_rotations != 0)
87 number_of_rotations_ = number_of_rotations;
88 step_ = 90.0f / static_cast <
float> (number_of_rotations_ + 1);
93 template <
typename Po
intInT,
typename Po
intOutT>
unsigned int 96 return (number_of_rotations_);
100 template <
typename Po
intInT,
typename Po
intOutT>
void 103 if (support_radius > 0.0f)
105 support_radius_ = support_radius;
106 sqr_support_radius_ = support_radius * support_radius;
111 template <
typename Po
intInT,
typename Po
intOutT>
float 114 return (support_radius_);
118 template <
typename Po
intInT,
typename Po
intOutT>
void 121 triangles_ = triangles;
125 template <
typename Po
intInT,
typename Po
intOutT>
void 128 triangles = triangles_;
132 template <
typename Po
intInT,
typename Po
intOutT>
void 135 if (triangles_.size () == 0)
137 output.points.clear ();
141 buildListOfPointsTriangles ();
144 unsigned int feature_size = number_of_rotations_ * 3 * 3 * 5;
145 unsigned int number_of_points = static_cast <
unsigned int> (indices_->size ());
146 output.points.resize (number_of_points, PointOutT ());
148 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
150 std::set <unsigned int> local_triangles;
151 std::vector <int> local_points;
152 getLocalSurface (input_->points[(*indices_)[i_point]], local_triangles, local_points);
154 Eigen::Matrix3f lrf_matrix;
155 computeLRF (input_->points[(*indices_)[i_point]], local_triangles, lrf_matrix);
158 transformCloud (input_->points[(*indices_)[i_point]], lrf_matrix, local_points, transformed_cloud);
161 axis[0].x = 1.0f; axis[0].y = 0.0f; axis[0].z = 0.0f;
162 axis[1].x = 0.0f; axis[1].y = 1.0f; axis[1].z = 0.0f;
163 axis[2].x = 0.0f; axis[2].y = 0.0f; axis[2].z = 1.0f;
164 std::vector <float> feature;
165 for (
unsigned int i_axis = 0; i_axis < 3; i_axis++)
172 Eigen::Vector3f min, max;
173 rotateCloud (axis[i_axis], theta, transformed_cloud, rotated_cloud, min, max);
176 for (
unsigned int i_proj = 0; i_proj < 3; i_proj++)
178 Eigen::MatrixXf distribution_matrix;
179 distribution_matrix.resize (number_of_bins_, number_of_bins_);
180 getDistributionMatrix (i_proj, min, max, rotated_cloud, distribution_matrix);
182 std::vector <float> moments;
183 computeCentralMoments (distribution_matrix, moments);
185 feature.insert (feature.end (), moments.begin (), moments.end ());
189 }
while (theta < 90.0f);
193 for (
unsigned int i_dim = 0; i_dim < feature_size; i_dim++)
194 norm += feature[i_dim];
195 if (abs (norm) < std::numeric_limits <float>::epsilon ())
200 for (
unsigned int i_dim = 0; i_dim < feature_size; i_dim++)
201 output.points[i_point].histogram[i_dim] = feature[i_dim] * norm;
206 template <
typename Po
intInT,
typename Po
intOutT>
void 209 triangles_of_the_point_.clear ();
211 const unsigned int number_of_triangles = static_cast <
unsigned int> (triangles_.size ());
213 std::vector <unsigned int> dummy;
215 triangles_of_the_point_.resize (surface_->points. size (), dummy);
217 for (
unsigned int i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
218 for (
unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
219 triangles_of_the_point_[triangles_[i_triangle].vertices[i_vertex]].push_back (i_triangle);
223 template <
typename Po
intInT,
typename Po
intOutT>
void 226 std::vector <float> distances;
227 tree_->radiusSearch (point, support_radius_, local_points, distances);
229 const unsigned int number_of_indices = static_cast <
unsigned int> (local_points.size ());
230 for (
unsigned int i = 0; i < number_of_indices; i++)
231 local_triangles.insert (triangles_of_the_point_[local_points[i]].begin (), triangles_of_the_point_[local_points[i]].end ());
235 template <
typename Po
intInT,
typename Po
intOutT>
void 238 const unsigned int number_of_triangles = static_cast <
unsigned int> (local_triangles.size ());
240 std::vector <Eigen::Matrix3f> scatter_matrices (number_of_triangles);
241 std::vector <float> triangle_area (number_of_triangles);
242 std::vector <float> distance_weight (number_of_triangles);
244 float total_area = 0.0f;
245 const float coeff = 1.0f / 12.0f;
246 const float coeff_1_div_3 = 1.0f / 3.0f;
248 Eigen::Vector3f feature_point (point.x, point.y, point.z);
250 std::set <unsigned int>::const_iterator it;
251 unsigned int i_triangle = 0;
252 for (it = local_triangles.begin (), i_triangle = 0; it != local_triangles.end (); it++, i_triangle++)
254 Eigen::Vector3f pt[3];
255 for (
unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
257 const unsigned int index = triangles_[*it].vertices[i_vertex];
258 pt[i_vertex] (0) = surface_->points[index].x;
259 pt[i_vertex] (1) = surface_->points[index].y;
260 pt[i_vertex] (2) = surface_->points[index].z;
263 const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm ();
264 triangle_area[i_triangle] = curr_area;
265 total_area += curr_area;
267 distance_weight[i_triangle] = pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f);
269 Eigen::Matrix3f curr_scatter_matrix;
270 curr_scatter_matrix.setZero ();
271 for (
unsigned int i_pt = 0; i_pt < 3; i_pt++)
273 Eigen::Vector3f vec = pt[i_pt] - feature_point;
274 curr_scatter_matrix += vec * (vec.transpose ());
275 for (
unsigned int j_pt = 0; j_pt < 3; j_pt++)
276 curr_scatter_matrix += vec * ((pt[j_pt] - feature_point).transpose ());
278 scatter_matrices[i_triangle] = coeff * curr_scatter_matrix;
281 if (abs (total_area) < std::numeric_limits <float>::epsilon ())
282 total_area = 1.0f / total_area;
286 Eigen::Matrix3f overall_scatter_matrix;
287 overall_scatter_matrix.setZero ();
288 std::vector<float> total_weight (number_of_triangles);
289 const float denominator = 1.0f / 6.0f;
290 for (
unsigned int i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
292 float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area;
293 overall_scatter_matrix += factor * scatter_matrices[i_triangle];
294 total_weight[i_triangle] = factor * denominator;
297 Eigen::Vector3f v1, v2, v3;
298 computeEigenVectors (overall_scatter_matrix, v1, v2, v3);
302 for (it = local_triangles.begin (), i_triangle = 0; it != local_triangles.end (); it++, i_triangle++)
304 Eigen::Vector3f pt[3];
305 for (
unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
307 const unsigned int index = triangles_[*it].vertices[i_vertex];
308 pt[i_vertex] (0) = surface_->points[index].x;
309 pt[i_vertex] (1) = surface_->points[index].y;
310 pt[i_vertex] (2) = surface_->points[index].z;
313 float factor1 = 0.0f;
314 float factor3 = 0.0f;
315 for (
unsigned int i_pt = 0; i_pt < 3; i_pt++)
317 Eigen::Vector3f vec = pt[i_pt] - feature_point;
318 factor1 += vec.dot (v1);
319 factor3 += vec.dot (v3);
321 h1 += total_weight[i_triangle] * factor1;
322 h3 += total_weight[i_triangle] * factor3;
325 if (h1 < 0.0f) v1 = -v1;
326 if (h3 < 0.0f) v3 = -v3;
330 lrf_matrix.row (0) = v1;
331 lrf_matrix.row (1) = v2;
332 lrf_matrix.row (2) = v3;
336 template <
typename Po
intInT,
typename Po
intOutT>
void 338 Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis)
const 340 Eigen::EigenSolver <Eigen::Matrix3f> eigen_solver;
343 Eigen::EigenSolver <Eigen::Matrix3f>::EigenvectorsType eigen_vectors;
344 Eigen::EigenSolver <Eigen::Matrix3f>::EigenvalueType eigen_values;
345 eigen_vectors = eigen_solver.eigenvectors ();
346 eigen_values = eigen_solver.eigenvalues ();
348 unsigned int temp = 0;
349 unsigned int major_index = 0;
350 unsigned int middle_index = 1;
351 unsigned int minor_index = 2;
353 if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
356 major_index = middle_index;
360 if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
363 major_index = minor_index;
367 if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
370 minor_index = middle_index;
374 major_axis = eigen_vectors.col (major_index).real ();
375 middle_axis = eigen_vectors.col (middle_index).real ();
376 minor_axis = eigen_vectors.col (minor_index).real ();
380 template <
typename Po
intInT,
typename Po
intOutT>
void 383 const unsigned int number_of_points = static_cast <
unsigned int> (local_points.size ());
384 transformed_cloud.points.resize (number_of_points, PointInT ());
386 for (
unsigned int i = 0; i < number_of_points; i++)
388 Eigen::Vector3f transformed_point (
389 surface_->points[local_points[i]].x - point.x,
390 surface_->points[local_points[i]].y - point.y,
391 surface_->points[local_points[i]].z - point.z);
393 transformed_point = matrix * transformed_point;
396 new_point.x = transformed_point (0);
397 new_point.y = transformed_point (1);
398 new_point.z = transformed_point (2);
399 transformed_cloud.points[i] = new_point;
404 template <
typename Po
intInT,
typename Po
intOutT>
void 407 Eigen::Matrix3f rotation_matrix;
408 const float x = axis.x;
409 const float y = axis.y;
410 const float z = axis.z;
411 const float rad = M_PI / 180.0f;
412 const float cosine = cos (angle * rad);
413 const float sine = sin (angle * rad);
414 rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y,
415 (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x,
416 (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z;
418 const unsigned int number_of_points = static_cast <
unsigned int> (cloud.points.size ());
420 rotated_cloud.header = cloud.header;
421 rotated_cloud.width = number_of_points;
422 rotated_cloud.height = 1;
423 rotated_cloud.points.resize (number_of_points, PointInT ());
425 min (0) = std::numeric_limits <float>::max ();
426 min (1) = std::numeric_limits <float>::max ();
427 min (2) = std::numeric_limits <float>::max ();
428 max (0) = -std::numeric_limits <float>::max ();
429 max (1) = -std::numeric_limits <float>::max ();
430 max (2) = -std::numeric_limits <float>::max ();
432 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
434 Eigen::Vector3f point (
435 cloud.points[i_point].x,
436 cloud.points[i_point].y,
437 cloud.points[i_point].z);
439 point = rotation_matrix * point;
440 PointInT rotated_point;
441 rotated_point.x = point (0);
442 rotated_point.y = point (1);
443 rotated_point.z = point (2);
444 rotated_cloud.points[i_point] = rotated_point;
446 if (min (0) > rotated_point.x) min (0) = rotated_point.x;
447 if (min (1) > rotated_point.y) min (1) = rotated_point.y;
448 if (min (2) > rotated_point.z) min (2) = rotated_point.z;
450 if (max (0) < rotated_point.x) max (0) = rotated_point.x;
451 if (max (1) < rotated_point.y) max (1) = rotated_point.y;
452 if (max (2) < rotated_point.z) max (2) = rotated_point.z;
457 template <
typename Po
intInT,
typename Po
intOutT>
void 462 const unsigned int number_of_points = static_cast <
unsigned int> (cloud.points.size ());
464 const unsigned int coord[3][2] = {
469 const float u_bin_length = (max (coord[projection][0]) - min (coord[projection][0])) / number_of_bins_;
470 const float v_bin_length = (max (coord[projection][1]) - min (coord[projection][1])) / number_of_bins_;
472 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
474 Eigen::Vector3f point (
475 cloud.points[i_point].x,
476 cloud.points[i_point].y,
477 cloud.points[i_point].z);
479 const float u_length = point (coord[projection][0]) - min[coord[projection][0]];
480 const float v_length = point (coord[projection][1]) - min[coord[projection][1]];
482 const float u_ratio = u_length / u_bin_length;
483 unsigned int row = static_cast <
unsigned int> (u_ratio);
484 if (row == number_of_bins_) row--;
486 const float v_ratio = v_length / v_bin_length;
487 unsigned int col = static_cast <
unsigned int> (v_ratio);
488 if (col == number_of_bins_) col--;
490 matrix (row, col) += 1.0f;
493 matrix /= static_cast <
float> (number_of_points);
497 template <
typename Po
intInT,
typename Po
intOutT>
void 503 for (
unsigned int i = 0; i < number_of_bins_; i++)
504 for (
unsigned int j = 0; j < number_of_bins_; j++)
506 const float m = matrix (i, j);
507 mean_i += static_cast <
float> (i + 1) * m;
508 mean_j += static_cast <
float> (j + 1) * m;
511 const unsigned int number_of_moments_to_compute = 4;
512 const float power[number_of_moments_to_compute][2] = {
518 float entropy = 0.0f;
519 moments.resize (number_of_moments_to_compute + 1, 0.0f);
520 for (
unsigned int i = 0; i < number_of_bins_; i++)
522 const float i_factor = static_cast <
float> (i + 1) - mean_i;
523 for (
unsigned int j = 0; j < number_of_bins_; j++)
525 const float j_factor = static_cast <
float> (j + 1) - mean_j;
526 const float m = matrix (i, j);
528 entropy -= m * log (m);
529 for (
unsigned int i_moment = 0; i_moment < number_of_moments_to_compute; i_moment++)
530 moments[i_moment] += pow (i_factor, power[i_moment][0]) * pow (j_factor, power[i_moment][1]) * m;
534 moments[number_of_moments_to_compute] = entropy;
537 #endif // PCL_ROPS_ESTIMATION_HPP_ This class implements the method for extracting RoPS features presented in the article "Rotational Pr...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...