| 75 | } |
| 76 | |
| 77 | void |
| 78 | compute (const Cloud::ConstPtr &cloud_a, const Cloud::ConstPtr &cloud_b) |
| 79 | { |
| 80 | // Estimate |
| 81 | TicToc tt; |
| 82 | tt.tic (); |
| 83 | |
| 84 | print_highlight (stderr, "Computing "); |
| 85 | |
| 86 | // compare A to B |
| 87 | pcl::search::KdTree<PointType> tree_b; |
| 88 | tree_b.setInputCloud (cloud_b); |
| 89 | float max_dist_a = -std::numeric_limits<float>::max (); |
| 90 | for (const auto &point : (*cloud_a)) |
| 91 | { |
| 92 | pcl::Indices indices (1); |
| 93 | std::vector<float> sqr_distances (1); |
| 94 | |
| 95 | tree_b.nearestKSearch (point, 1, indices, sqr_distances); |
| 96 | if (sqr_distances[0] > max_dist_a) |
| 97 | max_dist_a = sqr_distances[0]; |
| 98 | } |
| 99 | |
| 100 | // compare B to A |
| 101 | pcl::search::KdTree<PointType> tree_a; |
| 102 | tree_a.setInputCloud (cloud_a); |
| 103 | float max_dist_b = -std::numeric_limits<float>::max (); |
| 104 | for (const auto &point : (*cloud_b)) |
| 105 | { |
| 106 | pcl::Indices indices (1); |
| 107 | std::vector<float> sqr_distances (1); |
| 108 | |
| 109 | tree_a.nearestKSearch (point, 1, indices, sqr_distances); |
| 110 | if (sqr_distances[0] > max_dist_b) |
| 111 | max_dist_b = sqr_distances[0]; |
| 112 | } |
| 113 | |
| 114 | max_dist_a = std::sqrt (max_dist_a); |
| 115 | max_dist_b = std::sqrt (max_dist_b); |
| 116 | |
| 117 | float dist = std::max (max_dist_a, max_dist_b); |
| 118 | |
| 119 | print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); |
| 120 | print_info ("A->B: "); print_value ("%f", max_dist_a); |
| 121 | print_info (", B->A: "); print_value ("%f", max_dist_b); |
| 122 | print_info (", Hausdorff Distance: "); print_value ("%f", dist); |
| 123 | print_info (" ]\n"); |
| 124 | } |
| 125 | |
| 126 | /* ---[ */ |
| 127 | int |
no test coverage detected