| 280 | |
| 281 | template <class Map> |
| 282 | void cluster(Map& map, Clustering const& clustering) |
| 283 | { |
| 284 | std::unordered_set<ufo::Node> seen; |
| 285 | std::vector<ufo::Sphere> queue; |
| 286 | |
| 287 | auto depth = clustering.depth; |
| 288 | auto max_distance = clustering.max_distance; |
| 289 | auto min_points = clustering.min_points; |
| 290 | |
| 291 | ufo::label_t l{1}; |
| 292 | for (auto node : map.query(ufo::pred::Leaf(depth) && ufo::pred::SeenFree() && |
| 293 | ufo::pred::HitsMin(1) && ufo::pred::Label(0))) { |
| 294 | if (map.label(node.index())) { // FIXME: This is because how the iterator works |
| 295 | continue; |
| 296 | } |
| 297 | |
| 298 | seen = {node}; |
| 299 | queue.assign(1, ufo::Sphere(map.center(node), max_distance)); |
| 300 | |
| 301 | map.setLabel(node, l, false); |
| 302 | |
| 303 | while (!queue.empty()) { |
| 304 | auto p = ufo::pred::Intersects(queue); |
| 305 | queue.clear(); |
| 306 | for (auto const& node : map.query( |
| 307 | ufo::pred::Leaf(depth) && ufo::pred::SeenFree() && ufo::pred::HitsMin(1) && |
| 308 | ufo::pred::Label(0) && std::move(p) && |
| 309 | ufo::pred::Satisfies([&seen](auto n) { return seen.insert(n).second; }))) { |
| 310 | queue.emplace_back(map.center(node), max_distance); |
| 311 | map.setLabel(node, l, false); |
| 312 | } |
| 313 | } |
| 314 | |
| 315 | if (seen.size() < min_points) { |
| 316 | for (auto e : seen) { |
| 317 | if (l == map.label(e)) { |
| 318 | map.setLabel(e, -1, false); |
| 319 | } |
| 320 | } |
| 321 | } |
| 322 | |
| 323 | l += seen.size() >= min_points; |
| 324 | |
| 325 | map.propagateModified(); // FIXME: Should this be here? |
| 326 | } |
| 327 | } |
| 328 | |
| 329 | template <class PointCloud> |
| 330 | void filterminDistance(PointCloud& cloud, ufo::Point origin, float min_distance) |