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);
157 PointCloudIn transformed_cloud;
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++)
171 PointCloudIn rotated_cloud;
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 += std::abs (feature[i_dim]);
195 if (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, Eigen::aligned_allocator<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 (std::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_