From 3730c87e47fd27a59d7354555cfdcc8be640a2c0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 20 Oct 2025 18:44:47 +0200 Subject: [PATCH 1/7] Initial working version MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_ros/src/navmap_ros/conversions.cpp | 887 +++++++++++++++++----- 1 file changed, 687 insertions(+), 200 deletions(-) diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index cc333b2..f1997e0 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -19,6 +19,10 @@ #include #include #include +#include +#include +#include +#include #include "geometry_msgs/msg/pose.hpp" #include @@ -505,7 +509,7 @@ bool build_navmap_from_mesh( out_msg.navcels_v2.push_back(static_cast(i2)); } -// 3) One surface listing all triangles (required for raycasting/BVH) + // 3) One surface listing all triangles (required for raycasting/BVH) { navmap_ros_interfaces::msg::NavMapSurface s; s.frame_id = frame_id; @@ -546,7 +550,6 @@ bool build_navmap_from_mesh( // ----------------- Surface from PC2 ----------------- - using Triangle = Eigen::Vector3i; static inline float sqr(float v) {return v * v;} @@ -583,7 +586,7 @@ static inline float tri_slope_deg( return theta * 180.0f / static_cast(M_PI); } -// Keys to avoid duplicates +// Keys to avoid duplicates (edges/triangles) struct EdgeKey { int a, b; @@ -734,6 +737,7 @@ inline void sort_neighbors_angular( } // Attempt to add triangle (i,j,k) under the given constraints +// (NOW includes max_dz and neighbor_radius constraints) inline bool try_add_triangle( int i, int j, int k, const pcl::PointCloud & cloud, @@ -750,18 +754,38 @@ inline bool try_add_triangle( const auto & C = cloud[k]; if (!pcl::isFinite(A) || !pcl::isFinite(B) || !pcl::isFinite(C)) {return false;} - // Max edge length + // Max edge length (3D) auto dAB = dist3(A, B); auto dBC = dist3(B, C); auto dCA = dist3(C, A); - if (dAB > P.max_edge_len || dBC > P.max_edge_len || dCA > P.max_edge_len) {return false;} + if (P.max_edge_len > 0.0f && + (dAB > P.max_edge_len || dBC > P.max_edge_len || dCA > P.max_edge_len)) {return false;} + + // Max |Δz| per edge (avoid bridges between floors) + if (P.max_dz > 0.0f) { + const float dzAB = std::fabs(A.z - B.z); + const float dzBC = std::fabs(B.z - C.z); + const float dzCA = std::fabs(C.z - A.z); + if (std::max({dzAB, dzBC, dzCA}) > P.max_dz) {return false;} + } + + // Neighbor radius (XY) as lateral constraint + if (P.neighbor_radius > 0.0f) { + auto distXY = [](const pcl::PointXYZ & Pp, const pcl::PointXYZ & Qq) { + const float dx = Pp.x - Qq.x, dy = Pp.y - Qq.y; + return std::sqrt(dx*dx + dy*dy); + }; + if (distXY(A,B) > P.neighbor_radius || + distXY(B,C) > P.neighbor_radius || + distXY(C,A) > P.neighbor_radius) {return false;} + } Eigen::Vector3f a(A.x, A.y, A.z); Eigen::Vector3f b(B.x, B.y, B.z); Eigen::Vector3f c(C.x, C.y, C.z); // Min area - if (tri_area(a, b, c) < P.min_area) {return false;} + if (tri_area(a, b, c) < std::max(P.min_area, 1e-9f)) {return false;} // Max slope float slope = tri_slope_deg(a, b, c); @@ -774,6 +798,7 @@ inline bool try_add_triangle( return false; } + // Orient consistently (up-facing) { const Eigen::Vector3f up = Eigen::Vector3f::UnitZ(); Eigen::Vector3f n = (b - a).cross(c - a); @@ -792,6 +817,8 @@ inline bool try_add_triangle( return true; } +// ----------------- Downsampling ----------------- + struct Voxel { int x, y, z; @@ -919,7 +946,7 @@ downsample_voxelize_avgXYZ( int best_idx = -1; float best_d2 = std::numeric_limits::max(); - // Search 27 neighboring cells for a close cluster + // Search 27 neighboring cells for a close cluster for (int dz = -1; dz <= 1; ++dz) { for (int dy = -1; dy <= 1; ++dy) { for (int dx = -1; dx <= 1; ++dx) { @@ -940,13 +967,13 @@ downsample_voxelize_avgXYZ( } if (best_idx >= 0 && best_d2 <= merge_radius2) { - // Merge into existing cluster + // Merge into existing cluster clusters[best_idx].sum_x += p.x; clusters[best_idx].sum_y += p.y; clusters[best_idx].sum_z += p.z; clusters[best_idx].count += 1; } else { - // Create new cluster + // Create new cluster VoxelAccum acc; acc.sum_x = p.x; acc.sum_y = p.y; acc.sum_z = p.z; acc.count = 1; int new_idx = static_cast(clusters.size()); @@ -975,6 +1002,7 @@ downsample_voxelize_avgXYZ( return output; } +// Downsampling que NO colapsa plantas: un punto por voxel XY con Z media pcl::PointCloud downsample_voxelize_avgZ( const pcl::PointCloud & input_points, @@ -1010,7 +1038,7 @@ downsample_voxelize_avgZ( acc.count += 1; } - // Emit one point per voxel + // Emit one point per voxel (centro XY del voxel y Z media) output.points.reserve(voxels.size()); for (const auto & kv : voxels) { const Voxel & v = kv.first; @@ -1032,6 +1060,8 @@ downsample_voxelize_avgZ( return output; } +// ----------------- Crecimiento local desde semilla ----------------- + std::vector grow_surface_from_seed( const pcl::PointCloud & cloud, int seed_idx, @@ -1079,7 +1109,7 @@ std::vector grow_surface_from_seed( if (idx == v) {continue;} const auto & Pn = cloud[idx]; if (!pcl::isFinite(Pn)) {continue;} - if (dist3(V, Pn) <= P.max_edge_len) { + if (P.max_edge_len <= 0.0f || dist3(V, Pn) <= P.max_edge_len) { candidates.emplace_back(idx, Eigen::Vector3f(Pn.x, Pn.y, Pn.z)); } } @@ -1091,7 +1121,6 @@ std::vector grow_surface_from_seed( sort_neighbors_angular(vpos, candidates, ordered); // 4) Try fan triangles from consecutive pairs - // Optionally close the fan by connecting last with first. for (size_t t = 0; t + 1 < ordered.size(); ++t) { int i = v; int j = ordered[t]; @@ -1103,259 +1132,713 @@ std::vector grow_surface_from_seed( } } // Optional fan closure - int j0 = ordered.front(), k0 = ordered.back(); - try_add_triangle(v, k0, j0, cloud, P, tri_set, edge_set, tris); + if (ordered.size() >= 2) { + int j0 = ordered.front(), k0 = ordered.back(); + try_add_triangle(v, k0, j0, cloud, P, tri_set, edge_set, tris); + } } return tris; } +// ----------------- Banded Delaunay (gap-based) ----------------- + +// Separa la nube en "bandas" siguiendo saltos en Z: +// - Ordena por Z y empieza nueva banda cuando (z_i - z_{i-1}) > gap_z. +// - Útil para plantas en edificios (saltos grandes) sin romper rampas suaves. +static std::vector> +split_points_by_z_gaps(const pcl::PointCloud & cloud, + float gap_z, + int min_points_per_band = 3) +{ + std::vector idx(cloud.size()); + std::iota(idx.begin(), idx.end(), 0); + std::sort(idx.begin(), idx.end(), + [&](int a, int b){ return cloud[a].z < cloud[b].z; }); + + std::vector> bands; + if (idx.empty()) return bands; + + std::vector cur; + cur.reserve(256); + cur.push_back(idx[0]); + + for (size_t t = 1; t < idx.size(); ++t) { + const int i_prev = idx[t-1]; + const int i_curr = idx[t]; + const float dz = cloud[i_curr].z - cloud[i_prev].z; + + if (dz > gap_z) { + if ((int)cur.size() >= min_points_per_band) bands.push_back(std::move(cur)); + cur.clear(); + } + cur.push_back(i_curr); + } + if ((int)cur.size() >= min_points_per_band) bands.push_back(std::move(cur)); + + return bands; +} + +// ---- from_points: Delaunay por banda + filtros 3D (slope, max_dz, neighbor_radius, etc.) navmap::NavMap from_points( const pcl::PointCloud & input_points, navmap_ros_interfaces::msg::NavMap & out_msg, BuildParams params) { - // --- Overview ------------------------------------------------------------ - // 1) Optional voxel downsampling in XY (averaging XYZ). - // 2) 2D Delaunay triangulation in the XY plane (no crossing edges by construction). - // 3) Lift triangles back to 3D and filter by: - // - area bounds, - // - maximum edge length, - // - maximum vertical discontinuity per edge (max_dz), - // - maximum slope w.r.t. +Z (max_slope_deg). - // 4) Emit a single-surface NavMap and mirror it into out_msg. - // ------------------------------------------------------------------------- - using std::vector; + out_msg = navmap_ros_interfaces::msg::NavMap(); if (input_points.empty() || input_points.size() < 3) { - out_msg = navmap_ros_interfaces::msg::NavMap(); return navmap::NavMap(); } + // 1) Downsample que NO colapsa plantas (promedia Z por voxel XY) pcl::PointCloud cloud; if (params.resolution > 0.0f) { - cloud = downsample_voxelize_avgXYZ(input_points, params.resolution); + cloud = downsample_voxelize_avgZ(input_points, params.resolution); } else { cloud = input_points; } if (cloud.size() < 3) { - out_msg = navmap_ros_interfaces::msg::NavMap(); return navmap::NavMap(); } + // 2) Bandas por SALTOS en Z + // Para parkings típicos (2.5–3.0 m entre plantas), gap_z=1.0 separa bien. + const float gap_z = (params.max_dz > 0.0f) + ? std::max(1.0f, 3.0f * params.max_dz) + : 1.0f; + auto bands = split_points_by_z_gaps(cloud, gap_z, /*min_points_per_band=*/3); + if (bands.empty()) { + return navmap::NavMap(); + } + + // 3) Delaunay 2D (XY) por banda + filtros 3D struct Pt2 { double x, y; int idx3d; }; struct Tri2 { int a, b, c; }; auto orient2d = [](const Pt2 & a, const Pt2 & b, const Pt2 & c) -> double { - return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x); - }; + return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x); + }; + auto in_circumcircle = [&](const Pt2 & p, const Pt2 & A, const Pt2 & B, const Pt2 & C) -> bool { + const double ax = A.x - p.x, ay = A.y - p.y; + const double bx = B.x - p.x, by = B.y - p.y; + const double cx = C.x - p.x, cy = C.y - p.y; + const double det = (ax*ax + ay*ay) * (bx*cy - by*cx) + - (bx*bx + by*by) * (ax*cy - ay*cx) + + (cx*cx + cy*cy) * (ax*by - ay*bx); + const double o = orient2d(A, B, C); + return (o > 0.0) ? (det > 0.0) : (det < 0.0); + }; - auto in_circumcircle = [&](const Pt2 & p, - const Pt2 & A, const Pt2 & B, const Pt2 & C) -> bool - { - const double ax = A.x - p.x, ay = A.y - p.y; - const double bx = B.x - p.x, by = B.y - p.y; - const double cx = C.x - p.x, cy = C.y - p.y; - const double det = (ax * ax + ay * ay) * (bx * cy - by * cx) - - (bx * bx + by * by) * (ax * cy - ay * cx) + - (cx * cx + cy * cy) * (ax * by - ay * bx); - const double o = orient2d(A, B, C); - return (o > 0.0) ? (det > 0.0) : (det < 0.0); - }; + vector all_triangles; + all_triangles.reserve(static_cast(cloud.size()) * 2); - vector pts2; pts2.reserve(cloud.size()); - for (int i = 0; i < static_cast(cloud.size()); ++i) { - pts2.push_back({static_cast(cloud[i].x), - static_cast(cloud[i].y), i}); - } - - double minx = 1e300, miny = 1e300, maxx = -1e300, maxy = -1e300; - for (const auto & p : pts2) { - if (p.x < minx) {minx = p.x;} - if (p.y < miny) {miny = p.y;} - if (p.x > maxx) {maxx = p.x;} - if (p.y > maxy) {maxy = p.y;} - } - const double dx = maxx - minx, dy = maxy - miny, d = std::max(dx, dy); - Pt2 S1{minx - 10 * d, miny - d, -1}; - Pt2 S2{minx + 0.5 * d, maxy + 10 * d, -2}; - Pt2 S3{maxx + 10 * d, miny - d, -3}; - - vector vs = pts2; - const int iS1 = static_cast(vs.size()); vs.push_back(S1); - const int iS2 = static_cast(vs.size()); vs.push_back(S2); - const int iS3 = static_cast(vs.size()); vs.push_back(S3); - - vector tris; - if (orient2d(vs[iS1], vs[iS2], vs[iS3]) <= 0.0) { - std::swap(vs[iS2], vs[iS3]); - } - tris.push_back({iS1, iS2, iS3}); - - // Incremental Bowyer–Watson - for (int pi = 0; pi < static_cast(pts2.size()); ++pi) { - const Pt2 p = vs[pi]; - - // 2.1) Collect "bad" triangles (circumcircle contains p) - vector bad; bad.reserve(tris.size()); - for (int ti = 0; ti < static_cast(tris.size()); ++ti) { - const auto & t = tris[ti]; - if (in_circumcircle(p, vs[t.a], vs[t.b], vs[t.c])) { - bad.push_back(ti); - } + vector> band_tri_ranges; // [offset, count] por banda + + const float cos_max_slope = + std::cos(params.max_slope_deg * static_cast(M_PI) / 180.0f); + const float min_area = std::max(params.min_area, 1e-9f); + const float max_edge = + (params.max_edge_len > 0.0f) ? params.max_edge_len + : std::numeric_limits::infinity(); + const float max_dz = + (params.max_dz > 0.0f) ? params.max_dz + : std::numeric_limits::infinity(); + const float neigh_xy = + (params.neighbor_radius > 0.0f) ? params.neighbor_radius + : std::numeric_limits::infinity(); + + auto edge_len3D = [&](int i, int j) -> float { + const auto & A = cloud[i]; + const auto & B = cloud[j]; + const float dx = A.x - B.x, dy = A.y - B.y, dz = A.z - B.z; + return std::sqrt(dx*dx + dy*dy + dz*dz); + }; + auto distXY = [&](const pcl::PointXYZ & P, const pcl::PointXYZ & Q) -> float { + const float dx = P.x - Q.x, dy = P.y - Q.y; + return std::sqrt(dx*dx + dy*dy); + }; + + for (const auto & band : bands) { + if (band.size() < 3) { + band_tri_ranges.emplace_back(all_triangles.size(), 0); + continue; } - // 2.2) Boundary of the polygonal cavity: edges touched exactly once - struct EdgeKey - { - int u, v; - bool operator==(const EdgeKey & o) const noexcept - { - return u == o.u && v == o.v; - } - }; - struct EdgeKeyHash - { - std::size_t operator()(const EdgeKey & e) const noexcept - { - return (static_cast(e.u) << 32) ^ static_cast(e.v); + // Puntos 2D de la banda + vector vs; vs.reserve(band.size() + 3); + double minx = 1e300, miny = 1e300, maxx = -1e300, maxy = -1e300; + for (int i3d : band) { + const auto & p = cloud[i3d]; + vs.push_back({static_cast(p.x), static_cast(p.y), i3d}); + minx = std::min(minx, (double)p.x); + miny = std::min(miny, (double)p.y); + maxx = std::max(maxx, (double)p.x); + maxy = std::max(maxy, (double)p.y); + } + + // Super-triángulo + const double dx = maxx - minx, dy = maxy - miny, d = std::max(dx, dy); + const int iS1 = (int)vs.size(); vs.push_back({minx - 10*d, miny - d, -1}); + const int iS2 = (int)vs.size(); vs.push_back({minx + 0.5*d, maxy + 10*d, -2}); + const int iS3 = (int)vs.size(); vs.push_back({maxx + 10*d, miny - d, -3}); + + vector tris2; tris2.push_back({iS1, iS2, iS3}); + + // Bowyer–Watson + for (int pi = 0; pi < (int)band.size(); ++pi) { + const Pt2 Pp = vs[pi]; + vector bad; bad.reserve(tris2.size()); + for (int t = 0; t < (int)tris2.size(); ++t) { + const auto & T = tris2[t]; + if (in_circumcircle(Pp, vs[T.a], vs[T.b], vs[T.c])) bad.push_back(t); } - }; - std::unordered_map edge_count; - edge_count.reserve(bad.size() * 3); + // borde de cavidad + struct EdgeBW { int a, b; }; + vector poly; poly.reserve(bad.size() * 3); + std::vector removed(tris2.size(), 0); + for (int bi : bad) removed[bi] = 1; - auto add_edge = [&](int u, int v) { - // canonicalize (min, max) so opposite directions collide - if (u > v) {std::swap(u, v);} - EdgeKey e{u, v}; - ++edge_count[e]; + auto add_edge = [&](int a, int b){ + for (auto it = poly.begin(); it != poly.end(); ++it) { + if (it->a == b && it->b == a) { poly.erase(it); return; } + } + poly.push_back({a, b}); }; - for (int ti : bad) { - const auto & t = tris[ti]; - add_edge(t.a, t.b); - add_edge(t.b, t.c); - add_edge(t.c, t.a); + for (int t = 0; t < (int)tris2.size(); ++t) { + if (!removed[t]) continue; + const auto & T = tris2[t]; + add_edge(T.a, T.b); + add_edge(T.b, T.c); + add_edge(T.c, T.a); + } + + vector keep; keep.reserve(tris2.size()); + for (int t = 0; t < (int)tris2.size(); ++t) { + if (!removed[t]) keep.push_back(tris2[t]); + } + tris2.swap(keep); + + for (const auto & e : poly) tris2.push_back({e.a, e.b, pi}); } - // 2.3) Remove bad triangles - vector kept; kept.reserve(tris.size()); - vector removed(tris.size(), 0); - for (int idx : bad) { - removed[idx] = 1; + // Triángulos finales (sin super-triángulo) + vector final_tris2; final_tris2.reserve(tris2.size()); + for (const auto & T : tris2) { + if (T.a >= iS1 && T.a <= iS3) continue; + if (T.b >= iS1 && T.b <= iS3) continue; + if (T.c >= iS1 && T.c <= iS3) continue; + final_tris2.push_back(T); } - for (int ti = 0; ti < static_cast(tris.size()); ++ti) { - if (!removed[ti]) {kept.push_back(tris[ti]);} + + // Filtros 3D + const size_t tri_off = all_triangles.size(); + size_t accepted = 0; + + for (const auto & T : final_tris2) { + const int ia = vs[T.a].idx3d; + const int ib = vs[T.b].idx3d; + const int ic = vs[T.c].idx3d; + + const Eigen::Vector3f A(cloud[ia].x, cloud[ia].y, cloud[ia].z); + const Eigen::Vector3f B(cloud[ib].x, cloud[ib].y, cloud[ib].z); + const Eigen::Vector3f C(cloud[ic].x, cloud[ic].y, cloud[ic].z); + + const float area = tri_area(A, B, C); + if (area < min_area) continue; + + const float lAB = edge_len3D(ia, ib); + const float lBC = edge_len3D(ib, ic); + const float lCA = edge_len3D(ic, ia); + if (lAB > max_edge || lBC > max_edge || lCA > max_edge) continue; + + const float dzAB = std::fabs(A.z() - B.z()); + const float dzBC = std::fabs(B.z() - C.z()); + const float dzCA = std::fabs(C.z() - A.z()); + if (std::max({dzAB, dzBC, dzCA}) > max_dz) continue; + + if (neigh_xy < std::numeric_limits::infinity()) { + const auto & A3 = cloud[ia]; + const auto & B3 = cloud[ib]; + const auto & C3 = cloud[ic]; + if (distXY(A3, B3) > neigh_xy || + distXY(B3, C3) > neigh_xy || + distXY(C3, A3) > neigh_xy) { + continue; + } + } + + Eigen::Vector3f n = (B - A).cross(C - A); + const float nn = n.norm(); + if (nn < 1e-6f) continue; + n /= nn; + if (n.dot(Eigen::Vector3f::UnitZ()) < cos_max_slope) continue; + + all_triangles.emplace_back(ia, ib, ic); + ++accepted; } - tris.swap(kept); - - // 2.4) Retriangulate the cavity with p - for (const auto & kv : edge_count) { - if (kv.second != 1) {continue;} // interior edges appear twice - int u = kv.first.u, v = kv.first.v; - // enforce CCW for (u, v, p) - if (orient2d(vs[u], vs[v], p) <= 0.0) {std::swap(u, v);} - tris.push_back({u, v, pi}); + + band_tri_ranges.emplace_back(tri_off, accepted); + } + + if (all_triangles.empty()) { + return navmap::NavMap(); + } + + // 4) Construcción del NavMap + const std::string frame_id = "map"; + navmap::NavMap core; + if (!build_navmap_from_mesh(cloud, all_triangles, frame_id, out_msg, &core)) { + out_msg = navmap_ros_interfaces::msg::NavMap(); + return navmap::NavMap(); + } + + // Reclasifica surfaces por banda asumiendo que se preserva el orden. + // Si tu builder reordena, puedes re-clasificar por Z media (ver variante en mensajes previos). + out_msg.surfaces.clear(); + for (const auto & oc : band_tri_ranges) { + const size_t off = oc.first, cnt = oc.second; + if (cnt == 0) continue; + navmap_ros_interfaces::msg::NavMapSurface s; + s.frame_id = frame_id; + s.navcels.resize(cnt); + for (size_t k = 0; k < cnt; ++k) { + s.navcels[k] = static_cast(off + k); } + out_msg.surfaces.push_back(std::move(s)); + } + + navmap::NavMap nm = from_msg(out_msg); + return nm; +} + +// ----------------- Variante Local Grow (opcional) ----------------- + +navmap::NavMap from_points_local_grow( + const pcl::PointCloud & input_points, + navmap_ros_interfaces::msg::NavMap & out_msg, + BuildParams P) +{ + using std::cerr; + using std::endl; + + out_msg = navmap_ros_interfaces::msg::NavMap(); + + auto fmtf = [](float v){ std::ostringstream ss; ss< cloud; + if (P.resolution > 0.0f) { + cloud = downsample_voxelize_avgZ(input_points, P.resolution); + } else { + cloud = input_points; + } + cerr << "[local_grow] cloud after downsample: " << cloud.size() << " points\n"; + if (cloud.size() < 3) { + cerr << "[local_grow] Less than 3 points after downsample. RETURN.\n"; + return navmap::NavMap(); } - // 2.5) Discard triangles touching the super-triangle - vector final_tris; final_tris.reserve(tris.size()); - for (const auto & t : tris) { - if (t.a >= static_cast(pts2.size()) || - t.b >= static_cast(pts2.size()) || - t.c >= static_cast(pts2.size())) + // 1) KdTree + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(cloud.makeShared()); + cerr << "[local_grow] KdTree built.\n"; + + const int N = static_cast(cloud.size()); + std::vector used_vertex(N, 0); + std::vector triangles; + triangles.reserve(N * 3); + + // Seeds en orden de Z ascendente + std::vector order(N); + std::iota(order.begin(), order.end(), 0); + std::sort(order.begin(), order.end(), + [&](int a, int b){ return cloud[a].z < cloud[b].z; }); + cerr << "[local_grow] seeds (ordered by Z): " << order.size() << "\n"; + + // Rangos por superficie + std::vector> surf_tri_ranges; + + // Estructuras temporales por componente + std::unordered_set tri_set; + std::unordered_set edge_set; + + std::queue frontier; + + // Contadores globales + struct RejectCounts { + size_t edge_len = 0; + size_t dz = 0; + size_t xy = 0; + size_t dup = 0; + size_t geom = 0; // min_area / slope / min_angle / degenerado + } global_rej{}; + + size_t global_tri_fan_attempts = 0, global_tri_fan_accept = 0; + size_t global_tri_bfs_attempts = 0, global_tri_bfs_accept = 0; + + auto dist3f = [](const pcl::PointXYZ &A, const pcl::PointXYZ &B){ + const float dx=A.x-B.x, dy=A.y-B.y, dz=A.z-B.z; return std::sqrt(dx*dx+dy*dy+dz*dz); + }; + auto distXY = [](const pcl::PointXYZ &A, const pcl::PointXYZ &B){ + const float dx=A.x-B.x, dy=A.y-B.y; return std::sqrt(dx*dx+dy*dy); + }; + + enum class Phase { FAN, BFS }; + + auto precheck = [&](int i, int j, int k, Phase phase, RejectCounts & rej, bool &dup_out)->bool { + // Duplicado + TriKey tk = make_tri(i,j,k); + if (tri_set.find(tk) != tri_set.end()) { rej.dup++; dup_out = true; return false; } + dup_out = false; + + const auto &A = cloud[i], &B = cloud[j], &C = cloud[k]; + + // Max edge (3D) — en ambas fases + if (P.max_edge_len > 0.0f) { + const float lAB = dist3f(A,B), lBC = dist3f(B,C), lCA = dist3f(C,A); + if (lAB > P.max_edge_len || lBC > P.max_edge_len || lCA > P.max_edge_len) { + rej.edge_len++; + cerr << "[local_grow] REJECT(edge_len) |AB|="< 0.0f) { + const float dzAB = std::fabs(A.z - B.z); + const float dzBC = std::fabs(B.z - C.z); + const float dzCA = std::fabs(C.z - A.z); + const float dzmax = std::max({dzAB, dzBC, dzCA}); + if (dzmax > P.max_dz) { + rej.dz++; + cerr << "[local_grow] REJECT(dz) dzAB="< 0.0f) { + const float r = P.neighbor_radius; + const float dABxy = distXY(A,B); + const float dBCxy = distXY(B,C); + const float dCAxy = distXY(C,A); + + bool bad_xy = false; + if (phase == Phase::FAN) { + // i = semilla + if (dABxy > r || dCAxy > r) bad_xy = true; // (i-j) y (i-k) + // NO comprobamos (j-k) aquí + } else { + // BFS: comprueba las tres + if (dABxy > r || dBCxy > r || dCAxy > r) bad_xy = true; + } + + if (bad_xy) { + rej.xy++; + cerr << "[local_grow] REJECT(xy phase="<<(phase==Phase::FAN?"FAN":"BFS") + << ") dAB="< si falla, contaremos como 'geom'. + return true; + }; + + // Bucle de semillas + size_t seed_idx_counter = 0; + for (int seed_idx : order) { + ++seed_idx_counter; + if (used_vertex[seed_idx]) continue; + + const auto &S = cloud[seed_idx]; + cerr << "\n[local_grow] -- Seed #" << seed_idx_counter + << " (idx=" << seed_idx << ") pos=(" + << fmtf(S.x) << "," << fmtf(S.y) << "," << fmtf(S.z) << ")\n"; + + // Vecinos de la semilla + std::vector neigh_seed; { + std::vector inds; + std::vector dists; + if (P.neighbor_radius > 0.0f) { + const int found = kdtree.radiusSearch(S, P.neighbor_radius, inds, dists); + cerr << "[local_grow] radiusSearch: found=" << found << " (including self)\n"; + if (found > 0) for (int id : inds) if (id != seed_idx) neigh_seed.push_back(id); + } else { + const int K = std::max(8, P.k_neighbors); + const int found = kdtree.nearestKSearch(S, K, inds, dists); + cerr << "[local_grow] kNN: K=" << K << " found=" << found << " (including self)\n"; + if (found > 0) for (int id : inds) if (id != seed_idx) neigh_seed.push_back(id); + } + } + cerr << "[local_grow] neigh_seed (raw, excl. self): " << neigh_seed.size() << "\n"; + + // Filtro rápido por edge_len y finitos (semilla–vecino) + if (P.max_edge_len > 0.0f) { + size_t before = neigh_seed.size(); + neigh_seed.erase(std::remove_if(neigh_seed.begin(), neigh_seed.end(), + [&](int j){ + const auto &Q = cloud[j]; + if (!pcl::isFinite(Q)) return true; + return dist3f(S, Q) > P.max_edge_len; + }), neigh_seed.end()); + cerr << "[local_grow] neigh_seed after edge_len filter: " << neigh_seed.size() + << " (removed " << (before - neigh_seed.size()) << ")\n"; + } + if (neigh_seed.size() < 2) { + used_vertex[seed_idx] = 1; + cerr << "[local_grow] Insufficient neighbors (<2) after filtering. Seed skipped.\n"; continue; } - final_tris.push_back(t); - } - // ---- 3) Lift to 3D and filter ------------------------------------------ - auto tri_area3D = [](const Eigen::Vector3f & A, - const Eigen::Vector3f & B, - const Eigen::Vector3f & C) -> float { - return 0.5f * ((B - A).cross(C - A)).norm(); - }; - auto tri_normal = [](const Eigen::Vector3f & A, - const Eigen::Vector3f & B, - const Eigen::Vector3f & C) -> Eigen::Vector3f { - Eigen::Vector3f n = (B - A).cross(C - A); - const float L = n.norm(); - return (L > 1e-12f) ? (n / L) : Eigen::Vector3f(0.f, 0.f, 1.f); - }; - auto edge_len3D = [&](int i, int j) -> float { - const auto & a = cloud[i]; const auto & b = cloud[j]; - const float dx = a.x - b.x, dy = a.y - b.y, dz = a.z - b.z; - return std::sqrt(dx * dx + dy * dy + dz * dz); - }; + // Orden angular XY + { + const auto & Cc = cloud[seed_idx]; + std::sort(neigh_seed.begin(), neigh_seed.end(), [&](int a, int b){ + const float ax = cloud[a].x - Cc.x, ay = cloud[a].y - Cc.y; + const float bx = cloud[b].x - Cc.x, by = cloud[b].y - Cc.y; + return std::atan2(ay, ax) < std::atan2(by, bx); + }); + } - const float cos_max_slope = - std::cos(params.max_slope_deg * static_cast(M_PI) / 180.0f); - const float min_area = std::max(params.min_area, 1e-9f); - const float max_edge = - (params.max_edge_len > 0.0f) ? - params.max_edge_len : - std::numeric_limits::infinity(); - const float max_dz = - (params.max_dz > 0.0f) ? - params.max_dz : - std::numeric_limits::infinity(); + const size_t tri_off = triangles.size(); + tri_set.clear(); + edge_set.clear(); + + // Contadores por componente + RejectCounts comp_rej{}; + size_t comp_fan_attempts = 0, comp_fan_accept = 0; + size_t comp_bfs_attempts = 0, comp_bfs_accept = 0; + + // 2) Abanico inicial (relajamos XY en el lado vecino–vecino) + cerr << "[local_grow] Fan: pairs=" << (neigh_seed.size() > 0 ? (neigh_seed.size()-1) : 0) << "\n"; + for (size_t t = 0; t + 1 < neigh_seed.size(); ++t) { + const int j = neigh_seed[t]; + const int k = neigh_seed[t+1]; + comp_fan_attempts++; global_tri_fan_attempts++; + + bool dup=false; + if (!precheck(seed_idx, j, k, Phase::FAN, comp_rej, dup)) { + continue; + } + if (try_add_triangle(seed_idx, j, k, cloud, P, tri_set, edge_set, triangles)) { + comp_fan_accept++; global_tri_fan_accept++; + } else { + if (!dup) { comp_rej.geom++; cerr << "[local_grow] REJECT(geom) by try_add_triangle\n"; } + } + } + // Cierre del abanico + if (neigh_seed.size() >= 3) { + const int j = neigh_seed.back(); + const int k = neigh_seed.front(); + comp_fan_attempts++; global_tri_fan_attempts++; + + bool dup=false; + if (precheck(seed_idx, j, k, Phase::FAN, comp_rej, dup)) { + if (try_add_triangle(seed_idx, j, k, cloud, P, tri_set, edge_set, triangles)) { + comp_fan_accept++; global_tri_fan_accept++; + } else { + if (!dup) { comp_rej.geom++; cerr << "[local_grow] REJECT(geom) by try_add_triangle\n"; } + } + } + } - vector triangles; - triangles.reserve(final_tris.size()); + cerr << "[local_grow] Fan result: attempts=" << comp_fan_attempts + << " accepted=" << comp_fan_accept + << " tri_now=" << (triangles.size() - tri_off) << "\n"; + if (triangles.size() == tri_off) { + used_vertex[seed_idx] = 1; + cerr << "[local_grow] No triangle from fan. Seed skipped.\n"; + // Acumula rechazos del componente en globales y sigue + global_rej.edge_len += comp_rej.edge_len; + global_rej.dz += comp_rej.dz; + global_rej.xy += comp_rej.xy; + global_rej.dup += comp_rej.dup; + global_rej.geom += comp_rej.geom; + continue; + } + + // Marca vértices usados + for (size_t u = tri_off; u < triangles.size(); ++u) { + used_vertex[triangles[u][0]] = 1; + used_vertex[triangles[u][1]] = 1; + used_vertex[triangles[u][2]] = 1; + } + + // Inicializa frontera con aristas de los nuevos triángulos + { + size_t edges_queued = 0; + for (size_t u = tri_off; u < triangles.size(); ++u) { + const auto &t = triangles[u]; + frontier.emplace(EdgeKey(t[0], t[1])); edges_queued++; + frontier.emplace(EdgeKey(t[1], t[2])); edges_queued++; + frontier.emplace(EdgeKey(t[2], t[0])); edges_queued++; + } + cerr << "[local_grow] Frontier initialized: " << edges_queued << " edges\n"; + } + + // 3) Expansión BFS (aquí sí exigimos radio XY en TODAS las aristas del triángulo) + size_t bfs_iters = 0; + while (!frontier.empty()) { + EdgeKey e = frontier.front(); frontier.pop(); + ++bfs_iters; + + // Vecinos de e.a y e.b + std::vector neigh_a, neigh_b; + { + std::vector inds; std::vector dists; + if (P.neighbor_radius > 0.0f) { + if (kdtree.radiusSearch(cloud[e.a], P.neighbor_radius, inds, dists) > 0) { + for (int id : inds) if (id != e.a) neigh_a.push_back(id); + } + inds.clear(); dists.clear(); + if (kdtree.radiusSearch(cloud[e.b], P.neighbor_radius, inds, dists) > 0) { + for (int id : inds) if (id != e.b) neigh_b.push_back(id); + } + } else { + const int K = std::max(8, P.k_neighbors); + if (kdtree.nearestKSearch(cloud[e.a], K, inds, dists) > 0) { + for (int id : inds) if (id != e.a) neigh_a.push_back(id); + } + inds.clear(); dists.clear(); + if (kdtree.nearestKSearch(cloud[e.b], K, inds, dists) > 0) { + for (int id : inds) if (id != e.b) neigh_b.push_back(id); + } + } + } - // Debug counters - size_t dropped_area = 0, dropped_edge = 0, dropped_dz = 0, dropped_slope = 0; + for (int c : neigh_a) { + if (c == e.a || c == e.b) continue; - for (const auto & t : final_tris) { - const int ia = vs[t.a].idx3d; - const int ib = vs[t.b].idx3d; - const int ic = vs[t.c].idx3d; + // Filtro rápido: que c esté también cerca de e.b en XY (si hay radio) + if (P.neighbor_radius > 0.0f) { + const float dx = cloud[c].x - cloud[e.b].x; + const float dy = cloud[c].y - cloud[e.b].y; + if ((dx*dx + dy*dy) > P.neighbor_radius * P.neighbor_radius) continue; + } - const Eigen::Vector3f A(cloud[ia].x, cloud[ia].y, cloud[ia].z); - const Eigen::Vector3f B(cloud[ib].x, cloud[ib].y, cloud[ib].z); - const Eigen::Vector3f C(cloud[ic].x, cloud[ic].y, cloud[ic].z); + comp_bfs_attempts++; global_tri_bfs_attempts++; - const float area = tri_area3D(A, B, C); - if (area < min_area) {++dropped_area; continue;} + bool dup=false; + if (!precheck(e.a, e.b, c, Phase::BFS, comp_rej, dup)) { + continue; + } + if (try_add_triangle(e.a, e.b, c, cloud, P, tri_set, edge_set, triangles)) { + comp_bfs_accept++; global_tri_bfs_accept++; + frontier.emplace(EdgeKey(e.a, c)); + frontier.emplace(EdgeKey(c, e.b)); + used_vertex[e.a] = used_vertex[e.b] = used_vertex[c] = 1; + } else { + if (!dup) { comp_rej.geom++; cerr << "[local_grow] REJECT(geom) by try_add_triangle\n"; } + } + } - const float lAB = edge_len3D(ia, ib); - const float lBC = edge_len3D(ib, ic); - const float lCA = edge_len3D(ic, ia); - if (lAB > max_edge || lBC > max_edge || lCA > max_edge) {++dropped_edge; continue;} + if (bfs_iters % 128 == 0) { + cerr << "[local_grow] BFS iters=" << bfs_iters + << " tri_now=" << (triangles.size() - tri_off) + << " frontier_size~" << frontier.size() << "\n"; + } + } - const float dzAB = std::fabs(A.z() - B.z()); - const float dzBC = std::fabs(B.z() - C.z()); - const float dzCA = std::fabs(C.z() - A.z()); - if (std::max({dzAB, dzBC, dzCA}) > max_dz) {++dropped_dz; continue;} + // Superficie final para esta semilla + const size_t tri_cnt = triangles.size() - tri_off; + if (tri_cnt > 0) { + surf_tri_ranges.emplace_back(tri_off, tri_cnt); + } - const Eigen::Vector3f n = tri_normal(A, B, C); - if (n.dot(Eigen::Vector3f::UnitZ()) < cos_max_slope) {++dropped_slope; continue;} + // Acumular contadores globales + global_rej.edge_len += comp_rej.edge_len; + global_rej.dz += comp_rej.dz; + global_rej.xy += comp_rej.xy; + global_rej.dup += comp_rej.dup; + global_rej.geom += comp_rej.geom; + + cerr << "[local_grow] Component done: triangles=" << tri_cnt + << " (fan acc=" << comp_fan_accept << "/" << comp_fan_attempts + << ", bfs acc=" << comp_bfs_accept << "/" << comp_bfs_attempts << ")\n"; + cerr << "[local_grow] Rejects: edge=" << comp_rej.edge_len + << " dz=" << comp_rej.dz + << " xy=" << comp_rej.xy + << " dup=" << comp_rej.dup + << " geom=" << comp_rej.geom << "\n"; + } - triangles.emplace_back(ia, ib, ic); + // Resumen global + cerr << "\n[local_grow] ====== SUMMARY ======\n"; + cerr << "[local_grow] total triangles: " << triangles.size() << "\n"; + cerr << "[local_grow] components: " << surf_tri_ranges.size() << "\n"; + cerr << "[local_grow] Fan: attempts=" << global_tri_fan_attempts + << " accepted=" << global_tri_fan_accept << "\n"; + cerr << "[local_grow] BFS: attempts=" << global_tri_bfs_attempts + << " accepted=" << global_tri_bfs_accept << "\n"; + cerr << "[local_grow] Rejects: edge=" << global_rej.edge_len + << " dz=" << global_rej.dz + << " xy=" << global_rej.xy + << " dup=" << global_rej.dup + << " geom=" << global_rej.geom << "\n"; + + if (triangles.empty()) { + cerr << "[local_grow] No triangles produced. RETURN empty.\n"; + return navmap::NavMap(); } - // ---- 4) Emit message + core -------------------------------------------- + // 4) Construye NavMap + surfaces + const std::string frame_id = "map"; + navmap_ros_interfaces::msg::NavMap msg_tmp; navmap::NavMap core; - build_navmap_from_mesh(cloud, triangles, /*frame_id*/ "map", out_msg, &core); - - // ---- Debug report ------------------------------------------------------ - // std::cerr << "[from_points] Candidate tris: " << final_tris.size() - // << " | Accepted: " << triangles.size() - // << " | Dropped (area=" << dropped_area - // << ", edge=" << dropped_edge - // << ", dz=" << dropped_dz - // << ", slope=" << dropped_slope - // << ")\n"; - - return core; + if (!build_navmap_from_mesh(cloud, triangles, frame_id, msg_tmp, &core)) { + cerr << "[local_grow] build_navmap_from_mesh FAILED. RETURN empty.\n"; + return navmap::NavMap(); + } + + msg_tmp.surfaces.clear(); + for (const auto & oc : surf_tri_ranges) { + if (oc.second == 0) continue; + navmap_ros_interfaces::msg::NavMapSurface s; + s.frame_id = frame_id; + s.navcels.resize(oc.second); + for (size_t k = 0; k < oc.second; ++k) { + s.navcels[k] = static_cast(oc.first + k); + } + msg_tmp.surfaces.push_back(std::move(s)); + } + + out_msg = std::move(msg_tmp); + cerr << "[local_grow] Surfaces emitted: " << out_msg.surfaces.size() << "\n"; + cerr << "[local_grow] ====== END from_points_local_grow ======\n\n"; + return from_msg(out_msg); } +// ----------------- ROS PointCloud2 entry ----------------- + navmap::NavMap from_pointcloud2( const sensor_msgs::msg::PointCloud2 & pc2, navmap_ros_interfaces::msg::NavMap & out_msg, @@ -1364,7 +1847,11 @@ navmap::NavMap from_pointcloud2( pcl::PointCloud input_points; pcl::fromROSMsg(pc2, input_points); - return from_points(input_points, out_msg, params); + // MODO por defecto: Banded Delaunay + // return from_points(input_points, out_msg, params); + + // Para probar la variante local: + return from_points_local_grow(input_points, out_msg, params); } } // namespace navmap_ros From 27cea21aa23866c5ae71cbb25e4cf6d99d5fc033 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 20 Oct 2025 19:13:01 +0200 Subject: [PATCH 2/7] Working slow with many points MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_ros/src/navmap_ros/conversions.cpp | 141 ++++++++++++++++++---- 1 file changed, 117 insertions(+), 24 deletions(-) diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index f1997e0..2a081fa 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -1416,6 +1416,59 @@ navmap::NavMap from_points( return nm; } +pcl::PointCloud +downsample_voxelize_topZ( + const pcl::PointCloud & input_points, + float resolution) +{ + pcl::PointCloud output; + if (input_points.empty() || !(resolution > 0.0f)) { + output.width = 0; output.height = 1; output.is_dense = true; + return output; + } + + struct Accum { + // Guardamos el punto de mayor Z visto en el voxel + float cx, cy, zmax; + bool has{false}; + }; + + std::unordered_map vox; + vox.reserve(input_points.size() / 2); + + for (const auto & pt : input_points) { + if (!pcl::isFinite(pt)) continue; + + const int vx = static_cast(std::floor(pt.x / resolution)); + const int vy = static_cast(std::floor(pt.y / resolution)); + const int vz = static_cast(std::floor(pt.z / resolution)); // solo para ID + Voxel v{vx, vy, vz}; + + auto & a = vox[v]; + if (!a.has) { + a.has = true; + a.cx = (vx + 0.5f) * resolution; // centro XY del voxel + a.cy = (vy + 0.5f) * resolution; + a.zmax = pt.z; + } else if (pt.z > a.zmax) { + a.zmax = pt.z; + } + } + + output.points.reserve(vox.size()); + for (const auto & kv : vox) { + const auto & a = kv.second; + // emitimos el “techo” del voxel + output.emplace_back(a.cx, a.cy, a.zmax); + } + + output.width = static_cast(output.points.size()); + output.height = 1; + output.is_dense = true; + return output; +} + + // ----------------- Variante Local Grow (opcional) ----------------- navmap::NavMap from_points_local_grow( @@ -1451,7 +1504,7 @@ navmap::NavMap from_points_local_grow( // 0) Downsample que no colapsa plantas pcl::PointCloud cloud; if (P.resolution > 0.0f) { - cloud = downsample_voxelize_avgZ(input_points, P.resolution); + cloud = downsample_voxelize_topZ(input_points, P.resolution); } else { cloud = input_points; } @@ -1494,6 +1547,8 @@ navmap::NavMap from_points_local_grow( size_t xy = 0; size_t dup = 0; size_t geom = 0; // min_area / slope / min_angle / degenerado + size_t zwin_seed = 0; // NUEVO: vecinos filtrados por ventana Z en fan + size_t zwin_bfs = 0; // NUEVO: candidatos filtrados por ventana Z en BFS } global_rej{}; size_t global_tri_fan_attempts = 0, global_tri_fan_accept = 0; @@ -1516,17 +1571,17 @@ navmap::NavMap from_points_local_grow( const auto &A = cloud[i], &B = cloud[j], &C = cloud[k]; - // Max edge (3D) — en ambas fases + // Max edge (3D) if (P.max_edge_len > 0.0f) { const float lAB = dist3f(A,B), lBC = dist3f(B,C), lCA = dist3f(C,A); if (lAB > P.max_edge_len || lBC > P.max_edge_len || lCA > P.max_edge_len) { rej.edge_len++; - cerr << "[local_grow] REJECT(edge_len) |AB|="< 0.0f) { const float dzAB = std::fabs(A.z - B.z); const float dzBC = std::fabs(B.z - C.z); @@ -1534,14 +1589,12 @@ navmap::NavMap from_points_local_grow( const float dzmax = std::max({dzAB, dzBC, dzCA}); if (dzmax > P.max_dz) { rej.dz++; - cerr << "[local_grow] REJECT(dz) dzAB="< 0.0f) { const float r = P.neighbor_radius; const float dABxy = distXY(A,B); @@ -1550,27 +1603,26 @@ navmap::NavMap from_points_local_grow( bool bad_xy = false; if (phase == Phase::FAN) { - // i = semilla - if (dABxy > r || dCAxy > r) bad_xy = true; // (i-j) y (i-k) - // NO comprobamos (j-k) aquí + // en FAN exigimos radio sólo para las aristas con la semilla (i) + if (dABxy > r || dCAxy > r) bad_xy = true; } else { - // BFS: comprueba las tres if (dABxy > r || dBCxy > r || dCAxy > r) bad_xy = true; } if (bad_xy) { rej.xy++; - cerr << "[local_grow] REJECT(xy phase="<<(phase==Phase::FAN?"FAN":"BFS") - << ") dAB="< si falla, contaremos como 'geom'. return true; }; + // Ventanas Z (ligeramente más holgadas que max_dz para inicio de rampa) + const float z_window_seed = std::max(P.max_dz, 0.35f); + const float z_window_bfs = std::max(P.max_dz, 0.35f); + // Bucle de semillas size_t seed_idx_counter = 0; for (int seed_idx : order) { @@ -1600,7 +1652,7 @@ navmap::NavMap from_points_local_grow( } cerr << "[local_grow] neigh_seed (raw, excl. self): " << neigh_seed.size() << "\n"; - // Filtro rápido por edge_len y finitos (semilla–vecino) + // Filtro rápido por edge_len (semilla–vecino) y finitos if (P.max_edge_len > 0.0f) { size_t before = neigh_seed.size(); neigh_seed.erase(std::remove_if(neigh_seed.begin(), neigh_seed.end(), @@ -1612,6 +1664,28 @@ navmap::NavMap from_points_local_grow( cerr << "[local_grow] neigh_seed after edge_len filter: " << neigh_seed.size() << " (removed " << (before - neigh_seed.size()) << ")\n"; } + + // *** NUEVO ***: Filtro por ventana Z respecto a la semilla + { + size_t before = neigh_seed.size(); + size_t dump_max = 3, dumped = 0; + neigh_seed.erase(std::remove_if(neigh_seed.begin(), neigh_seed.end(), + [&](int j){ + const float dz = std::fabs(cloud[j].z - S.z); + if (dz > z_window_seed) { + if (dumped < dump_max) + cerr << "[local_grow] drop(neigh zwin_seed) j="<< j + << " dz="<< fmtf(dz) << " > " << fmtf(z_window_seed) << "\n"; + ++dumped; + return true; + } + return false; + }), neigh_seed.end()); + cerr << "[local_grow] neigh_seed after Z-window (±"<< fmtf(z_window_seed) << "): " + << neigh_seed.size() << " (removed " << (before - neigh_seed.size()) << ")\n"; + global_rej.zwin_seed += (before - neigh_seed.size()); + } + if (neigh_seed.size() < 2) { used_vertex[seed_idx] = 1; cerr << "[local_grow] Insufficient neighbors (<2) after filtering. Seed skipped.\n"; @@ -1637,7 +1711,7 @@ navmap::NavMap from_points_local_grow( size_t comp_fan_attempts = 0, comp_fan_accept = 0; size_t comp_bfs_attempts = 0, comp_bfs_accept = 0; - // 2) Abanico inicial (relajamos XY en el lado vecino–vecino) + // 2) Abanico inicial (con radio XY relajado en lado vecino–vecino; ya lo teníamos) cerr << "[local_grow] Fan: pairs=" << (neigh_seed.size() > 0 ? (neigh_seed.size()-1) : 0) << "\n"; for (size_t t = 0; t + 1 < neigh_seed.size(); ++t) { const int j = neigh_seed[t]; @@ -1651,7 +1725,7 @@ navmap::NavMap from_points_local_grow( if (try_add_triangle(seed_idx, j, k, cloud, P, tri_set, edge_set, triangles)) { comp_fan_accept++; global_tri_fan_accept++; } else { - if (!dup) { comp_rej.geom++; cerr << "[local_grow] REJECT(geom) by try_add_triangle\n"; } + if (!dup) { comp_rej.geom++; /* cerr << "[local_grow] REJECT(geom)\n"; */ } } } // Cierre del abanico @@ -1665,7 +1739,7 @@ navmap::NavMap from_points_local_grow( if (try_add_triangle(seed_idx, j, k, cloud, P, tri_set, edge_set, triangles)) { comp_fan_accept++; global_tri_fan_accept++; } else { - if (!dup) { comp_rej.geom++; cerr << "[local_grow] REJECT(geom) by try_add_triangle\n"; } + if (!dup) { comp_rej.geom++; } } } } @@ -1682,6 +1756,8 @@ navmap::NavMap from_points_local_grow( global_rej.xy += comp_rej.xy; global_rej.dup += comp_rej.dup; global_rej.geom += comp_rej.geom; + global_rej.zwin_seed += comp_rej.zwin_seed; + global_rej.zwin_bfs += comp_rej.zwin_bfs; continue; } @@ -1704,12 +1780,17 @@ navmap::NavMap from_points_local_grow( cerr << "[local_grow] Frontier initialized: " << edges_queued << " edges\n"; } - // 3) Expansión BFS (aquí sí exigimos radio XY en TODAS las aristas del triángulo) + // 3) Expansión BFS (ahora con ventana Z respecto a la arista) size_t bfs_iters = 0; while (!frontier.empty()) { EdgeKey e = frontier.front(); frontier.pop(); ++bfs_iters; + const float z_mid = 0.5f * (cloud[e.a].z + cloud[e.b].z); + const float z_window_up = std::max(P.max_dz, 0.35f); // margen “hacia arriba” + const float z_window_down = 0.10f; // margen “hacia abajo” (estricto) + + // Vecinos de e.a y e.b std::vector neigh_a, neigh_b; { @@ -1737,13 +1818,19 @@ navmap::NavMap from_points_local_grow( for (int c : neigh_a) { if (c == e.a || c == e.b) continue; - // Filtro rápido: que c esté también cerca de e.b en XY (si hay radio) + // Rápido: que c esté también cerca de e.b en XY (si hay radio) if (P.neighbor_radius > 0.0f) { const float dx = cloud[c].x - cloud[e.b].x; const float dy = cloud[c].y - cloud[e.b].y; if ((dx*dx + dy*dy) > P.neighbor_radius * P.neighbor_radius) continue; } + const float dz = cloud[c].z - z_mid; + if (dz < -z_window_down || dz > z_window_up) { + ++global_rej.zwin_bfs; + continue; + } + comp_bfs_attempts++; global_tri_bfs_attempts++; bool dup=false; @@ -1756,7 +1843,7 @@ navmap::NavMap from_points_local_grow( frontier.emplace(EdgeKey(c, e.b)); used_vertex[e.a] = used_vertex[e.b] = used_vertex[c] = 1; } else { - if (!dup) { comp_rej.geom++; cerr << "[local_grow] REJECT(geom) by try_add_triangle\n"; } + if (!dup) { comp_rej.geom++; } } } @@ -1779,6 +1866,8 @@ navmap::NavMap from_points_local_grow( global_rej.xy += comp_rej.xy; global_rej.dup += comp_rej.dup; global_rej.geom += comp_rej.geom; + global_rej.zwin_seed += comp_rej.zwin_seed; + global_rej.zwin_bfs += comp_rej.zwin_bfs; cerr << "[local_grow] Component done: triangles=" << tri_cnt << " (fan acc=" << comp_fan_accept << "/" << comp_fan_attempts @@ -1802,7 +1891,10 @@ navmap::NavMap from_points_local_grow( << " dz=" << global_rej.dz << " xy=" << global_rej.xy << " dup=" << global_rej.dup - << " geom=" << global_rej.geom << "\n"; + << " geom=" << global_rej.geom + << " zwin_seed=" << global_rej.zwin_seed + << " zwin_bfs=" << global_rej.zwin_bfs + << "\n"; if (triangles.empty()) { cerr << "[local_grow] No triangles produced. RETURN empty.\n"; @@ -1837,6 +1929,7 @@ navmap::NavMap from_points_local_grow( } + // ----------------- ROS PointCloud2 entry ----------------- navmap::NavMap from_pointcloud2( From c9fd47eb1248d0b896f91251439cde0f9159f0c4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 20 Oct 2025 19:38:28 +0200 Subject: [PATCH 3/7] Acelerated respecting floors MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_ros/src/navmap_ros/conversions.cpp | 923 ++++++++++++---------- 1 file changed, 501 insertions(+), 422 deletions(-) diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index 2a081fa..953682c 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -23,6 +23,9 @@ #include #include #include +#include +#include +#include #include "geometry_msgs/msg/pose.hpp" #include @@ -572,19 +575,6 @@ static inline float tri_area( { return 0.5f * ((b - a).cross(c - a)).norm(); } -static inline float tri_slope_deg( - const Eigen::Vector3f & a, - const Eigen::Vector3f & b, - const Eigen::Vector3f & c) -{ - Eigen::Vector3f n = (b - a).cross(c - a); - float nn = n.norm(); - if (nn < 1e-9f) {return 0.0f;} - float cos_theta = std::abs(n.normalized().dot(Eigen::Vector3f::UnitZ())); // cosine w.r.t. vertical - cos_theta = std::clamp(cos_theta, 0.0f, 1.0f); - float theta = std::acos(cos_theta); // angle w.r.t. vertical - return theta * 180.0f / static_cast(M_PI); -} // Keys to avoid duplicates (edges/triangles) struct EdgeKey @@ -632,45 +622,6 @@ static inline TriKey make_tri(int i, int j, int k) return {v[0], v[1], v[2]}; } -// Local PCA to obtain a tangent plane at v. -// Returns two orthogonal axes t1, t2 on the tangent plane. -// If PCA is unreliable (too few points), fall back to a stable orthonormal pair. -inline void local_tangent_basis( - const std::vector & nbrs, - Eigen::Vector3f & t1, Eigen::Vector3f & t2) -{ - if (nbrs.size() < 3) { - t1 = Eigen::Vector3f::UnitX(); - t2 = Eigen::Vector3f::UnitY(); - return; - } - - // Center - Eigen::Vector3f mean = Eigen::Vector3f::Zero(); - for (auto & p : nbrs) { - mean += p; - } - mean /= static_cast(nbrs.size()); - - // Covariance - Eigen::Matrix3f C = Eigen::Matrix3f::Zero(); - for (auto & p : nbrs) { - Eigen::Vector3f d = p - mean; - C += d * d.transpose(); - } - C /= static_cast(nbrs.size()); - - // Eigenvectors - Eigen::SelfAdjointEigenSolver es(C); - // Ascending eigenvalues: column 0 ~ smallest (approximate normal) - Eigen::Vector3f e1 = es.eigenvectors().col(1); - Eigen::Vector3f e2 = es.eigenvectors().col(2); - - // Tangent plane basis {e1, e2} - t1 = e1.normalized(); - t2 = (e2 - e2.dot(t1) * t1).normalized(); -} - inline void triangle_angles_deg( const Eigen::Vector3f & A, const Eigen::Vector3f & B, @@ -700,123 +651,6 @@ inline void triangle_angles_deg( angC = angle_from(c, a, b); } -// Sort neighbors by angle on the tangent plane at v -inline void sort_neighbors_angular( - const Eigen::Vector3f & vpos, - const std::vector> & nbrs, // (idx, pos) - std::vector & out_ordered) -{ - std::vector pts; - pts.reserve(nbrs.size() + 1); - pts.push_back(vpos); - for (auto & it : nbrs) { - pts.push_back(it.second); - } - - Eigen::Vector3f t1, t2; - local_tangent_basis(pts, t1, t2); - - std::vector> ang_idx; - ang_idx.reserve(nbrs.size()); - for (auto & it : nbrs) { - Eigen::Vector3f d = it.second - vpos; - float x = d.dot(t1); - float y = d.dot(t2); - float ang = std::atan2(y, x); // -pi..pi - ang_idx.emplace_back(ang, it.first); - } - - std::sort(ang_idx.begin(), ang_idx.end(), - [](auto & a, auto & b){return a.first < b.first;}); - - out_ordered.clear(); - out_ordered.reserve(ang_idx.size()); - for (auto & ai : ang_idx) { - out_ordered.push_back(ai.second); - } -} - -// Attempt to add triangle (i,j,k) under the given constraints -// (NOW includes max_dz and neighbor_radius constraints) -inline bool try_add_triangle( - int i, int j, int k, - const pcl::PointCloud & cloud, - const BuildParams & P, - std::unordered_set & tri_set, - std::unordered_set & edge_set, - std::vector & tris) -{ - TriKey tk = make_tri(i, j, k); - if (tri_set.find(tk) != tri_set.end()) {return false;} - - const auto & A = cloud[i]; - const auto & B = cloud[j]; - const auto & C = cloud[k]; - if (!pcl::isFinite(A) || !pcl::isFinite(B) || !pcl::isFinite(C)) {return false;} - - // Max edge length (3D) - auto dAB = dist3(A, B); - auto dBC = dist3(B, C); - auto dCA = dist3(C, A); - if (P.max_edge_len > 0.0f && - (dAB > P.max_edge_len || dBC > P.max_edge_len || dCA > P.max_edge_len)) {return false;} - - // Max |Δz| per edge (avoid bridges between floors) - if (P.max_dz > 0.0f) { - const float dzAB = std::fabs(A.z - B.z); - const float dzBC = std::fabs(B.z - C.z); - const float dzCA = std::fabs(C.z - A.z); - if (std::max({dzAB, dzBC, dzCA}) > P.max_dz) {return false;} - } - - // Neighbor radius (XY) as lateral constraint - if (P.neighbor_radius > 0.0f) { - auto distXY = [](const pcl::PointXYZ & Pp, const pcl::PointXYZ & Qq) { - const float dx = Pp.x - Qq.x, dy = Pp.y - Qq.y; - return std::sqrt(dx*dx + dy*dy); - }; - if (distXY(A,B) > P.neighbor_radius || - distXY(B,C) > P.neighbor_radius || - distXY(C,A) > P.neighbor_radius) {return false;} - } - - Eigen::Vector3f a(A.x, A.y, A.z); - Eigen::Vector3f b(B.x, B.y, B.z); - Eigen::Vector3f c(C.x, C.y, C.z); - - // Min area - if (tri_area(a, b, c) < std::max(P.min_area, 1e-9f)) {return false;} - - // Max slope - float slope = tri_slope_deg(a, b, c); - if (slope > P.max_slope_deg) {return false;} - - // Min angle at each vertex - float angA, angB, angC; - triangle_angles_deg(a, b, c, angA, angB, angC); - if (angA < P.min_angle_deg || angB < P.min_angle_deg || angC < P.min_angle_deg) { - return false; - } - - // Orient consistently (up-facing) - { - const Eigen::Vector3f up = Eigen::Vector3f::UnitZ(); - Eigen::Vector3f n = (b - a).cross(c - a); - if (n.norm() < 1e-9f) {return false;} - if (n.dot(up) < 0.0f) { - std::swap(j, k); - } - } - - // Accept - tri_set.insert(tk); - edge_set.insert(make_edge(i, j)); - edge_set.insert(make_edge(j, k)); - edge_set.insert(make_edge(k, i)); - tris.emplace_back(Triangle(i, j, k)); - return true; -} - // ----------------- Downsampling ----------------- struct Voxel @@ -851,45 +685,6 @@ struct VoxelAccum int count = 0; }; - -// Compute voxel index with an offset (used for shifted grids) -static inline Voxel voxel_index_of_offset( - const pcl::PointXYZ & p, float res, float ox, float oy, float oz) -{ - return Voxel{ - static_cast(std::floor((p.x - ox) / res)), - static_cast(std::floor((p.y - oy) / res)), - static_cast(std::floor((p.z - oz) / res)) - }; -} - -// Single voxelization pass with offset; returns centroids per voxel -static std::vector voxel_pass_offset( - const pcl::PointCloud & in, float res, float ox, float oy, float oz) -{ - std::unordered_map map; - map.reserve(in.size() / 2 + 1); - - for (const auto & pt : in.points) { - if (!pcl::isFinite(pt)) {continue;} - Voxel v = voxel_index_of_offset(pt, res, ox, oy, oz); - auto & acc = map[v]; - acc.sum_x += pt.x; - acc.sum_y += pt.y; - acc.sum_z += pt.z; - acc.count += 1; - } - - std::vector out; - out.reserve(map.size()); - for (const auto & kv : map) { - const VoxelAccum & acc = kv.second; - const float inv = acc.count > 0 ? 1.0f / static_cast(acc.count) : 0.0f; - out.emplace_back(acc.sum_x * inv, acc.sum_y * inv, acc.sum_z * inv); - } - return out; -} - // Compute hash grid cell index for a point static inline Voxel cell_of_point(const pcl::PointXYZ & p, float cell) { @@ -924,10 +719,45 @@ downsample_voxelize_avgXYZ( const float half = 0.5f * r; // Pass A: regular grid - auto centroids_a = voxel_pass_offset(input_points, r, 0.0f, 0.0f, 0.0f); + std::unordered_map mapA; + mapA.reserve(input_points.size() / 2 + 1); + + for (const auto & pt : input_points.points) { + if (!pcl::isFinite(pt)) {continue;} + Voxel v{ + static_cast(std::floor(pt.x / r)), + static_cast(std::floor(pt.y / r)), + static_cast(std::floor(pt.z / r)) + }; + auto & acc = mapA[v]; + acc.sum_x += pt.x; acc.sum_y += pt.y; acc.sum_z += pt.z; acc.count += 1; + } + + std::vector centroids_a; + centroids_a.reserve(mapA.size()); + for (const auto & kv : mapA) { + centroids_a.push_back(accum_centroid(kv.second)); + } // Pass B: grid shifted by half resolution - auto centroids_b = voxel_pass_offset(input_points, r, half, half, half); + std::unordered_map mapB; + mapB.reserve(input_points.size() / 2 + 1); + for (const auto & pt : input_points.points) { + if (!pcl::isFinite(pt)) {continue;} + Voxel v{ + static_cast(std::floor((pt.x - half) / r)), + static_cast(std::floor((pt.y - half) / r)), + static_cast(std::floor((pt.z - half) / r)) + }; + auto & acc = mapB[v]; + acc.sum_x += pt.x; acc.sum_y += pt.y; acc.sum_z += pt.z; acc.count += 1; + } + + std::vector centroids_b; + centroids_b.reserve(mapB.size()); + for (const auto & kv : mapB) { + centroids_b.push_back(accum_centroid(kv.second)); + } // Merge centroids using a hash grid to join those split by voxel borders const float merge_radius = half; @@ -1060,7 +890,311 @@ downsample_voxelize_avgZ( return output; } -// ----------------- Crecimiento local desde semilla ----------------- +// Downsampling “Top-Z”: un punto por voxel XY con el mayor Z observado (evita contaminar rampas) +pcl::PointCloud +downsample_voxelize_topZ( + const pcl::PointCloud & input_points, + float resolution) +{ + pcl::PointCloud output; + if (input_points.empty() || !(resolution > 0.0f)) { + output.width = 0; output.height = 1; output.is_dense = true; + return output; + } + + struct Accum { + float cx, cy, zmax; + bool has{false}; + }; + + std::unordered_map vox; + vox.reserve(input_points.size() / 2); + + for (const auto & pt : input_points) { + if (!pcl::isFinite(pt)) continue; + + const int vx = static_cast(std::floor(pt.x / resolution)); + const int vy = static_cast(std::floor(pt.y / resolution)); + const int vz = static_cast(std::floor(pt.z / resolution)); // solo para ID + Voxel v{vx, vy, vz}; + + auto & a = vox[v]; + if (!a.has) { + a.has = true; + a.cx = (vx + 0.5f) * resolution; // centro XY del voxel + a.cy = (vy + 0.5f) * resolution; + a.zmax = pt.z; + } else if (pt.z > a.zmax) { + a.zmax = pt.z; + } + } + + output.points.reserve(vox.size()); + for (const auto & kv : vox) { + const auto & a = kv.second; + output.emplace_back(a.cx, a.cy, a.zmax); + } + + output.width = static_cast(output.points.size()); + output.height = 1; + output.is_dense = true; + return output; +} + +// === Downsample con capas por Z (un punto por grupo en cada celda XY) === +// Si params.max_dz <= 0, usamos un fallback razonable (p.ej. 0.3 m). + +pcl::PointCloud +downsample_voxelize_avgZ_layered( + const pcl::PointCloud & input_points, + float resolution, + float dz_merge /* usa params.max_dz */) +{ + pcl::PointCloud output; + if (input_points.empty() || !(resolution > 0.0f)) { + output.width = 0; output.height = 1; output.is_dense = true; + return output; + } + if (!(dz_merge > 0.0f)) dz_merge = 0.30f; // fallback + + struct VoxelXY { int x, y; }; + struct HashXY { + size_t operator()(const VoxelXY& v) const noexcept { + return (static_cast(v.x) * 73856093u) ^ (static_cast(v.y) * 19349663u); + } + }; + struct EqXY { + bool operator()(const VoxelXY& a, const VoxelXY& b) const noexcept { + return a.x == b.x && a.y == b.y; + } + }; + + // Recolecta puntos por celda XY + std::unordered_map, HashXY, EqXY> buckets; + buckets.reserve(input_points.size() / 4); + + for (int i = 0; i < static_cast(input_points.size()); ++i) { + const auto &pt = input_points[i]; + if (!pcl::isFinite(pt)) continue; + const int vx = static_cast(std::floor(pt.x / resolution)); + const int vy = static_cast(std::floor(pt.y / resolution)); + buckets[{vx, vy}].push_back(i); + } + + output.points.reserve(buckets.size()); // mínimo + + // Por cada celda, clusterea por Z y emite 1 punto por cluster (X,Y,Z = medias) + for (auto & kv : buckets) { + auto & idxs = kv.second; + if (idxs.empty()) continue; + + std::sort(idxs.begin(), idxs.end(), + [&](int a, int b){ return input_points[a].z < input_points[b].z; }); + + // Acumuladores del cluster actual + double sum_x = 0.0, sum_y = 0.0, sum_z = 0.0; + int count = 0; + float last_z = input_points[idxs.front()].z; + + auto flush_cluster = [&](){ + if (count <= 0) return; + const float cx = static_cast(sum_x / count); + const float cy = static_cast(sum_y / count); + const float cz = static_cast(sum_z / count); + output.emplace_back(cx, cy, cz); + sum_x = sum_y = sum_z = 0.0; count = 0; + }; + + for (int ii = 0; ii < static_cast(idxs.size()); ++ii) { + const auto &p = input_points[idxs[ii]]; + if (count == 0) { + // inicia cluster + sum_x = p.x; sum_y = p.y; sum_z = p.z; count = 1; last_z = p.z; + } else { + const float dz = std::fabs(p.z - last_z); + if (dz <= dz_merge) { + sum_x += p.x; sum_y += p.y; sum_z += p.z; ++count; last_z = p.z; + } else { + // salta a otra capa + flush_cluster(); + sum_x = p.x; sum_y = p.y; sum_z = p.z; count = 1; last_z = p.z; + } + } + } + flush_cluster(); + } + + output.width = static_cast(output.points.size()); + output.height = 1; + output.is_dense = true; + return output; +} + +pcl::PointCloud +downsample_voxelize_topZ_layered( + const pcl::PointCloud & input_points, + float resolution, + float dz_merge /* usa params.max_dz */) +{ + pcl::PointCloud output; + if (input_points.empty() || !(resolution > 0.0f)) { + output.width = 0; output.height = 1; output.is_dense = true; + return output; + } + if (!(dz_merge > 0.0f)) dz_merge = 0.30f; // fallback + + struct VoxelXY { int x, y; }; + struct HashXY { + size_t operator()(const VoxelXY& v) const noexcept { + return (static_cast(v.x) * 73856093u) ^ (static_cast(v.y) * 19349663u); + } + }; + struct EqXY { + bool operator()(const VoxelXY& a, const VoxelXY& b) const noexcept { + return a.x == b.x && a.y == b.y; + } + }; + + std::unordered_map, HashXY, EqXY> buckets; + buckets.reserve(input_points.size() / 4); + + for (int i = 0; i < static_cast(input_points.size()); ++i) { + const auto &pt = input_points[i]; + if (!pcl::isFinite(pt)) continue; + const int vx = static_cast(std::floor(pt.x / resolution)); + const int vy = static_cast(std::floor(pt.y / resolution)); + buckets[{vx, vy}].push_back(i); + } + + output.points.reserve(buckets.size()); + + for (auto & kv : buckets) { + auto & idxs = kv.second; + if (idxs.empty()) continue; + + std::sort(idxs.begin(), idxs.end(), + [&](int a, int b){ return input_points[a].z < input_points[b].z; }); + + // Cluster por Z; emitimos punto con X,Y medios y Z = z_max del cluster + double sum_x = 0.0, sum_y = 0.0; + float z_max = -std::numeric_limits::infinity(); + int count = 0; + float last_z = input_points[idxs.front()].z; + + auto flush_cluster = [&](){ + if (count <= 0) return; + const float cx = static_cast(sum_x / count); + const float cy = static_cast(sum_y / count); + output.emplace_back(cx, cy, z_max); + sum_x = 0.0; sum_y = 0.0; z_max = -std::numeric_limits::infinity(); count = 0; + }; + + for (int ii = 0; ii < static_cast(idxs.size()); ++ii) { + const auto &p = input_points[idxs[ii]]; + if (count == 0) { + sum_x = p.x; sum_y = p.y; z_max = p.z; count = 1; last_z = p.z; + } else { + const float dz = std::fabs(p.z - last_z); + if (dz <= dz_merge) { + sum_x += p.x; sum_y += p.y; z_max = std::max(z_max, p.z); ++count; last_z = p.z; + } else { + flush_cluster(); + sum_x = p.x; sum_y = p.y; z_max = p.z; count = 1; last_z = p.z; + } + } + } + flush_cluster(); + } + + output.width = static_cast(output.points.size()); + output.height = 1; + output.is_dense = true; + return output; +} + + +// ----------------- Attempt to add triangle (fast filters first) ----------------- +// Incluye: max_edge_len, max_dz, neighbor_radius (XY), min_area, min_angle, pendiente por n·z +inline bool try_add_triangle( + int i, int j, int k, + const pcl::PointCloud & cloud, + const BuildParams & P, + std::unordered_set & tri_set, + std::unordered_set & edge_set, + std::vector & tris) +{ + TriKey tk = make_tri(i, j, k); + if (tri_set.find(tk) != tri_set.end()) {return false;} + + const auto & A = cloud[i]; + const auto & B = cloud[j]; + const auto & C = cloud[k]; + if (!pcl::isFinite(A) || !pcl::isFinite(B) || !pcl::isFinite(C)) {return false;} + + // Max edge length (3D) + auto dAB = dist3(A, B); + auto dBC = dist3(B, C); + auto dCA = dist3(C, A); + if (P.max_edge_len > 0.0f && + (dAB > P.max_edge_len || dBC > P.max_edge_len || dCA > P.max_edge_len)) {return false;} + + // Max |Δz| per edge + if (P.max_dz > 0.0f) { + const float dzAB = std::fabs(A.z - B.z); + const float dzBC = std::fabs(B.z - C.z); + const float dzCA = std::fabs(C.z - A.z); + if (std::max({dzAB, dzBC, dzCA}) > P.max_dz) {return false;} + } + + // Neighbor radius (XY) + if (P.neighbor_radius > 0.0f) { + auto distXY = [](const pcl::PointXYZ & Pp, const pcl::PointXYZ & Qq) { + const float dx = Pp.x - Qq.x, dy = Pp.y - Qq.y; + return std::sqrt(dx*dx + dy*dy); + }; + if (distXY(A,B) > P.neighbor_radius || + distXY(B,C) > P.neighbor_radius || + distXY(C,A) > P.neighbor_radius) {return false;} + } + + Eigen::Vector3f a(A.x, A.y, A.z); + Eigen::Vector3f b(B.x, B.y, B.z); + Eigen::Vector3f c(C.x, C.y, C.z); + + // Min area + if (tri_area(a, b, c) < std::max(P.min_area, 1e-9f)) {return false;} + + // Max slope: usa n·z >= cos(max_slope) (sin acos) + const float cos_max_slope = + std::cos(P.max_slope_deg * static_cast(M_PI) / 180.0f); + Eigen::Vector3f n = (b - a).cross(c - a); + const float nn = n.norm(); + if (nn < 1e-9f) {return false;} + n /= nn; + if (n.dot(Eigen::Vector3f::UnitZ()) < cos_max_slope) {return false;} + + // Min angle en cada vértice + float angA, angB, angC; + triangle_angles_deg(a, b, c, angA, angB, angC); + if (angA < P.min_angle_deg || angB < P.min_angle_deg || angC < P.min_angle_deg) { + return false; + } + + // Orientación coherente (normal hacia +Z) + if (n.dot(Eigen::Vector3f::UnitZ()) < 0.0f) { + std::swap(j, k); + } + + // Accept + tri_set.insert(tk); + edge_set.insert(make_edge(i, j)); + edge_set.insert(make_edge(j, k)); + edge_set.insert(make_edge(k, i)); + tris.emplace_back(Triangle(i, j, k)); + return true; +} + +// ----------------- Crecimiento local desde semilla (acelerado) ----------------- std::vector grow_surface_from_seed( const pcl::PointCloud & cloud, @@ -1083,59 +1217,57 @@ std::vector grow_surface_from_seed( std::unordered_set tri_set; std::unordered_set edge_set; - std::vector nbr_indices; - std::vector nbr_dists; + const int MAX_NEIGH_SEED = 32; // cap vecinos semilla + const int MAX_NEIGH_BFS = 24; // cap vecinos por arista + + auto dist2XY = [](const pcl::PointXYZ &P, const pcl::PointXYZ &Q){ + const float dx=P.x-Q.x, dy=P.y-Q.y; return dx*dx+dy*dy; + }; while (!frontier.empty()) { int v = frontier.front(); frontier.pop(); const auto & V = cloud[v]; - if (!pcl::isFinite(V)) {continue;} - // 1) Neighborhood - nbr_indices.clear(); nbr_dists.clear(); - int found = 0; - if (P.use_radius) { - found = kdtree.radiusSearch(V, P.neighbor_radius, nbr_indices, nbr_dists); + // 1) Vecindad de la semilla (cap por densidad) + std::vector neigh; std::vector dists; + if (P.neighbor_radius > 0.0f) { + kdtree.radiusSearch(V, P.neighbor_radius, neigh, dists); + // ordenamos por d² en XY y cap + std::sort(neigh.begin(), neigh.end(), [&](int a, int b){ + return dist2XY(cloud[v], cloud[a]) < dist2XY(cloud[v], cloud[b]); + }); + if ((int)neigh.size() > MAX_NEIGH_SEED) neigh.resize(MAX_NEIGH_SEED); } else { - found = kdtree.nearestKSearch(V, P.k_neighbors, nbr_indices, nbr_dists); + const int K = std::max(P.k_neighbors, MAX_NEIGH_SEED); + kdtree.nearestKSearch(V, K, neigh, dists); } - if (found <= 1) {continue;} // only itself - - // 2) Filter by edge length and drop v itself - std::vector> candidates; - candidates.reserve(found); - for (int idx : nbr_indices) { - if (idx == v) {continue;} - const auto & Pn = cloud[idx]; - if (!pcl::isFinite(Pn)) {continue;} - if (P.max_edge_len <= 0.0f || dist3(V, Pn) <= P.max_edge_len) { - candidates.emplace_back(idx, Eigen::Vector3f(Pn.x, Pn.y, Pn.z)); - } - } - if (candidates.size() < 2) {continue;} - - // 3) Angular order on the local tangent plane at v - Eigen::Vector3f vpos(V.x, V.y, V.z); - std::vector ordered; - sort_neighbors_angular(vpos, candidates, ordered); - // 4) Try fan triangles from consecutive pairs - for (size_t t = 0; t + 1 < ordered.size(); ++t) { - int i = v; - int j = ordered[t]; - int k = ordered[t + 1]; - - if (try_add_triangle(i, j, k, cloud, P, tri_set, edge_set, tris)) { + // elimina self + neigh.erase(std::remove(neigh.begin(), neigh.end(), v), neigh.end()); + if (neigh.size() < 2) continue; + + // 2) Orden angular simple en XY + std::sort(neigh.begin(), neigh.end(), [&](int a, int b){ + const float ax = cloud[a].x - V.x, ay = cloud[a].y - V.y; + const float bx = cloud[b].x - V.x, by = cloud[b].y - V.y; + return std::atan2(ay, ax) < std::atan2(by, bx); + }); + + // 3) Fan + for (size_t t=0; t+1= 2) { - int j0 = ordered.front(), k0 = ordered.back(); - try_add_triangle(v, k0, j0, cloud, P, tri_set, edge_set, tris); + if (neigh.size() >= 3) { + int j = neigh.back(), k = neigh.front(); + try_add_triangle(v, j, k, cloud, P, tri_set, edge_set, tris); } + + // 4) BFS por aristas recientes: ya se gestiona en caller en la variante con trazas } return tris; @@ -1143,9 +1275,7 @@ std::vector grow_surface_from_seed( // ----------------- Banded Delaunay (gap-based) ----------------- -// Separa la nube en "bandas" siguiendo saltos en Z: -// - Ordena por Z y empieza nueva banda cuando (z_i - z_{i-1}) > gap_z. -// - Útil para plantas en edificios (saltos grandes) sin romper rampas suaves. +// Separa la nube en "bandas" siguiendo saltos en Z. static std::vector> split_points_by_z_gaps(const pcl::PointCloud & cloud, float gap_z, @@ -1196,7 +1326,8 @@ navmap::NavMap from_points( // 1) Downsample que NO colapsa plantas (promedia Z por voxel XY) pcl::PointCloud cloud; if (params.resolution > 0.0f) { - cloud = downsample_voxelize_avgZ(input_points, params.resolution); + cloud = downsample_voxelize_avgZ_layered(input_points, params.resolution, + (params.max_dz > 0.0f ? params.max_dz : 0.30f)); } else { cloud = input_points; } @@ -1205,7 +1336,6 @@ navmap::NavMap from_points( } // 2) Bandas por SALTOS en Z - // Para parkings típicos (2.5–3.0 m entre plantas), gap_z=1.0 separa bien. const float gap_z = (params.max_dz > 0.0f) ? std::max(1.0f, 3.0f * params.max_dz) : 1.0f; @@ -1398,7 +1528,6 @@ navmap::NavMap from_points( } // Reclasifica surfaces por banda asumiendo que se preserva el orden. - // Si tu builder reordena, puedes re-clasificar por Z media (ver variante en mensajes previos). out_msg.surfaces.clear(); for (const auto & oc : band_tri_ranges) { const size_t off = oc.first, cnt = oc.second; @@ -1416,60 +1545,7 @@ navmap::NavMap from_points( return nm; } -pcl::PointCloud -downsample_voxelize_topZ( - const pcl::PointCloud & input_points, - float resolution) -{ - pcl::PointCloud output; - if (input_points.empty() || !(resolution > 0.0f)) { - output.width = 0; output.height = 1; output.is_dense = true; - return output; - } - - struct Accum { - // Guardamos el punto de mayor Z visto en el voxel - float cx, cy, zmax; - bool has{false}; - }; - - std::unordered_map vox; - vox.reserve(input_points.size() / 2); - - for (const auto & pt : input_points) { - if (!pcl::isFinite(pt)) continue; - - const int vx = static_cast(std::floor(pt.x / resolution)); - const int vy = static_cast(std::floor(pt.y / resolution)); - const int vz = static_cast(std::floor(pt.z / resolution)); // solo para ID - Voxel v{vx, vy, vz}; - - auto & a = vox[v]; - if (!a.has) { - a.has = true; - a.cx = (vx + 0.5f) * resolution; // centro XY del voxel - a.cy = (vy + 0.5f) * resolution; - a.zmax = pt.z; - } else if (pt.z > a.zmax) { - a.zmax = pt.z; - } - } - - output.points.reserve(vox.size()); - for (const auto & kv : vox) { - const auto & a = kv.second; - // emitimos el “techo” del voxel - output.emplace_back(a.cx, a.cy, a.zmax); - } - - output.width = static_cast(output.points.size()); - output.height = 1; - output.is_dense = true; - return output; -} - - -// ----------------- Variante Local Grow (opcional) ----------------- +// ----------------- Variante Local Grow (con trazas + aceleraciones) ----------------- navmap::NavMap from_points_local_grow( const pcl::PointCloud & input_points, @@ -1501,7 +1577,7 @@ navmap::NavMap from_points_local_grow( return navmap::NavMap(); } - // 0) Downsample que no colapsa plantas + // 0) Downsample robusto (Top-Z) pcl::PointCloud cloud; if (P.resolution > 0.0f) { cloud = downsample_voxelize_topZ(input_points, P.resolution); @@ -1547,8 +1623,8 @@ navmap::NavMap from_points_local_grow( size_t xy = 0; size_t dup = 0; size_t geom = 0; // min_area / slope / min_angle / degenerado - size_t zwin_seed = 0; // NUEVO: vecinos filtrados por ventana Z en fan - size_t zwin_bfs = 0; // NUEVO: candidatos filtrados por ventana Z en BFS + size_t zwin_seed = 0; + size_t zwin_bfs = 0; } global_rej{}; size_t global_tri_fan_attempts = 0, global_tri_fan_accept = 0; @@ -1557,8 +1633,8 @@ navmap::NavMap from_points_local_grow( auto dist3f = [](const pcl::PointXYZ &A, const pcl::PointXYZ &B){ const float dx=A.x-B.x, dy=A.y-B.y, dz=A.z-B.z; return std::sqrt(dx*dx+dy*dy+dz*dz); }; - auto distXY = [](const pcl::PointXYZ &A, const pcl::PointXYZ &B){ - const float dx=A.x-B.x, dy=A.y-B.y; return std::sqrt(dx*dx+dy*dy); + auto dist2XY = [](const pcl::PointXYZ &A, const pcl::PointXYZ &B){ + const float dx=A.x-B.x, dy=A.y-B.y; return dx*dx+dy*dy; }; enum class Phase { FAN, BFS }; @@ -1576,7 +1652,6 @@ navmap::NavMap from_points_local_grow( const float lAB = dist3f(A,B), lBC = dist3f(B,C), lCA = dist3f(C,A); if (lAB > P.max_edge_len || lBC > P.max_edge_len || lCA > P.max_edge_len) { rej.edge_len++; - // cerr << "[local_grow] REJECT(edge_len) |AB|="< P.max_dz) { rej.dz++; - // cerr << "[local_grow] REJECT(dz) dzAB="< 0.0f) { - const float r = P.neighbor_radius; - const float dABxy = distXY(A,B); - const float dBCxy = distXY(B,C); - const float dCAxy = distXY(C,A); + const float r2 = P.neighbor_radius * P.neighbor_radius; + const float dAB2 = dist2XY(A,B); + const float dBC2 = dist2XY(B,C); + const float dCA2 = dist2XY(C,A); bool bad_xy = false; if (phase == Phase::FAN) { - // en FAN exigimos radio sólo para las aristas con la semilla (i) - if (dABxy > r || dCAxy > r) bad_xy = true; + if (dAB2 > r2 || dCA2 > r2) bad_xy = true; } else { - if (dABxy > r || dBCxy > r || dCAxy > r) bad_xy = true; + if (dAB2 > r2 || dBC2 > r2 || dCA2 > r2) bad_xy = true; } if (bad_xy) { rej.xy++; - // cerr << "[local_grow] REJECT(xy "<<(phase==Phase::FAN?"FAN":"BFS")<<") dAB="< neigh_seed; - { - std::vector inds; - std::vector dists; - if (P.neighbor_radius > 0.0f) { - const int found = kdtree.radiusSearch(S, P.neighbor_radius, inds, dists); - cerr << "[local_grow] radiusSearch: found=" << found << " (including self)\n"; - if (found > 0) for (int id : inds) if (id != seed_idx) neigh_seed.push_back(id); - } else { - const int K = std::max(8, P.k_neighbors); - const int found = kdtree.nearestKSearch(S, K, inds, dists); - cerr << "[local_grow] kNN: K=" << K << " found=" << found << " (including self)\n"; - if (found > 0) for (int id : inds) if (id != seed_idx) neigh_seed.push_back(id); - } + std::vector neigh_seed, inds; + std::vector dists; + if (P.neighbor_radius > 0.0f) { + const int found = kdtree.radiusSearch(S, P.neighbor_radius, inds, dists); + std::cerr << "[local_grow] radiusSearch: found=" << found << " (including self)\n"; + if (found > 0) for (int id : inds) if (id != seed_idx) neigh_seed.push_back(id); + // Cap por densidad (orden por d² XY) + std::sort(neigh_seed.begin(), neigh_seed.end(), [&](int a, int b){ + return dist2XY(cloud[seed_idx], cloud[a]) < dist2XY(cloud[seed_idx], cloud[b]); + }); + if ((int)neigh_seed.size() > MAX_NEIGH_SEED) neigh_seed.resize(MAX_NEIGH_SEED); + } else { + const int K = std::max(8, std::max(P.k_neighbors, MAX_NEIGH_SEED)); + const int found = kdtree.nearestKSearch(S, K, inds, dists); + std::cerr << "[local_grow] kNN: K=" << K << " found=" << found << " (including self)\n"; + if (found > 0) for (int id : inds) if (id != seed_idx) neigh_seed.push_back(id); } - cerr << "[local_grow] neigh_seed (raw, excl. self): " << neigh_seed.size() << "\n"; + std::cerr << "[local_grow] neigh_seed (raw, excl. self): " << neigh_seed.size() << "\n"; // Filtro rápido por edge_len (semilla–vecino) y finitos if (P.max_edge_len > 0.0f) { @@ -1661,11 +1740,11 @@ navmap::NavMap from_points_local_grow( if (!pcl::isFinite(Q)) return true; return dist3f(S, Q) > P.max_edge_len; }), neigh_seed.end()); - cerr << "[local_grow] neigh_seed after edge_len filter: " << neigh_seed.size() - << " (removed " << (before - neigh_seed.size()) << ")\n"; + std::cerr << "[local_grow] neigh_seed after edge_len filter: " << neigh_seed.size() + << " (removed " << (before - neigh_seed.size()) << ")\n"; } - // *** NUEVO ***: Filtro por ventana Z respecto a la semilla + // Filtro por ventana Z respecto a la semilla (simétrica ±z_window_seed) { size_t before = neigh_seed.size(); size_t dump_max = 3, dumped = 0; @@ -1674,21 +1753,21 @@ navmap::NavMap from_points_local_grow( const float dz = std::fabs(cloud[j].z - S.z); if (dz > z_window_seed) { if (dumped < dump_max) - cerr << "[local_grow] drop(neigh zwin_seed) j="<< j - << " dz="<< fmtf(dz) << " > " << fmtf(z_window_seed) << "\n"; + std::cerr << "[local_grow] drop(neigh zwin_seed) j="<< j + << " dz="<< fmtf(dz) << " > " << fmtf(z_window_seed) << "\n"; ++dumped; return true; } return false; }), neigh_seed.end()); - cerr << "[local_grow] neigh_seed after Z-window (±"<< fmtf(z_window_seed) << "): " - << neigh_seed.size() << " (removed " << (before - neigh_seed.size()) << ")\n"; + std::cerr << "[local_grow] neigh_seed after Z-window (±"<< fmtf(z_window_seed) << "): " + << neigh_seed.size() << " (removed " << (before - neigh_seed.size()) << ")\n"; global_rej.zwin_seed += (before - neigh_seed.size()); } if (neigh_seed.size() < 2) { used_vertex[seed_idx] = 1; - cerr << "[local_grow] Insufficient neighbors (<2) after filtering. Seed skipped.\n"; + std::cerr << "[local_grow] Insufficient neighbors (<2) after filtering. Seed skipped.\n"; continue; } @@ -1711,8 +1790,8 @@ navmap::NavMap from_points_local_grow( size_t comp_fan_attempts = 0, comp_fan_accept = 0; size_t comp_bfs_attempts = 0, comp_bfs_accept = 0; - // 2) Abanico inicial (con radio XY relajado en lado vecino–vecino; ya lo teníamos) - cerr << "[local_grow] Fan: pairs=" << (neigh_seed.size() > 0 ? (neigh_seed.size()-1) : 0) << "\n"; + // 2) Abanico inicial + std::cerr << "[local_grow] Fan: pairs=" << (neigh_seed.size() > 0 ? (neigh_seed.size()-1) : 0) << "\n"; for (size_t t = 0; t + 1 < neigh_seed.size(); ++t) { const int j = neigh_seed[t]; const int k = neigh_seed[t+1]; @@ -1725,7 +1804,7 @@ navmap::NavMap from_points_local_grow( if (try_add_triangle(seed_idx, j, k, cloud, P, tri_set, edge_set, triangles)) { comp_fan_accept++; global_tri_fan_accept++; } else { - if (!dup) { comp_rej.geom++; /* cerr << "[local_grow] REJECT(geom)\n"; */ } + if (!dup) { comp_rej.geom++; } } } // Cierre del abanico @@ -1744,12 +1823,12 @@ navmap::NavMap from_points_local_grow( } } - cerr << "[local_grow] Fan result: attempts=" << comp_fan_attempts - << " accepted=" << comp_fan_accept - << " tri_now=" << (triangles.size() - tri_off) << "\n"; + std::cerr << "[local_grow] Fan result: attempts=" << comp_fan_attempts + << " accepted=" << comp_fan_accept + << " tri_now=" << (triangles.size() - tri_off) << "\n"; if (triangles.size() == tri_off) { used_vertex[seed_idx] = 1; - cerr << "[local_grow] No triangle from fan. Seed skipped.\n"; + std::cerr << "[local_grow] No triangle from fan. Seed skipped.\n"; // Acumula rechazos del componente en globales y sigue global_rej.edge_len += comp_rej.edge_len; global_rej.dz += comp_rej.dz; @@ -1777,54 +1856,56 @@ navmap::NavMap from_points_local_grow( frontier.emplace(EdgeKey(t[1], t[2])); edges_queued++; frontier.emplace(EdgeKey(t[2], t[0])); edges_queued++; } - cerr << "[local_grow] Frontier initialized: " << edges_queued << " edges\n"; + std::cerr << "[local_grow] Frontier initialized: " << edges_queued << " edges\n"; } - // 3) Expansión BFS (ahora con ventana Z respecto a la arista) + // 3) Expansión BFS (ventana Z asimétrica + cap vecinos) size_t bfs_iters = 0; while (!frontier.empty()) { EdgeKey e = frontier.front(); frontier.pop(); ++bfs_iters; const float z_mid = 0.5f * (cloud[e.a].z + cloud[e.b].z); - const float z_window_up = std::max(P.max_dz, 0.35f); // margen “hacia arriba” - const float z_window_down = 0.10f; // margen “hacia abajo” (estricto) - - // Vecinos de e.a y e.b - std::vector neigh_a, neigh_b; - { - std::vector inds; std::vector dists; - if (P.neighbor_radius > 0.0f) { - if (kdtree.radiusSearch(cloud[e.a], P.neighbor_radius, inds, dists) > 0) { - for (int id : inds) if (id != e.a) neigh_a.push_back(id); - } - inds.clear(); dists.clear(); - if (kdtree.radiusSearch(cloud[e.b], P.neighbor_radius, inds, dists) > 0) { - for (int id : inds) if (id != e.b) neigh_b.push_back(id); - } - } else { - const int K = std::max(8, P.k_neighbors); - if (kdtree.nearestKSearch(cloud[e.a], K, inds, dists) > 0) { - for (int id : inds) if (id != e.a) neigh_a.push_back(id); - } - inds.clear(); dists.clear(); - if (kdtree.nearestKSearch(cloud[e.b], K, inds, dists) > 0) { - for (int id : inds) if (id != e.b) neigh_b.push_back(id); - } + // Vecinos de e.a y e.b (cap por densidad) + std::vector neigh_a, neigh_b; std::vector d_aux; + if (P.neighbor_radius > 0.0f) { + if (kdtree.radiusSearch(cloud[e.a], P.neighbor_radius, neigh_a, d_aux) > 0) { + neigh_a.erase(std::remove(neigh_a.begin(), neigh_a.end(), e.a), neigh_a.end()); + std::sort(neigh_a.begin(), neigh_a.end(), [&](int a, int b){ + return dist2XY(cloud[e.a], cloud[a]) < dist2XY(cloud[e.a], cloud[b]); + }); + if ((int)neigh_a.size() > MAX_NEIGH_BFS) neigh_a.resize(MAX_NEIGH_BFS); + } + d_aux.clear(); + if (kdtree.radiusSearch(cloud[e.b], P.neighbor_radius, neigh_b, d_aux) > 0) { + neigh_b.erase(std::remove(neigh_b.begin(), neigh_b.end(), e.b), neigh_b.end()); + std::sort(neigh_b.begin(), neigh_b.end(), [&](int a, int b){ + return dist2XY(cloud[e.b], cloud[a]) < dist2XY(cloud[e.b], cloud[b]); + }); + if ((int)neigh_b.size() > MAX_NEIGH_BFS) neigh_b.resize(MAX_NEIGH_BFS); + } + } else { + const int K = std::max(8, std::max(P.k_neighbors, MAX_NEIGH_BFS)); + if (kdtree.nearestKSearch(cloud[e.a], K, neigh_a, d_aux) > 0) { + neigh_a.erase(std::remove(neigh_a.begin(), neigh_a.end(), e.a), neigh_a.end()); + } + d_aux.clear(); + if (kdtree.nearestKSearch(cloud[e.b], K, neigh_b, d_aux) > 0) { + neigh_b.erase(std::remove(neigh_b.begin(), neigh_b.end(), e.b), neigh_b.end()); } } + // Probamos candidatos de 'a' que estén también razonablemente cerca de 'b' for (int c : neigh_a) { if (c == e.a || c == e.b) continue; // Rápido: que c esté también cerca de e.b en XY (si hay radio) if (P.neighbor_radius > 0.0f) { - const float dx = cloud[c].x - cloud[e.b].x; - const float dy = cloud[c].y - cloud[e.b].y; - if ((dx*dx + dy*dy) > P.neighbor_radius * P.neighbor_radius) continue; + if (dist2XY(cloud[c], cloud[e.b]) > P.neighbor_radius * P.neighbor_radius) continue; } + // Ventana Z asimétrica respecto a la arista const float dz = cloud[c].z - z_mid; if (dz < -z_window_down || dz > z_window_up) { ++global_rej.zwin_bfs; @@ -1848,9 +1929,9 @@ navmap::NavMap from_points_local_grow( } if (bfs_iters % 128 == 0) { - cerr << "[local_grow] BFS iters=" << bfs_iters - << " tri_now=" << (triangles.size() - tri_off) - << " frontier_size~" << frontier.size() << "\n"; + std::cerr << "[local_grow] BFS iters=" << bfs_iters + << " tri_now=" << (triangles.size() - tri_off) + << " frontier_size~" << frontier.size() << "\n"; } } @@ -1869,35 +1950,35 @@ navmap::NavMap from_points_local_grow( global_rej.zwin_seed += comp_rej.zwin_seed; global_rej.zwin_bfs += comp_rej.zwin_bfs; - cerr << "[local_grow] Component done: triangles=" << tri_cnt - << " (fan acc=" << comp_fan_accept << "/" << comp_fan_attempts - << ", bfs acc=" << comp_bfs_accept << "/" << comp_bfs_attempts << ")\n"; - cerr << "[local_grow] Rejects: edge=" << comp_rej.edge_len - << " dz=" << comp_rej.dz - << " xy=" << comp_rej.xy - << " dup=" << comp_rej.dup - << " geom=" << comp_rej.geom << "\n"; + std::cerr << "[local_grow] Component done: triangles=" << tri_cnt + << " (fan acc=" << comp_fan_accept << "/" << comp_fan_attempts + << ", bfs acc=" << comp_bfs_accept << "/" << comp_bfs_attempts << ")\n"; + std::cerr << "[local_grow] Rejects: edge=" << comp_rej.edge_len + << " dz=" << comp_rej.dz + << " xy=" << comp_rej.xy + << " dup=" << comp_rej.dup + << " geom=" << comp_rej.geom << "\n"; } // Resumen global - cerr << "\n[local_grow] ====== SUMMARY ======\n"; - cerr << "[local_grow] total triangles: " << triangles.size() << "\n"; - cerr << "[local_grow] components: " << surf_tri_ranges.size() << "\n"; - cerr << "[local_grow] Fan: attempts=" << global_tri_fan_attempts - << " accepted=" << global_tri_fan_accept << "\n"; - cerr << "[local_grow] BFS: attempts=" << global_tri_bfs_attempts - << " accepted=" << global_tri_bfs_accept << "\n"; - cerr << "[local_grow] Rejects: edge=" << global_rej.edge_len - << " dz=" << global_rej.dz - << " xy=" << global_rej.xy - << " dup=" << global_rej.dup - << " geom=" << global_rej.geom - << " zwin_seed=" << global_rej.zwin_seed - << " zwin_bfs=" << global_rej.zwin_bfs - << "\n"; + std::cerr << "\n[local_grow] ====== SUMMARY ======\n"; + std::cerr << "[local_grow] total triangles: " << triangles.size() << "\n"; + std::cerr << "[local_grow] components: " << surf_tri_ranges.size() << "\n"; + std::cerr << "[local_grow] Fan: attempts=" << global_tri_fan_attempts + << " accepted=" << global_tri_fan_accept << "\n"; + std::cerr << "[local_grow] BFS: attempts=" << global_tri_bfs_attempts + << " accepted=" << global_tri_bfs_accept << "\n"; + std::cerr << "[local_grow] Rejects: edge=" << global_rej.edge_len + << " dz=" << global_rej.dz + << " xy=" << global_rej.xy + << " dup=" << global_rej.dup + << " geom=" << global_rej.geom + << " zwin_seed=" << global_rej.zwin_seed + << " zwin_bfs=" << global_rej.zwin_bfs + << "\n"; if (triangles.empty()) { - cerr << "[local_grow] No triangles produced. RETURN empty.\n"; + std::cerr << "[local_grow] No triangles produced. RETURN empty.\n"; return navmap::NavMap(); } @@ -1906,7 +1987,7 @@ navmap::NavMap from_points_local_grow( navmap_ros_interfaces::msg::NavMap msg_tmp; navmap::NavMap core; if (!build_navmap_from_mesh(cloud, triangles, frame_id, msg_tmp, &core)) { - cerr << "[local_grow] build_navmap_from_mesh FAILED. RETURN empty.\n"; + std::cerr << "[local_grow] build_navmap_from_mesh FAILED. RETURN empty.\n"; return navmap::NavMap(); } @@ -1923,13 +2004,11 @@ navmap::NavMap from_points_local_grow( } out_msg = std::move(msg_tmp); - cerr << "[local_grow] Surfaces emitted: " << out_msg.surfaces.size() << "\n"; - cerr << "[local_grow] ====== END from_points_local_grow ======\n\n"; + std::cerr << "[local_grow] Surfaces emitted: " << out_msg.surfaces.size() << "\n"; + std::cerr << "[local_grow] ====== END from_points_local_grow ======\n\n"; return from_msg(out_msg); } - - // ----------------- ROS PointCloud2 entry ----------------- navmap::NavMap from_pointcloud2( @@ -1943,7 +2022,7 @@ navmap::NavMap from_pointcloud2( // MODO por defecto: Banded Delaunay // return from_points(input_points, out_msg, params); - // Para probar la variante local: + // Para la variante local (con aceleraciones y trazas): return from_points_local_grow(input_points, out_msg, params); } From a8cbca3726a3f0a7342af15505e649580b67455c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 20 Oct 2025 22:00:25 +0200 Subject: [PATCH 4/7] Final working version MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_ros/include/navmap_ros/conversions.hpp | 2 + navmap_ros/src/navmap_ros/conversions.cpp | 568 +++++++++++------- 2 files changed, 338 insertions(+), 232 deletions(-) diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index 2d35de0..91d66b6 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -213,6 +213,8 @@ struct BuildParams /** @brief Minimum interior angle (degrees) to avoid sliver triangles. */ float min_angle_deg = 20.0f; // minimum interior angle (deg) to avoid sliver triangles + + int max_surfaces = 0; // 0 ó <0 => sin límite. >0 => conservar solo las N mayores }; /** diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index 953682c..a4970c3 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -1146,16 +1146,6 @@ inline bool try_add_triangle( if (std::max({dzAB, dzBC, dzCA}) > P.max_dz) {return false;} } - // Neighbor radius (XY) - if (P.neighbor_radius > 0.0f) { - auto distXY = [](const pcl::PointXYZ & Pp, const pcl::PointXYZ & Qq) { - const float dx = Pp.x - Qq.x, dy = Pp.y - Qq.y; - return std::sqrt(dx*dx + dy*dy); - }; - if (distXY(A,B) > P.neighbor_radius || - distXY(B,C) > P.neighbor_radius || - distXY(C,A) > P.neighbor_radius) {return false;} - } Eigen::Vector3f a(A.x, A.y, A.z); Eigen::Vector3f b(B.x, B.y, B.z); @@ -1309,6 +1299,8 @@ split_points_by_z_gaps(const pcl::PointCloud & cloud, return bands; } + + // ---- from_points: Delaunay por banda + filtros 3D (slope, max_dz, neighbor_radius, etc.) navmap::NavMap from_points( @@ -1545,6 +1537,147 @@ navmap::NavMap from_points( return nm; } +// Mantiene solo las 'max_surfaces' superficies más grandes (por nº de navcels), en orden descendente. +// max_surfaces <= 0 => no hace nada. +static void keep_top_surfaces_by_size(navmap_ros_interfaces::msg::NavMap & msg, int max_surfaces) +{ + if (max_surfaces <= 0) return; + if (msg.surfaces.size() <= static_cast(max_surfaces)) return; + + // Índices 0..S-1 + std::vector ids(msg.surfaces.size()); + std::iota(ids.begin(), ids.end(), 0); + + // Ordenar por tamaño de superficie (desc) + std::sort(ids.begin(), ids.end(), [&](size_t a, size_t b){ + return msg.surfaces[a].navcels.size() > msg.surfaces[b].navcels.size(); + }); + + // Construir nueva lista con las N primeras + std::vector kept; + kept.reserve(static_cast(max_surfaces)); + for (int i = 0; i < max_surfaces; ++i) { + kept.push_back(std::move(msg.surfaces[ids[static_cast(i)]])); + } + msg.surfaces.swap(kept); +} + + +// --- Reconstrucción de surfaces por conectividad de triángulos --- +// Agrupa triángulos en componentes conectadas por ARISTAS compartidas. +// No aplica ningún límite: conserva todas las superficies, ordenadas por tamaño (desc). +// --- Reconstrucción de surfaces por conectividad (rápida, limitada a triángulos ya conservados) --- +static void rebuild_surfaces_by_connectivity(navmap_ros_interfaces::msg::NavMap & msg) +{ + const size_t ntris = msg.navcels_v0.size(); + if (ntris == 0) { msg.surfaces.clear(); return; } + + // 1) Recolecta el conjunto de triángulos que realmente queremos considerar (los que están en surfaces actuales). + // Si no hay surfaces (caso raro), considera todos. + std::vector keep(ntris, 0); + size_t kept_count = 0; + if (!msg.surfaces.empty()) { + for (const auto & s : msg.surfaces) { + for (uint32_t cid : s.navcels) { + if (cid < ntris && !keep[cid]) { keep[cid] = 1; ++kept_count; } + } + } + } else { + // Fallback: sin filtro + kept_count = ntris; + std::fill(keep.begin(), keep.end(), 1); + } + if (kept_count == 0) { msg.surfaces.clear(); return; } + + // 2) Mapea tri_global -> tri_local (0..K-1) y lista inversa. + std::vector glob2loc(ntris, UINT32_MAX); + std::vector loc2glob; + loc2glob.reserve(kept_count); + for (uint32_t t = 0; t < ntris; ++t) { + if (keep[t]) { + glob2loc[t] = static_cast(loc2glob.size()); + loc2glob.push_back(t); + } + } + const uint32_t K = static_cast(loc2glob.size()); + + // 3) Estructura Union-Find (DSU) + std::vector parent(K), rankv(K, 0); + std::iota(parent.begin(), parent.end(), 0); + auto findp = [&](uint32_t x) { + while (parent[x] != x) { parent[x] = parent[parent[x]]; x = parent[x]; } + return x; + }; + auto unite = [&](uint32_t a, uint32_t b) { + a = findp(a); b = findp(b); + if (a == b) return; + if (rankv[a] < rankv[b]) std::swap(a, b); + parent[b] = a; + if (rankv[a] == rankv[b]) ++rankv[a]; + }; + + // 4) Mapa de arista -> tri_local (usa clave 64-bit con (min(v), max(v))) + struct U64Hash { + size_t operator()(const uint64_t &k) const noexcept { return static_cast(k ^ (k >> 33)); } + }; + auto edge_key = [](uint32_t a, uint32_t b) -> uint64_t { + const uint64_t mn = (a < b) ? a : b; + const uint64_t mx = (a < b) ? b : a; + return (mn << 32) | mx; + }; + + std::unordered_map edge2tri; + edge2tri.reserve(static_cast(K) * 3u); + + // 5) Una pasada: para cada tri_local, intenta unir con el tri que ya tenía la misma arista + for (uint32_t tl = 0; tl < K; ++tl) { + const uint32_t tg = loc2glob[tl]; + const uint32_t v0 = msg.navcels_v0[tg]; + const uint32_t v1 = msg.navcels_v1[tg]; + const uint32_t v2 = msg.navcels_v2[tg]; + + const uint64_t e01 = edge_key(v0, v1); + const uint64_t e12 = edge_key(v1, v2); + const uint64_t e20 = edge_key(v2, v0); + + for (const uint64_t e : {e01, e12, e20}) { + auto it = edge2tri.find(e); + if (it == edge2tri.end()) { + edge2tri.emplace(e, tl); + } else { + unite(tl, it->second); + } + } + } + + // 6) Recolecta componentes: parent_local -> lista de tri_global + std::unordered_map> comp; // root -> tri_global list + comp.reserve(K); + for (uint32_t tl = 0; tl < K; ++tl) { + uint32_t r = findp(tl); + comp[r].push_back(loc2glob[tl]); + } + + // 7) Pasa a vector y ordénalas por tamaño (desc) + std::vector> comps; + comps.reserve(comp.size()); + for (auto & kv : comp) comps.push_back(std::move(kv.second)); + std::sort(comps.begin(), comps.end(), + [](const auto& A, const auto& B){ return A.size() > B.size(); }); + + // 8) Emite las nuevas surfaces (sin recorte aquí; ya hiciste keep_top_surfaces_by_size) + const std::string fid = msg.header.frame_id; + std::vector out; + out.reserve(comps.size()); + for (auto & C : comps) { + navmap_ros_interfaces::msg::NavMapSurface s; + s.frame_id = fid; + s.navcels.assign(C.begin(), C.end()); // índices globales de triángulo + out.push_back(std::move(s)); + } + msg.surfaces.swap(out); +} + // ----------------- Variante Local Grow (con trazas + aceleraciones) ----------------- navmap::NavMap from_points_local_grow( @@ -1553,7 +1686,6 @@ navmap::NavMap from_points_local_grow( BuildParams P) { using std::cerr; - using std::endl; out_msg = navmap_ros_interfaces::msg::NavMap(); @@ -1570,6 +1702,7 @@ navmap::NavMap from_points_local_grow( << " max_slope_deg=" << P.max_slope_deg << " min_area=" << P.min_area << " min_angle_deg=" << P.min_angle_deg + << " max_surfaces=" << P.max_surfaces << "\n"; if (input_points.size() < 3) { @@ -1577,10 +1710,11 @@ navmap::NavMap from_points_local_grow( return navmap::NavMap(); } - // 0) Downsample robusto (Top-Z) + // Downsample que preserva plantas: top-Z por capa de Z (cluster en cada celda XY usando dz_merge) pcl::PointCloud cloud; if (P.resolution > 0.0f) { - cloud = downsample_voxelize_topZ(input_points, P.resolution); + const float dz_merge = (P.max_dz > 0.0f ? P.max_dz : 0.30f); + cloud = downsample_voxelize_topZ_layered(input_points, P.resolution, dz_merge); } else { cloud = input_points; } @@ -1590,7 +1724,7 @@ navmap::NavMap from_points_local_grow( return navmap::NavMap(); } - // 1) KdTree + // KdTree pcl::KdTreeFLANN kdtree; kdtree.setInputCloud(cloud.makeShared()); cerr << "[local_grow] KdTree built.\n"; @@ -1598,7 +1732,7 @@ navmap::NavMap from_points_local_grow( const int N = static_cast(cloud.size()); std::vector used_vertex(N, 0); std::vector triangles; - triangles.reserve(N * 3); + triangles.reserve(N * 2); // Seeds en orden de Z ascendente std::vector order(N); @@ -1607,171 +1741,134 @@ navmap::NavMap from_points_local_grow( [&](int a, int b){ return cloud[a].z < cloud[b].z; }); cerr << "[local_grow] seeds (ordered by Z): " << order.size() << "\n"; - // Rangos por superficie - std::vector> surf_tri_ranges; + // --- Estado global para toda la construcción --- + std::unordered_set tri_set_global; + std::unordered_set edge_set_global; // requerido por try_add_triangle + std::unordered_map edge_use_count; - // Estructuras temporales por componente - std::unordered_set tri_set; - std::unordered_set edge_set; + auto inc_edge = [&](const EdgeKey& e) -> uint16_t { + auto it = edge_use_count.find(e); + if (it == edge_use_count.end()) { edge_use_count.emplace(e, 1); return 1; } + return ++(it->second); + }; + auto edge_count = [&](const EdgeKey& e) -> uint16_t { + auto it = edge_use_count.find(e); + return (it == edge_use_count.end()) ? 0 : it->second; + }; std::queue frontier; + std::unordered_set in_frontier; + auto push_frontier_if_border = [&](const EdgeKey& e) { + if (edge_count(e) == 1) { + if (in_frontier.insert(e).second) { + frontier.emplace(e); + } + } + }; - // Contadores globales - struct RejectCounts { - size_t edge_len = 0; - size_t dz = 0; - size_t xy = 0; - size_t dup = 0; - size_t geom = 0; // min_area / slope / min_angle / degenerado - size_t zwin_seed = 0; - size_t zwin_bfs = 0; - } global_rej{}; - + // Contadores (trazas) + struct RejectCounts { size_t edge_len=0,dz=0,xy=0,dup=0,geom=0,zwin_seed=0,zwin_bfs=0; }; + RejectCounts global_rej{}; size_t global_tri_fan_attempts = 0, global_tri_fan_accept = 0; size_t global_tri_bfs_attempts = 0, global_tri_bfs_accept = 0; auto dist3f = [](const pcl::PointXYZ &A, const pcl::PointXYZ &B){ const float dx=A.x-B.x, dy=A.y-B.y, dz=A.z-B.z; return std::sqrt(dx*dx+dy*dy+dz*dz); }; - auto dist2XY = [](const pcl::PointXYZ &A, const pcl::PointXYZ &B){ - const float dx=A.x-B.x, dy=A.y-B.y; return dx*dx+dy*dy; + auto distXY = [](const pcl::PointXYZ &A, const pcl::PointXYZ &B){ + const float dx=A.x-B.x, dy=A.y-B.y; return std::sqrt(dx*dx+dy*dy); }; enum class Phase { FAN, BFS }; - auto precheck = [&](int i, int j, int k, Phase phase, RejectCounts & rej, bool &dup_out)->bool { - // Duplicado TriKey tk = make_tri(i,j,k); - if (tri_set.find(tk) != tri_set.end()) { rej.dup++; dup_out = true; return false; } + if (tri_set_global.find(tk) != tri_set_global.end()) { rej.dup++; dup_out = true; return false; } dup_out = false; const auto &A = cloud[i], &B = cloud[j], &C = cloud[k]; - // Max edge (3D) if (P.max_edge_len > 0.0f) { const float lAB = dist3f(A,B), lBC = dist3f(B,C), lCA = dist3f(C,A); - if (lAB > P.max_edge_len || lBC > P.max_edge_len || lCA > P.max_edge_len) { - rej.edge_len++; - return false; - } + if (lAB > P.max_edge_len || lBC > P.max_edge_len || lCA > P.max_edge_len) { rej.edge_len++; return false; } } - // Max dz por arista if (P.max_dz > 0.0f) { const float dzAB = std::fabs(A.z - B.z); const float dzBC = std::fabs(B.z - C.z); const float dzCA = std::fabs(C.z - A.z); - const float dzmax = std::max({dzAB, dzBC, dzCA}); - if (dzmax > P.max_dz) { - rej.dz++; - return false; - } + if (std::max({dzAB, dzBC, dzCA}) > P.max_dz) { rej.dz++; return false; } } - // neighbor_radius (XY) if (P.neighbor_radius > 0.0f) { - const float r2 = P.neighbor_radius * P.neighbor_radius; - const float dAB2 = dist2XY(A,B); - const float dBC2 = dist2XY(B,C); - const float dCA2 = dist2XY(C,A); - - bool bad_xy = false; - if (phase == Phase::FAN) { - if (dAB2 > r2 || dCA2 > r2) bad_xy = true; - } else { - if (dAB2 > r2 || dBC2 > r2 || dCA2 > r2) bad_xy = true; - } - - if (bad_xy) { - rej.xy++; - return false; - } + const float r = P.neighbor_radius; + const float dABxy = distXY(A,B), dBCxy = distXY(B,C), dCAxy = distXY(C,A); + bool bad_xy = (phase == Phase::FAN) ? (dABxy > r || dCAxy > r) : (dABxy > r || dBCxy > r || dCAxy > r); + if (bad_xy) { rej.xy++; return false; } } return true; }; - // Ventanas Z (asimétricas para BFS) + // Ventanas Z: algo más holgadas hacia arriba para no cortar rampas const float z_window_seed = std::max(P.max_dz, 0.35f); - const float z_window_up = std::max(P.max_dz, 0.35f); - const float z_window_down = 0.10f; + const float z_window_bfs = std::max(P.max_dz, 0.35f); + const float z_window_down = 0.10f; // estricta hacia abajo en BFS - // Límites de vecinos para contener combinatoria - const int MAX_NEIGH_SEED = 32; - const int MAX_NEIGH_BFS = 24; + std::vector> surf_tri_ranges; // [offset, count] - // Bucle de semillas size_t seed_idx_counter = 0; for (int seed_idx : order) { ++seed_idx_counter; if (used_vertex[seed_idx]) continue; const auto &S = cloud[seed_idx]; - std::cerr << "\n[local_grow] -- Seed #" << seed_idx_counter - << " (idx=" << seed_idx << ") pos=(" - << fmtf(S.x) << "," << fmtf(S.y) << "," << fmtf(S.z) << ")\n"; + cerr << "\n[local_grow] -- Seed #" << seed_idx_counter + << " (idx=" << seed_idx << ") pos=(" + << fmtf(S.x) << "," << fmtf(S.y) << "," << fmtf(S.z) << ")\n"; // Vecinos de la semilla - std::vector neigh_seed, inds; - std::vector dists; - if (P.neighbor_radius > 0.0f) { - const int found = kdtree.radiusSearch(S, P.neighbor_radius, inds, dists); - std::cerr << "[local_grow] radiusSearch: found=" << found << " (including self)\n"; - if (found > 0) for (int id : inds) if (id != seed_idx) neigh_seed.push_back(id); - // Cap por densidad (orden por d² XY) - std::sort(neigh_seed.begin(), neigh_seed.end(), [&](int a, int b){ - return dist2XY(cloud[seed_idx], cloud[a]) < dist2XY(cloud[seed_idx], cloud[b]); - }); - if ((int)neigh_seed.size() > MAX_NEIGH_SEED) neigh_seed.resize(MAX_NEIGH_SEED); - } else { - const int K = std::max(8, std::max(P.k_neighbors, MAX_NEIGH_SEED)); - const int found = kdtree.nearestKSearch(S, K, inds, dists); - std::cerr << "[local_grow] kNN: K=" << K << " found=" << found << " (including self)\n"; - if (found > 0) for (int id : inds) if (id != seed_idx) neigh_seed.push_back(id); + std::vector neigh_seed; + { + std::vector inds; std::vector dists; + if (P.neighbor_radius > 0.0f) { + const int found = kdtree.radiusSearch(S, P.neighbor_radius, inds, dists); + cerr << "[local_grow] radiusSearch: found=" << found << " (including self)\n"; + if (found > 0) for (int id : inds) if (id != seed_idx) neigh_seed.push_back(id); + } else { + const int K = std::max(8, P.k_neighbors); + const int found = kdtree.nearestKSearch(S, K, inds, dists); + cerr << "[local_grow] kNN: K=" << K << " found=" << found << " (including self)\n"; + if (found > 0) for (int id : inds) if (id != seed_idx) neigh_seed.push_back(id); + } } - std::cerr << "[local_grow] neigh_seed (raw, excl. self): " << neigh_seed.size() << "\n"; + cerr << "[local_grow] neigh_seed (raw, excl. self): " << neigh_seed.size() << "\n"; - // Filtro rápido por edge_len (semilla–vecino) y finitos + // Filtros rápidos if (P.max_edge_len > 0.0f) { size_t before = neigh_seed.size(); neigh_seed.erase(std::remove_if(neigh_seed.begin(), neigh_seed.end(), - [&](int j){ - const auto &Q = cloud[j]; - if (!pcl::isFinite(Q)) return true; - return dist3f(S, Q) > P.max_edge_len; - }), neigh_seed.end()); - std::cerr << "[local_grow] neigh_seed after edge_len filter: " << neigh_seed.size() - << " (removed " << (before - neigh_seed.size()) << ")\n"; + [&](int j){ const auto &Q = cloud[j]; if (!pcl::isFinite(Q)) return true; return dist3f(S,Q) > P.max_edge_len; }), + neigh_seed.end()); + cerr << "[local_grow] neigh_seed after edge_len filter: " << neigh_seed.size() + << " (removed " << (before - neigh_seed.size()) << ")\n"; } - - // Filtro por ventana Z respecto a la semilla (simétrica ±z_window_seed) { size_t before = neigh_seed.size(); - size_t dump_max = 3, dumped = 0; neigh_seed.erase(std::remove_if(neigh_seed.begin(), neigh_seed.end(), - [&](int j){ - const float dz = std::fabs(cloud[j].z - S.z); - if (dz > z_window_seed) { - if (dumped < dump_max) - std::cerr << "[local_grow] drop(neigh zwin_seed) j="<< j - << " dz="<< fmtf(dz) << " > " << fmtf(z_window_seed) << "\n"; - ++dumped; - return true; - } - return false; - }), neigh_seed.end()); - std::cerr << "[local_grow] neigh_seed after Z-window (±"<< fmtf(z_window_seed) << "): " - << neigh_seed.size() << " (removed " << (before - neigh_seed.size()) << ")\n"; + [&](int j){ return std::fabs(cloud[j].z - S.z) > z_window_seed; }), + neigh_seed.end()); + cerr << "[local_grow] neigh_seed after Z-window (±"<< fmtf(z_window_seed) << "): " + << neigh_seed.size() << " (removed " << (before - neigh_seed.size()) << ")\n"; global_rej.zwin_seed += (before - neigh_seed.size()); } if (neigh_seed.size() < 2) { used_vertex[seed_idx] = 1; - std::cerr << "[local_grow] Insufficient neighbors (<2) after filtering. Seed skipped.\n"; + cerr << "[local_grow] Insufficient neighbors (<2) after filtering. Seed skipped.\n"; continue; } - // Orden angular XY + // Orden angular en XY { const auto & Cc = cloud[seed_idx]; std::sort(neigh_seed.begin(), neigh_seed.end(), [&](int a, int b){ @@ -1782,54 +1879,63 @@ navmap::NavMap from_points_local_grow( } const size_t tri_off = triangles.size(); - tri_set.clear(); - edge_set.clear(); - // Contadores por componente RejectCounts comp_rej{}; size_t comp_fan_attempts = 0, comp_fan_accept = 0; size_t comp_bfs_attempts = 0, comp_bfs_accept = 0; - // 2) Abanico inicial - std::cerr << "[local_grow] Fan: pairs=" << (neigh_seed.size() > 0 ? (neigh_seed.size()-1) : 0) << "\n"; + // --- FAN inicial --- + cerr << "[local_grow] Fan: pairs=" << (neigh_seed.size() > 0 ? (neigh_seed.size()-1) : 0) << "\n"; for (size_t t = 0; t + 1 < neigh_seed.size(); ++t) { const int j = neigh_seed[t]; const int k = neigh_seed[t+1]; - comp_fan_attempts++; global_tri_fan_attempts++; + ++comp_fan_attempts; ++global_tri_fan_attempts; bool dup=false; - if (!precheck(seed_idx, j, k, Phase::FAN, comp_rej, dup)) { - continue; - } - if (try_add_triangle(seed_idx, j, k, cloud, P, tri_set, edge_set, triangles)) { - comp_fan_accept++; global_tri_fan_accept++; - } else { - if (!dup) { comp_rej.geom++; } + if (!precheck(seed_idx, j, k, Phase::FAN, comp_rej, dup)) continue; + + if (try_add_triangle(seed_idx, j, k, cloud, P, tri_set_global, edge_set_global, triangles)) { + ++comp_fan_accept; ++global_tri_fan_accept; + + // Actualiza conteos y frontera + const EdgeKey e0 = make_edge(seed_idx, j); + const EdgeKey e1 = make_edge(j, k); + const EdgeKey e2 = make_edge(k, seed_idx); + inc_edge(e0); inc_edge(e1); inc_edge(e2); + push_frontier_if_border(e0); + push_frontier_if_border(e1); + push_frontier_if_border(e2); + } else if (!dup) { + comp_rej.geom++; } } - // Cierre del abanico if (neigh_seed.size() >= 3) { const int j = neigh_seed.back(); const int k = neigh_seed.front(); - comp_fan_attempts++; global_tri_fan_attempts++; - + ++comp_fan_attempts; ++global_tri_fan_attempts; bool dup=false; if (precheck(seed_idx, j, k, Phase::FAN, comp_rej, dup)) { - if (try_add_triangle(seed_idx, j, k, cloud, P, tri_set, edge_set, triangles)) { - comp_fan_accept++; global_tri_fan_accept++; - } else { - if (!dup) { comp_rej.geom++; } + if (try_add_triangle(seed_idx, j, k, cloud, P, tri_set_global, edge_set_global, triangles)) { + ++comp_fan_accept; ++global_tri_fan_accept; + const EdgeKey e0 = make_edge(seed_idx, j); + const EdgeKey e1 = make_edge(j, k); + const EdgeKey e2 = make_edge(k, seed_idx); + inc_edge(e0); inc_edge(e1); inc_edge(e2); + push_frontier_if_border(e0); + push_frontier_if_border(e1); + push_frontier_if_border(e2); + } else if (!dup) { + comp_rej.geom++; } } } - std::cerr << "[local_grow] Fan result: attempts=" << comp_fan_attempts - << " accepted=" << comp_fan_accept - << " tri_now=" << (triangles.size() - tri_off) << "\n"; + cerr << "[local_grow] Fan result: attempts=" << comp_fan_attempts + << " accepted=" << comp_fan_accept + << " tri_now=" << (triangles.size() - tri_off) << "\n"; if (triangles.size() == tri_off) { used_vertex[seed_idx] = 1; - std::cerr << "[local_grow] No triangle from fan. Seed skipped.\n"; - // Acumula rechazos del componente en globales y sigue + cerr << "[local_grow] No triangle from fan. Seed skipped.\n"; global_rej.edge_len += comp_rej.edge_len; global_rej.dz += comp_rej.dz; global_rej.xy += comp_rej.xy; @@ -1840,98 +1946,87 @@ navmap::NavMap from_points_local_grow( continue; } - // Marca vértices usados + // Marca vértices usados de los nuevos triángulos for (size_t u = tri_off; u < triangles.size(); ++u) { used_vertex[triangles[u][0]] = 1; used_vertex[triangles[u][1]] = 1; used_vertex[triangles[u][2]] = 1; } - // Inicializa frontera con aristas de los nuevos triángulos - { - size_t edges_queued = 0; - for (size_t u = tri_off; u < triangles.size(); ++u) { - const auto &t = triangles[u]; - frontier.emplace(EdgeKey(t[0], t[1])); edges_queued++; - frontier.emplace(EdgeKey(t[1], t[2])); edges_queued++; - frontier.emplace(EdgeKey(t[2], t[0])); edges_queued++; - } - std::cerr << "[local_grow] Frontier initialized: " << edges_queued << " edges\n"; - } + cerr << "[local_grow] Frontier initialized: " << in_frontier.size() << " edges\n"; - // 3) Expansión BFS (ventana Z asimétrica + cap vecinos) + // --- BFS sobre aristas de frontera --- size_t bfs_iters = 0; while (!frontier.empty()) { EdgeKey e = frontier.front(); frontier.pop(); + in_frontier.erase(e); ++bfs_iters; + // Sólo expandimos bordes actuales (conteo==1) + if (edge_count(e) != 1) continue; + const float z_mid = 0.5f * (cloud[e.a].z + cloud[e.b].z); - // Vecinos de e.a y e.b (cap por densidad) - std::vector neigh_a, neigh_b; std::vector d_aux; - if (P.neighbor_radius > 0.0f) { - if (kdtree.radiusSearch(cloud[e.a], P.neighbor_radius, neigh_a, d_aux) > 0) { - neigh_a.erase(std::remove(neigh_a.begin(), neigh_a.end(), e.a), neigh_a.end()); - std::sort(neigh_a.begin(), neigh_a.end(), [&](int a, int b){ - return dist2XY(cloud[e.a], cloud[a]) < dist2XY(cloud[e.a], cloud[b]); - }); - if ((int)neigh_a.size() > MAX_NEIGH_BFS) neigh_a.resize(MAX_NEIGH_BFS); - } - d_aux.clear(); - if (kdtree.radiusSearch(cloud[e.b], P.neighbor_radius, neigh_b, d_aux) > 0) { - neigh_b.erase(std::remove(neigh_b.begin(), neigh_b.end(), e.b), neigh_b.end()); - std::sort(neigh_b.begin(), neigh_b.end(), [&](int a, int b){ - return dist2XY(cloud[e.b], cloud[a]) < dist2XY(cloud[e.b], cloud[b]); - }); - if ((int)neigh_b.size() > MAX_NEIGH_BFS) neigh_b.resize(MAX_NEIGH_BFS); - } - } else { - const int K = std::max(8, std::max(P.k_neighbors, MAX_NEIGH_BFS)); - if (kdtree.nearestKSearch(cloud[e.a], K, neigh_a, d_aux) > 0) { - neigh_a.erase(std::remove(neigh_a.begin(), neigh_a.end(), e.a), neigh_a.end()); - } - d_aux.clear(); - if (kdtree.nearestKSearch(cloud[e.b], K, neigh_b, d_aux) > 0) { - neigh_b.erase(std::remove(neigh_b.begin(), neigh_b.end(), e.b), neigh_b.end()); + // Vecinos de e.a y comprobación rápida con e.b (radio y ventana Z) + std::vector neigh_a; + { + std::vector inds; std::vector dists; + if (P.neighbor_radius > 0.0f) { + if (kdtree.radiusSearch(cloud[e.a], P.neighbor_radius, inds, dists) > 0) { + for (int id : inds) if (id != e.a) neigh_a.push_back(id); + } + } else { + const int K = std::max(8, P.k_neighbors); + if (kdtree.nearestKSearch(cloud[e.a], K, inds, dists) > 0) { + for (int id : inds) if (id != e.a) neigh_a.push_back(id); + } } } - // Probamos candidatos de 'a' que estén también razonablemente cerca de 'b' for (int c : neigh_a) { if (c == e.a || c == e.b) continue; - // Rápido: que c esté también cerca de e.b en XY (si hay radio) if (P.neighbor_radius > 0.0f) { - if (dist2XY(cloud[c], cloud[e.b]) > P.neighbor_radius * P.neighbor_radius) continue; + const float dx = cloud[c].x - cloud[e.b].x; + const float dy = cloud[c].y - cloud[e.b].y; + if ((dx*dx + dy*dy) > P.neighbor_radius * P.neighbor_radius) continue; } - // Ventana Z asimétrica respecto a la arista + const float z_half = std::max(P.max_dz, 0.35f); // o calcula en función de res & slope const float dz = cloud[c].z - z_mid; - if (dz < -z_window_down || dz > z_window_up) { - ++global_rej.zwin_bfs; - continue; - } + if (std::fabs(dz) > z_half) { ++global_rej.zwin_bfs; continue; } - comp_bfs_attempts++; global_tri_bfs_attempts++; + ++comp_bfs_attempts; ++global_tri_bfs_attempts; bool dup=false; - if (!precheck(e.a, e.b, c, Phase::BFS, comp_rej, dup)) { - continue; - } - if (try_add_triangle(e.a, e.b, c, cloud, P, tri_set, edge_set, triangles)) { - comp_bfs_accept++; global_tri_bfs_accept++; - frontier.emplace(EdgeKey(e.a, c)); - frontier.emplace(EdgeKey(c, e.b)); + if (!precheck(e.a, e.b, c, Phase::BFS, comp_rej, dup)) continue; + + if (try_add_triangle(e.a, e.b, c, cloud, P, tri_set_global, edge_set_global, triangles)) { + ++comp_bfs_accept; ++global_tri_bfs_accept; + + // Actualiza conteos: el borde (a,b) deja de ser frontera (pasa a 2) + const EdgeKey eab = make_edge(e.a, e.b); + const EdgeKey eac = make_edge(e.a, c); + const EdgeKey ecb = make_edge(c, e.b); + + inc_edge(eab); + inc_edge(eac); + inc_edge(ecb); + + // Encola SOLO los nuevos bordes de contorno + push_frontier_if_border(eac); + push_frontier_if_border(ecb); + used_vertex[e.a] = used_vertex[e.b] = used_vertex[c] = 1; - } else { - if (!dup) { comp_rej.geom++; } + } else if (!dup) { + comp_rej.geom++; } } - if (bfs_iters % 128 == 0) { - std::cerr << "[local_grow] BFS iters=" << bfs_iters - << " tri_now=" << (triangles.size() - tri_off) - << " frontier_size~" << frontier.size() << "\n"; + if (bfs_iters % 512 == 0) { + cerr << "[local_grow] BFS iters=" << bfs_iters + << " frontier_size~" << in_frontier.size() + << " tri_total=" << triangles.size() << "\n"; } } @@ -1939,6 +2034,17 @@ navmap::NavMap from_points_local_grow( const size_t tri_cnt = triangles.size() - tri_off; if (tri_cnt > 0) { surf_tri_ranges.emplace_back(tri_off, tri_cnt); + cerr << "[local_grow] Component done: triangles=" << tri_cnt + << " (fan acc=" << comp_fan_accept << "/" << comp_fan_attempts + << ", bfs acc=" << comp_bfs_accept << "/" << comp_bfs_attempts << ")\n"; + + // Corte temprano por número máximo de superficies + if (P.max_surfaces > 0 && + surf_tri_ranges.size() >= static_cast(P.max_surfaces)) { + cerr << "[local_grow] Reached max_surfaces (" << P.max_surfaces + << "). Early stop of seed loop.\n"; + break; + } } // Acumular contadores globales @@ -1949,48 +2055,40 @@ navmap::NavMap from_points_local_grow( global_rej.geom += comp_rej.geom; global_rej.zwin_seed += comp_rej.zwin_seed; global_rej.zwin_bfs += comp_rej.zwin_bfs; - - std::cerr << "[local_grow] Component done: triangles=" << tri_cnt - << " (fan acc=" << comp_fan_accept << "/" << comp_fan_attempts - << ", bfs acc=" << comp_bfs_accept << "/" << comp_bfs_attempts << ")\n"; - std::cerr << "[local_grow] Rejects: edge=" << comp_rej.edge_len - << " dz=" << comp_rej.dz - << " xy=" << comp_rej.xy - << " dup=" << comp_rej.dup - << " geom=" << comp_rej.geom << "\n"; } // Resumen global - std::cerr << "\n[local_grow] ====== SUMMARY ======\n"; - std::cerr << "[local_grow] total triangles: " << triangles.size() << "\n"; - std::cerr << "[local_grow] components: " << surf_tri_ranges.size() << "\n"; - std::cerr << "[local_grow] Fan: attempts=" << global_tri_fan_attempts - << " accepted=" << global_tri_fan_accept << "\n"; - std::cerr << "[local_grow] BFS: attempts=" << global_tri_bfs_attempts - << " accepted=" << global_tri_bfs_accept << "\n"; - std::cerr << "[local_grow] Rejects: edge=" << global_rej.edge_len - << " dz=" << global_rej.dz - << " xy=" << global_rej.xy - << " dup=" << global_rej.dup - << " geom=" << global_rej.geom - << " zwin_seed=" << global_rej.zwin_seed - << " zwin_bfs=" << global_rej.zwin_bfs - << "\n"; + cerr << "\n[local_grow] ====== SUMMARY ======\n"; + cerr << "[local_grow] total triangles: " << triangles.size() << "\n"; + cerr << "[local_grow] components (before prune): " << surf_tri_ranges.size() << "\n"; + cerr << "[local_grow] Fan: attempts=" << global_tri_fan_attempts + << " accepted=" << global_tri_fan_accept << "\n"; + cerr << "[local_grow] BFS: attempts=" << global_tri_bfs_attempts + << " accepted=" << global_tri_bfs_accept << "\n"; + cerr << "[local_grow] Rejects: edge=" << global_rej.edge_len + << " dz=" << global_rej.dz + << " xy=" << global_rej.xy + << " dup=" << global_rej.dup + << " geom=" << global_rej.geom + << " zwin_seed=" << global_rej.zwin_seed + << " zwin_bfs=" << global_rej.zwin_bfs + << "\n"; if (triangles.empty()) { - std::cerr << "[local_grow] No triangles produced. RETURN empty.\n"; + cerr << "[local_grow] No triangles produced. RETURN empty.\n"; return navmap::NavMap(); } - // 4) Construye NavMap + surfaces + // Construcción NavMap + surfaces const std::string frame_id = "map"; navmap_ros_interfaces::msg::NavMap msg_tmp; navmap::NavMap core; if (!build_navmap_from_mesh(cloud, triangles, frame_id, msg_tmp, &core)) { - std::cerr << "[local_grow] build_navmap_from_mesh FAILED. RETURN empty.\n"; + cerr << "[local_grow] build_navmap_from_mesh FAILED. RETURN empty.\n"; return navmap::NavMap(); } + // Construye surfaces a partir de los rangos por componente msg_tmp.surfaces.clear(); for (const auto & oc : surf_tri_ranges) { if (oc.second == 0) continue; @@ -2003,12 +2101,18 @@ navmap::NavMap from_points_local_grow( msg_tmp.surfaces.push_back(std::move(s)); } + // Mantener solo top-N superficies (si procede) + rebuild_surfaces_by_connectivity(msg_tmp); + keep_top_surfaces_by_size(msg_tmp, P.max_surfaces); + out_msg = std::move(msg_tmp); - std::cerr << "[local_grow] Surfaces emitted: " << out_msg.surfaces.size() << "\n"; - std::cerr << "[local_grow] ====== END from_points_local_grow ======\n\n"; + cerr << "[local_grow] Surfaces emitted: " << out_msg.surfaces.size() << "\n"; + cerr << "[local_grow] ====== END from_points_local_grow ======\n\n"; return from_msg(out_msg); } + + // ----------------- ROS PointCloud2 entry ----------------- navmap::NavMap from_pointcloud2( From dac1a97f6adabef06ec9b9f02a2a5d22e1f1d879 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 20 Oct 2025 22:12:41 +0200 Subject: [PATCH 5/7] Final working version MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_ros/src/navmap_ros/conversions.cpp | 1262 ++++----------------- 1 file changed, 231 insertions(+), 1031 deletions(-) diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index a4970c3..96e166b 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -23,9 +23,6 @@ #include #include #include -#include -#include -#include #include "geometry_msgs/msg/pose.hpp" #include @@ -64,12 +61,9 @@ static inline uint8_t occ_to_u8(int8_t v) static inline int8_t u8_to_occ(uint8_t u) { if (u == 255u) {return -1;} - // round back to 0..100 return static_cast(std::lround((u / 254.0) * 100.0)); } -// Given grid dims (W,H) and pattern=0, the two triangle indices for cell (i,j): -// tri0=(i,j)->(i+1,j)->(i+1,j+1), tri1=(i,j)->(i+1,j+1)->(i,j+1) static inline navmap::NavCelId tri_index_for_cell(uint32_t i, uint32_t j, uint32_t W) { return static_cast((j * W + i) * 2); @@ -181,8 +175,7 @@ navmap::NavMap from_msg(const NavMap & msg) msg.surfaces[i].navcels.end()); } - // Fallback: if no surfaces were provided but there are triangles, - // create a default single surface listing all triangles. + // Fallback: create a single surface if none provided and triangles exist. if (nm.surfaces.empty() && ntris > 0) { navmap::Surface s; s.navcels.resize(ntris); @@ -210,7 +203,6 @@ navmap::NavMap from_msg(const NavMap & msg) } } - // Derived nm.rebuild_geometry_accels(); return nm; } @@ -286,8 +278,6 @@ void from_msg( throw std::runtime_error("from_msg(NavMapLayer): unsupported type value " + std::to_string(msg.type)); } - - // No description/unit in the .msg schema; leave metadata empty or keep previous. } // ----------------- OccupancyGrid <-> NavMap ----------------- @@ -301,7 +291,7 @@ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid) const float res = grid.info.resolution; const auto & O = grid.info.origin; - // 1) Positions: shared vertices (W+1)*(H+1) + // Shared grid vertices nm.positions.x.reserve((W + 1) * (H + 1)); nm.positions.y.reserve((W + 1) * (H + 1)); nm.positions.z.reserve((W + 1) * (H + 1)); @@ -316,19 +306,17 @@ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid) return static_cast(j * (W + 1) + i); }; - // 2) Triangles: 2 per cell, diagonal pattern = 0 + // Triangles: 2 per cell nm.navcels.resize(static_cast(2ull * W * H)); size_t tidx = 0; for (uint32_t j = 0; j < H; ++j) { for (uint32_t i = 0; i < W; ++i) { - // tri0: (i,j)-(i+1,j)-(i+1,j+1) { auto & c = nm.navcels[tidx++]; c.v[0] = v_id(i, j); c.v[1] = v_id(i + 1, j); c.v[2] = v_id(i + 1, j + 1); } - // tri1: (i,j)-(i+1,j+1)-(i,j+1) { auto & c = nm.navcels[tidx++]; c.v[0] = v_id(i, j); @@ -338,7 +326,7 @@ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid) } } - // 3) One surface listing all triangles + // One surface listing all triangles nm.surfaces.resize(1); nm.surfaces[0].frame_id = grid.header.frame_id; nm.surfaces[0].navcels.resize(nm.navcels.size()); @@ -346,10 +334,9 @@ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid) nm.surfaces[0].navcels[cid] = cid; } - // 4) Derived geometry nm.rebuild_geometry_accels(); - // 5) Per-NavCel "occupancy" layer (U8), two triangles per cell with the same value + // Per-NavCel occupancy layer (U8) auto occ = nm.layers.add_or_get("occupancy", nm.navcels.size(), navmap::LayerType::U8); tidx = 0; auto cell_id = [W](uint32_t i, uint32_t j) {return j * W + i;}; @@ -371,10 +358,8 @@ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) nav_msgs::msg::OccupancyGrid g; g.header.frame_id = (nm.surfaces.empty() ? std::string() : nm.surfaces[0].frame_id); - // Find occupancy layer auto base = nm.layers.get("occupancy"); if (!base || base->type() != navmap::LayerType::U8) { - // Fallback to empty grid g.info.width = 0; g.info.height = 0; g.info.resolution = 1.0f; @@ -383,8 +368,7 @@ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) } auto occ = std::dynamic_pointer_cast>(base); - // Fast path: detect regular grid from positions and triangle layout. - // Assumptions: single flat Z plane, regular spacing in X/Y, raster triangle order. + // Fast regular-grid path if (nm.surfaces.size() == 1) { std::vector xs(nm.positions.x.begin(), nm.positions.x.end()); std::vector ys(nm.positions.y.begin(), nm.positions.y.end()); @@ -398,7 +382,6 @@ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) const uint32_t W = static_cast(xs.size() - 1); const uint32_t H = static_cast(ys.size() - 1); if (occ->size() == static_cast(2ull * W * H)) { - // Fill grid directly g.info.width = W; g.info.height = H; g.info.resolution = res; @@ -412,7 +395,6 @@ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) auto idx_cell = [W](uint32_t i, uint32_t j) {return j * W + i;}; for (uint32_t j = 0; j < H; ++j) { for (uint32_t i = 0; i < W; ++i) { - // Both triangle values should be equal; use the first. const uint8_t u8 = (*occ)[tidx]; g.data[idx_cell(i, j)] = u8_to_occ(u8); tidx += 2; @@ -423,7 +405,7 @@ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) } } - // Generic fallback: sample each cell center with closest_navcel() + // Generic sampling fallback std::vector xs(nm.positions.x.begin(), nm.positions.x.end()); std::vector ys(nm.positions.y.begin(), nm.positions.y.end()); if (xs.size() < 2 || ys.size() < 2) { @@ -458,7 +440,6 @@ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) Eigen::Vector3f closest; float sq = 0.0f; - // Use closest_navcel (const) instead of locate_navcel (non-const) if (nm.closest_navcel({cx, cy, static_cast(g.info.origin.position.z)}, sidx, cid, closest, sq)) { @@ -482,7 +463,7 @@ bool build_navmap_from_mesh( out_msg = navmap_ros_interfaces::msg::NavMap(); out_msg.header.frame_id = frame_id; - // 1) Vertices + // vertices out_msg.positions_x.reserve(cloud.size()); out_msg.positions_y.reserve(cloud.size()); out_msg.positions_z.reserve(cloud.size()); @@ -492,7 +473,7 @@ bool build_navmap_from_mesh( out_msg.positions_z.push_back(p.z); } - // 2) Triangles (validate indices) + // triangles (validate indices) const std::size_t N = cloud.size(); out_msg.navcels_v0.reserve(triangles.size()); out_msg.navcels_v1.reserve(triangles.size()); @@ -505,14 +486,14 @@ bool build_navmap_from_mesh( static_cast(i1) >= N || static_cast(i2) >= N) { - return false; // invalid index + return false; } out_msg.navcels_v0.push_back(static_cast(i0)); out_msg.navcels_v1.push_back(static_cast(i1)); out_msg.navcels_v2.push_back(static_cast(i2)); } - // 3) One surface listing all triangles (required for raycasting/BVH) + // single surface with all triangles (required for BVH) { navmap_ros_interfaces::msg::NavMapSurface s; s.frame_id = frame_id; @@ -524,7 +505,7 @@ bool build_navmap_from_mesh( out_msg.surfaces.push_back(std::move(s)); } - // 4) Per-navcel "elevation" layer (float32): mean Z of triangle vertices + // elevation layer (float32): mean Z per triangle { std::vector elev; elev.reserve(triangles.size()); @@ -533,18 +514,16 @@ bool build_navmap_from_mesh( const float z0 = cloud[i0].z; const float z1 = cloud[i1].z; const float z2 = cloud[i2].z; - const float mean_z = (z0 + z1 + z2) / 3.0f; - elev.push_back(mean_z); + elev.push_back((z0 + z1 + z2) / 3.0f); } navmap_ros_interfaces::msg::NavMapLayer layer; layer.name = "elevation"; - layer.type = 1; // 0=u8, 1=f32, 2=f64 + layer.type = 1; // F32 layer.data_f32 = std::move(elev); out_msg.layers.push_back(std::move(layer)); } - // 5) Convert to core structure if requested if (out_core_opt) { *out_core_opt = navmap_ros::from_msg(out_msg); } @@ -628,18 +607,16 @@ inline void triangle_angles_deg( const Eigen::Vector3f & C, float & angA, float & angB, float & angC) { - // Sides opposite to vertices A, B, C + // side lengths opposite to vertices A/B/C float a = (B - C).norm(); float b = (C - A).norm(); float c = (A - B).norm(); - // Avoid divisions by zero const float eps = 1e-12f; a = std::max(a, eps); b = std::max(b, eps); c = std::max(c, eps); - // Law of cosines with numeric clamping auto angle_from = [](float opp, float x, float y) -> float { float cosv = (x * x + y * y - opp * opp) / (2.0f * x * y); cosv = std::min(1.0f, std::max(-1.0f, cosv)); @@ -651,7 +628,7 @@ inline void triangle_angles_deg( angC = angle_from(c, a, b); } -// ----------------- Downsampling ----------------- +// ----------------- Downsampling (only what is used) ----------------- struct Voxel { @@ -666,8 +643,7 @@ struct VoxelHash { std::size_t operator()(const Voxel & v) const noexcept { - // Simple, stable hash - std::size_t h = 1469598103934665603ull; // FNV-1a offset + std::size_t h = 1469598103934665603ull; auto mix = [&](int k) { std::size_t x = static_cast(k); h ^= x + 0x9e3779b97f4a7c15ull + (h << 6) + (h >> 2); @@ -677,380 +653,32 @@ struct VoxelHash } }; -struct VoxelAccum -{ - float sum_x = 0.0f; - float sum_y = 0.0f; - float sum_z = 0.0f; - int count = 0; -}; - -// Compute hash grid cell index for a point -static inline Voxel cell_of_point(const pcl::PointXYZ & p, float cell) -{ - return Voxel{ - static_cast(std::floor(p.x / cell)), - static_cast(std::floor(p.y / cell)), - static_cast(std::floor(p.z / cell)) - }; -} - -static inline pcl::PointXYZ accum_centroid(const VoxelAccum & a) -{ - const float inv = a.count > 0 ? 1.0f / static_cast(a.count) : 0.0f; - return pcl::PointXYZ(a.sum_x * inv, a.sum_y * inv, a.sum_z * inv); -} - -// Downsampling by voxelization with two passes (normal and shifted grid), -// followed by merging centroids that fall within 0.5*resolution -pcl::PointCloud -downsample_voxelize_avgXYZ( - const pcl::PointCloud & input_points, - float resolution) -{ - pcl::PointCloud output; - - if (input_points.empty() || !(resolution > 0.0f)) { - output.width = 0; output.height = 1; output.is_dense = true; - return output; - } - - const float r = resolution; - const float half = 0.5f * r; - - // Pass A: regular grid - std::unordered_map mapA; - mapA.reserve(input_points.size() / 2 + 1); - - for (const auto & pt : input_points.points) { - if (!pcl::isFinite(pt)) {continue;} - Voxel v{ - static_cast(std::floor(pt.x / r)), - static_cast(std::floor(pt.y / r)), - static_cast(std::floor(pt.z / r)) - }; - auto & acc = mapA[v]; - acc.sum_x += pt.x; acc.sum_y += pt.y; acc.sum_z += pt.z; acc.count += 1; - } - - std::vector centroids_a; - centroids_a.reserve(mapA.size()); - for (const auto & kv : mapA) { - centroids_a.push_back(accum_centroid(kv.second)); - } - - // Pass B: grid shifted by half resolution - std::unordered_map mapB; - mapB.reserve(input_points.size() / 2 + 1); - for (const auto & pt : input_points.points) { - if (!pcl::isFinite(pt)) {continue;} - Voxel v{ - static_cast(std::floor((pt.x - half) / r)), - static_cast(std::floor((pt.y - half) / r)), - static_cast(std::floor((pt.z - half) / r)) - }; - auto & acc = mapB[v]; - acc.sum_x += pt.x; acc.sum_y += pt.y; acc.sum_z += pt.z; acc.count += 1; - } - - std::vector centroids_b; - centroids_b.reserve(mapB.size()); - for (const auto & kv : mapB) { - centroids_b.push_back(accum_centroid(kv.second)); - } - - // Merge centroids using a hash grid to join those split by voxel borders - const float merge_radius = half; - const float merge_radius2 = merge_radius * merge_radius; - const float grid_cell_size = r; - - std::vector clusters; - clusters.reserve(centroids_a.size()); - - std::unordered_map, VoxelHash> grid; - grid.reserve(centroids_a.size() + centroids_b.size()); - - auto try_insert = [&](const pcl::PointXYZ & p) - { - Voxel c = cell_of_point(p, grid_cell_size); - int best_idx = -1; - float best_d2 = std::numeric_limits::max(); - - // Search 27 neighboring cells for a close cluster - for (int dz = -1; dz <= 1; ++dz) { - for (int dy = -1; dy <= 1; ++dy) { - for (int dx = -1; dx <= 1; ++dx) { - Voxel nb{c.x + dx, c.y + dy, c.z + dz}; - auto it = grid.find(nb); - if (it == grid.end()) {continue;} - - for (int idx : it->second) { - const pcl::PointXYZ q = accum_centroid(clusters[idx]); - const float ex = p.x - q.x; - const float ey = p.y - q.y; - const float ez = p.z - q.z; - const float d2 = ex * ex + ey * ey + ez * ez; - if (d2 < best_d2) {best_d2 = d2; best_idx = idx;} - } - } - } - } - - if (best_idx >= 0 && best_d2 <= merge_radius2) { - // Merge into existing cluster - clusters[best_idx].sum_x += p.x; - clusters[best_idx].sum_y += p.y; - clusters[best_idx].sum_z += p.z; - clusters[best_idx].count += 1; - } else { - // Create new cluster - VoxelAccum acc; - acc.sum_x = p.x; acc.sum_y = p.y; acc.sum_z = p.z; acc.count = 1; - int new_idx = static_cast(clusters.size()); - clusters.push_back(acc); - grid[c].push_back(new_idx); - } - }; - - // Insert centroids from both passes - for (const auto & p : centroids_a) { - try_insert(p); - } - for (const auto & p : centroids_b) { - try_insert(p); - } - - // Emit one point per cluster - output.points.reserve(clusters.size()); - for (const auto & cl : clusters) { - output.points.push_back(accum_centroid(cl)); - } - - output.width = static_cast(output.points.size()); - output.height = 1; - output.is_dense = true; - return output; -} - -// Downsampling que NO colapsa plantas: un punto por voxel XY con Z media -pcl::PointCloud -downsample_voxelize_avgZ( - const pcl::PointCloud & input_points, - float resolution) -{ - pcl::PointCloud output; - - if (input_points.empty() || !(resolution > 0.0f)) { - output.width = 0; output.height = 1; output.is_dense = true; - return output; - } - - struct Accum - { - double sum_z{0.0}; - int count{0}; - }; - - std::unordered_map voxels; - voxels.reserve(input_points.size() / 2); - - // Accumulate Z per voxel - for (const auto & pt : input_points) { - if (!pcl::isFinite(pt)) {continue;} - - int vx = static_cast(std::floor(pt.x / resolution)); - int vy = static_cast(std::floor(pt.y / resolution)); - int vz = static_cast(std::floor(pt.z / resolution)); // only for voxel id - - Voxel v{vx, vy, vz}; - auto & acc = voxels[v]; - acc.sum_z += pt.z; - acc.count += 1; - } - - // Emit one point per voxel (centro XY del voxel y Z media) - output.points.reserve(voxels.size()); - for (const auto & kv : voxels) { - const Voxel & v = kv.first; - const Accum & acc = kv.second; - - // Center of voxel in XY - float cx = (v.x + 0.5f) * resolution; - float cy = (v.y + 0.5f) * resolution; - // Average Z of all points - float cz = static_cast(acc.sum_z / acc.count); - - output.emplace_back(cx, cy, cz); - } - - output.width = static_cast(output.points.size()); - output.height = 1; - output.is_dense = true; - - return output; -} - -// Downsampling “Top-Z”: un punto por voxel XY con el mayor Z observado (evita contaminar rampas) -pcl::PointCloud -downsample_voxelize_topZ( - const pcl::PointCloud & input_points, - float resolution) -{ - pcl::PointCloud output; - if (input_points.empty() || !(resolution > 0.0f)) { - output.width = 0; output.height = 1; output.is_dense = true; - return output; - } - - struct Accum { - float cx, cy, zmax; - bool has{false}; - }; - - std::unordered_map vox; - vox.reserve(input_points.size() / 2); - - for (const auto & pt : input_points) { - if (!pcl::isFinite(pt)) continue; - - const int vx = static_cast(std::floor(pt.x / resolution)); - const int vy = static_cast(std::floor(pt.y / resolution)); - const int vz = static_cast(std::floor(pt.z / resolution)); // solo para ID - Voxel v{vx, vy, vz}; - - auto & a = vox[v]; - if (!a.has) { - a.has = true; - a.cx = (vx + 0.5f) * resolution; // centro XY del voxel - a.cy = (vy + 0.5f) * resolution; - a.zmax = pt.z; - } else if (pt.z > a.zmax) { - a.zmax = pt.z; - } - } - - output.points.reserve(vox.size()); - for (const auto & kv : vox) { - const auto & a = kv.second; - output.emplace_back(a.cx, a.cy, a.zmax); - } - - output.width = static_cast(output.points.size()); - output.height = 1; - output.is_dense = true; - return output; -} - -// === Downsample con capas por Z (un punto por grupo en cada celda XY) === -// Si params.max_dz <= 0, usamos un fallback razonable (p.ej. 0.3 m). - -pcl::PointCloud -downsample_voxelize_avgZ_layered( - const pcl::PointCloud & input_points, - float resolution, - float dz_merge /* usa params.max_dz */) -{ - pcl::PointCloud output; - if (input_points.empty() || !(resolution > 0.0f)) { - output.width = 0; output.height = 1; output.is_dense = true; - return output; - } - if (!(dz_merge > 0.0f)) dz_merge = 0.30f; // fallback - - struct VoxelXY { int x, y; }; - struct HashXY { - size_t operator()(const VoxelXY& v) const noexcept { - return (static_cast(v.x) * 73856093u) ^ (static_cast(v.y) * 19349663u); - } - }; - struct EqXY { - bool operator()(const VoxelXY& a, const VoxelXY& b) const noexcept { - return a.x == b.x && a.y == b.y; - } - }; - - // Recolecta puntos por celda XY - std::unordered_map, HashXY, EqXY> buckets; - buckets.reserve(input_points.size() / 4); - - for (int i = 0; i < static_cast(input_points.size()); ++i) { - const auto &pt = input_points[i]; - if (!pcl::isFinite(pt)) continue; - const int vx = static_cast(std::floor(pt.x / resolution)); - const int vy = static_cast(std::floor(pt.y / resolution)); - buckets[{vx, vy}].push_back(i); - } - - output.points.reserve(buckets.size()); // mínimo - - // Por cada celda, clusterea por Z y emite 1 punto por cluster (X,Y,Z = medias) - for (auto & kv : buckets) { - auto & idxs = kv.second; - if (idxs.empty()) continue; - - std::sort(idxs.begin(), idxs.end(), - [&](int a, int b){ return input_points[a].z < input_points[b].z; }); - - // Acumuladores del cluster actual - double sum_x = 0.0, sum_y = 0.0, sum_z = 0.0; - int count = 0; - float last_z = input_points[idxs.front()].z; - - auto flush_cluster = [&](){ - if (count <= 0) return; - const float cx = static_cast(sum_x / count); - const float cy = static_cast(sum_y / count); - const float cz = static_cast(sum_z / count); - output.emplace_back(cx, cy, cz); - sum_x = sum_y = sum_z = 0.0; count = 0; - }; - - for (int ii = 0; ii < static_cast(idxs.size()); ++ii) { - const auto &p = input_points[idxs[ii]]; - if (count == 0) { - // inicia cluster - sum_x = p.x; sum_y = p.y; sum_z = p.z; count = 1; last_z = p.z; - } else { - const float dz = std::fabs(p.z - last_z); - if (dz <= dz_merge) { - sum_x += p.x; sum_y += p.y; sum_z += p.z; ++count; last_z = p.z; - } else { - // salta a otra capa - flush_cluster(); - sum_x = p.x; sum_y = p.y; sum_z = p.z; count = 1; last_z = p.z; - } - } - } - flush_cluster(); - } - - output.width = static_cast(output.points.size()); - output.height = 1; - output.is_dense = true; - return output; -} - +// Layered “top-Z” downsampling: one point per XY voxel, using max Z per Z-cluster. pcl::PointCloud downsample_voxelize_topZ_layered( const pcl::PointCloud & input_points, float resolution, - float dz_merge /* usa params.max_dz */) + float dz_merge) { pcl::PointCloud output; if (input_points.empty() || !(resolution > 0.0f)) { output.width = 0; output.height = 1; output.is_dense = true; return output; } - if (!(dz_merge > 0.0f)) dz_merge = 0.30f; // fallback + if (!(dz_merge > 0.0f)) {dz_merge = 0.30f;} struct VoxelXY { int x, y; }; - struct HashXY { - size_t operator()(const VoxelXY& v) const noexcept { + struct HashXY + { + size_t operator()(const VoxelXY & v) const noexcept + { return (static_cast(v.x) * 73856093u) ^ (static_cast(v.y) * 19349663u); } }; - struct EqXY { - bool operator()(const VoxelXY& a, const VoxelXY& b) const noexcept { + struct EqXY + { + bool operator()(const VoxelXY & a, const VoxelXY & b) const noexcept + { return a.x == b.x && a.y == b.y; } }; @@ -1059,8 +687,8 @@ downsample_voxelize_topZ_layered( buckets.reserve(input_points.size() / 4); for (int i = 0; i < static_cast(input_points.size()); ++i) { - const auto &pt = input_points[i]; - if (!pcl::isFinite(pt)) continue; + const auto & pt = input_points[i]; + if (!pcl::isFinite(pt)) {continue;} const int vx = static_cast(std::floor(pt.x / resolution)); const int vy = static_cast(std::floor(pt.y / resolution)); buckets[{vx, vy}].push_back(i); @@ -1070,27 +698,26 @@ downsample_voxelize_topZ_layered( for (auto & kv : buckets) { auto & idxs = kv.second; - if (idxs.empty()) continue; + if (idxs.empty()) {continue;} std::sort(idxs.begin(), idxs.end(), - [&](int a, int b){ return input_points[a].z < input_points[b].z; }); + [&](int a, int b){return input_points[a].z < input_points[b].z;}); - // Cluster por Z; emitimos punto con X,Y medios y Z = z_max del cluster double sum_x = 0.0, sum_y = 0.0; float z_max = -std::numeric_limits::infinity(); int count = 0; float last_z = input_points[idxs.front()].z; auto flush_cluster = [&](){ - if (count <= 0) return; - const float cx = static_cast(sum_x / count); - const float cy = static_cast(sum_y / count); - output.emplace_back(cx, cy, z_max); - sum_x = 0.0; sum_y = 0.0; z_max = -std::numeric_limits::infinity(); count = 0; - }; + if (count <= 0) {return;} + const float cx = static_cast(sum_x / count); + const float cy = static_cast(sum_y / count); + output.emplace_back(cx, cy, z_max); + sum_x = 0.0; sum_y = 0.0; z_max = -std::numeric_limits::infinity(); count = 0; + }; for (int ii = 0; ii < static_cast(idxs.size()); ++ii) { - const auto &p = input_points[idxs[ii]]; + const auto & p = input_points[idxs[ii]]; if (count == 0) { sum_x = p.x; sum_y = p.y; z_max = p.z; count = 1; last_z = p.z; } else { @@ -1112,9 +739,11 @@ downsample_voxelize_topZ_layered( return output; } +// ----------------- Triangle acceptance filters ----------------- +// +// Includes: max edge length, max |Δz|, XY neighbor radius, min area, +// min internal angle, and slope limit via n·z. -// ----------------- Attempt to add triangle (fast filters first) ----------------- -// Incluye: max_edge_len, max_dz, neighbor_radius (XY), min_area, min_angle, pendiente por n·z inline bool try_add_triangle( int i, int j, int k, const pcl::PointCloud & cloud, @@ -1136,7 +765,7 @@ inline bool try_add_triangle( auto dBC = dist3(B, C); auto dCA = dist3(C, A); if (P.max_edge_len > 0.0f && - (dAB > P.max_edge_len || dBC > P.max_edge_len || dCA > P.max_edge_len)) {return false;} + (dAB > P.max_edge_len || dBC > P.max_edge_len || dCA > P.max_edge_len)) {return false;} // Max |Δz| per edge if (P.max_dz > 0.0f) { @@ -1146,7 +775,6 @@ inline bool try_add_triangle( if (std::max({dzAB, dzBC, dzCA}) > P.max_dz) {return false;} } - Eigen::Vector3f a(A.x, A.y, A.z); Eigen::Vector3f b(B.x, B.y, B.z); Eigen::Vector3f c(C.x, C.y, C.z); @@ -1154,28 +782,27 @@ inline bool try_add_triangle( // Min area if (tri_area(a, b, c) < std::max(P.min_area, 1e-9f)) {return false;} - // Max slope: usa n·z >= cos(max_slope) (sin acos) + // Slope limit: n·z >= cos(max_slope) const float cos_max_slope = - std::cos(P.max_slope_deg * static_cast(M_PI) / 180.0f); + std::cos(P.max_slope_deg * static_cast(M_PI) / 180.0f); Eigen::Vector3f n = (b - a).cross(c - a); const float nn = n.norm(); if (nn < 1e-9f) {return false;} n /= nn; if (n.dot(Eigen::Vector3f::UnitZ()) < cos_max_slope) {return false;} - // Min angle en cada vértice + // Min internal angle float angA, angB, angC; triangle_angles_deg(a, b, c, angA, angB, angC); if (angA < P.min_angle_deg || angB < P.min_angle_deg || angC < P.min_angle_deg) { return false; } - // Orientación coherente (normal hacia +Z) + // Canonical orientation (normal facing +Z) if (n.dot(Eigen::Vector3f::UnitZ()) < 0.0f) { std::swap(j, k); } - // Accept tri_set.insert(tk); edge_set.insert(make_edge(i, j)); edge_set.insert(make_edge(j, k)); @@ -1184,376 +811,20 @@ inline bool try_add_triangle( return true; } -// ----------------- Crecimiento local desde semilla (acelerado) ----------------- - -std::vector grow_surface_from_seed( - const pcl::PointCloud & cloud, - int seed_idx, - const BuildParams & P) -{ - std::vector tris; - if (cloud.empty() || seed_idx < 0 || seed_idx >= static_cast(cloud.size())) { - return tris; - } - - pcl::KdTreeFLANN kdtree; - kdtree.setInputCloud(cloud.makeShared()); - - std::queue frontier; - std::unordered_set seen; - frontier.push(seed_idx); - seen.insert(seed_idx); - - std::unordered_set tri_set; - std::unordered_set edge_set; - - const int MAX_NEIGH_SEED = 32; // cap vecinos semilla - const int MAX_NEIGH_BFS = 24; // cap vecinos por arista - - auto dist2XY = [](const pcl::PointXYZ &P, const pcl::PointXYZ &Q){ - const float dx=P.x-Q.x, dy=P.y-Q.y; return dx*dx+dy*dy; - }; - - while (!frontier.empty()) { - int v = frontier.front(); frontier.pop(); - const auto & V = cloud[v]; - if (!pcl::isFinite(V)) {continue;} - - // 1) Vecindad de la semilla (cap por densidad) - std::vector neigh; std::vector dists; - if (P.neighbor_radius > 0.0f) { - kdtree.radiusSearch(V, P.neighbor_radius, neigh, dists); - // ordenamos por d² en XY y cap - std::sort(neigh.begin(), neigh.end(), [&](int a, int b){ - return dist2XY(cloud[v], cloud[a]) < dist2XY(cloud[v], cloud[b]); - }); - if ((int)neigh.size() > MAX_NEIGH_SEED) neigh.resize(MAX_NEIGH_SEED); - } else { - const int K = std::max(P.k_neighbors, MAX_NEIGH_SEED); - kdtree.nearestKSearch(V, K, neigh, dists); - } - - // elimina self - neigh.erase(std::remove(neigh.begin(), neigh.end(), v), neigh.end()); - if (neigh.size() < 2) continue; - - // 2) Orden angular simple en XY - std::sort(neigh.begin(), neigh.end(), [&](int a, int b){ - const float ax = cloud[a].x - V.x, ay = cloud[a].y - V.y; - const float bx = cloud[b].x - V.x, by = cloud[b].y - V.y; - return std::atan2(ay, ax) < std::atan2(by, bx); - }); - - // 3) Fan - for (size_t t=0; t+1= 3) { - int j = neigh.back(), k = neigh.front(); - try_add_triangle(v, j, k, cloud, P, tri_set, edge_set, tris); - } - - // 4) BFS por aristas recientes: ya se gestiona en caller en la variante con trazas - } - - return tris; -} - -// ----------------- Banded Delaunay (gap-based) ----------------- - -// Separa la nube en "bandas" siguiendo saltos en Z. -static std::vector> -split_points_by_z_gaps(const pcl::PointCloud & cloud, - float gap_z, - int min_points_per_band = 3) -{ - std::vector idx(cloud.size()); - std::iota(idx.begin(), idx.end(), 0); - std::sort(idx.begin(), idx.end(), - [&](int a, int b){ return cloud[a].z < cloud[b].z; }); - - std::vector> bands; - if (idx.empty()) return bands; - - std::vector cur; - cur.reserve(256); - cur.push_back(idx[0]); - - for (size_t t = 1; t < idx.size(); ++t) { - const int i_prev = idx[t-1]; - const int i_curr = idx[t]; - const float dz = cloud[i_curr].z - cloud[i_prev].z; - - if (dz > gap_z) { - if ((int)cur.size() >= min_points_per_band) bands.push_back(std::move(cur)); - cur.clear(); - } - cur.push_back(i_curr); - } - if ((int)cur.size() >= min_points_per_band) bands.push_back(std::move(cur)); - - return bands; -} - - - -// ---- from_points: Delaunay por banda + filtros 3D (slope, max_dz, neighbor_radius, etc.) - -navmap::NavMap from_points( - const pcl::PointCloud & input_points, - navmap_ros_interfaces::msg::NavMap & out_msg, - BuildParams params) -{ - using std::vector; - - out_msg = navmap_ros_interfaces::msg::NavMap(); - if (input_points.empty() || input_points.size() < 3) { - return navmap::NavMap(); - } - - // 1) Downsample que NO colapsa plantas (promedia Z por voxel XY) - pcl::PointCloud cloud; - if (params.resolution > 0.0f) { - cloud = downsample_voxelize_avgZ_layered(input_points, params.resolution, - (params.max_dz > 0.0f ? params.max_dz : 0.30f)); - } else { - cloud = input_points; - } - if (cloud.size() < 3) { - return navmap::NavMap(); - } - - // 2) Bandas por SALTOS en Z - const float gap_z = (params.max_dz > 0.0f) - ? std::max(1.0f, 3.0f * params.max_dz) - : 1.0f; - auto bands = split_points_by_z_gaps(cloud, gap_z, /*min_points_per_band=*/3); - if (bands.empty()) { - return navmap::NavMap(); - } - - // 3) Delaunay 2D (XY) por banda + filtros 3D - struct Pt2 { double x, y; int idx3d; }; - struct Tri2 { int a, b, c; }; - - auto orient2d = [](const Pt2 & a, const Pt2 & b, const Pt2 & c) -> double { - return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x); - }; - auto in_circumcircle = [&](const Pt2 & p, const Pt2 & A, const Pt2 & B, const Pt2 & C) -> bool { - const double ax = A.x - p.x, ay = A.y - p.y; - const double bx = B.x - p.x, by = B.y - p.y; - const double cx = C.x - p.x, cy = C.y - p.y; - const double det = (ax*ax + ay*ay) * (bx*cy - by*cx) - - (bx*bx + by*by) * (ax*cy - ay*cx) - + (cx*cx + cy*cy) * (ax*by - ay*bx); - const double o = orient2d(A, B, C); - return (o > 0.0) ? (det > 0.0) : (det < 0.0); - }; - - vector all_triangles; - all_triangles.reserve(static_cast(cloud.size()) * 2); - - vector> band_tri_ranges; // [offset, count] por banda - - const float cos_max_slope = - std::cos(params.max_slope_deg * static_cast(M_PI) / 180.0f); - const float min_area = std::max(params.min_area, 1e-9f); - const float max_edge = - (params.max_edge_len > 0.0f) ? params.max_edge_len - : std::numeric_limits::infinity(); - const float max_dz = - (params.max_dz > 0.0f) ? params.max_dz - : std::numeric_limits::infinity(); - const float neigh_xy = - (params.neighbor_radius > 0.0f) ? params.neighbor_radius - : std::numeric_limits::infinity(); - - auto edge_len3D = [&](int i, int j) -> float { - const auto & A = cloud[i]; - const auto & B = cloud[j]; - const float dx = A.x - B.x, dy = A.y - B.y, dz = A.z - B.z; - return std::sqrt(dx*dx + dy*dy + dz*dz); - }; - auto distXY = [&](const pcl::PointXYZ & P, const pcl::PointXYZ & Q) -> float { - const float dx = P.x - Q.x, dy = P.y - Q.y; - return std::sqrt(dx*dx + dy*dy); - }; - - for (const auto & band : bands) { - if (band.size() < 3) { - band_tri_ranges.emplace_back(all_triangles.size(), 0); - continue; - } - - // Puntos 2D de la banda - vector vs; vs.reserve(band.size() + 3); - double minx = 1e300, miny = 1e300, maxx = -1e300, maxy = -1e300; - for (int i3d : band) { - const auto & p = cloud[i3d]; - vs.push_back({static_cast(p.x), static_cast(p.y), i3d}); - minx = std::min(minx, (double)p.x); - miny = std::min(miny, (double)p.y); - maxx = std::max(maxx, (double)p.x); - maxy = std::max(maxy, (double)p.y); - } - - // Super-triángulo - const double dx = maxx - minx, dy = maxy - miny, d = std::max(dx, dy); - const int iS1 = (int)vs.size(); vs.push_back({minx - 10*d, miny - d, -1}); - const int iS2 = (int)vs.size(); vs.push_back({minx + 0.5*d, maxy + 10*d, -2}); - const int iS3 = (int)vs.size(); vs.push_back({maxx + 10*d, miny - d, -3}); - - vector tris2; tris2.push_back({iS1, iS2, iS3}); - - // Bowyer–Watson - for (int pi = 0; pi < (int)band.size(); ++pi) { - const Pt2 Pp = vs[pi]; - vector bad; bad.reserve(tris2.size()); - for (int t = 0; t < (int)tris2.size(); ++t) { - const auto & T = tris2[t]; - if (in_circumcircle(Pp, vs[T.a], vs[T.b], vs[T.c])) bad.push_back(t); - } - - // borde de cavidad - struct EdgeBW { int a, b; }; - vector poly; poly.reserve(bad.size() * 3); - std::vector removed(tris2.size(), 0); - for (int bi : bad) removed[bi] = 1; - - auto add_edge = [&](int a, int b){ - for (auto it = poly.begin(); it != poly.end(); ++it) { - if (it->a == b && it->b == a) { poly.erase(it); return; } - } - poly.push_back({a, b}); - }; +// ----------------- Keep only largest surfaces ----------------- - for (int t = 0; t < (int)tris2.size(); ++t) { - if (!removed[t]) continue; - const auto & T = tris2[t]; - add_edge(T.a, T.b); - add_edge(T.b, T.c); - add_edge(T.c, T.a); - } - - vector keep; keep.reserve(tris2.size()); - for (int t = 0; t < (int)tris2.size(); ++t) { - if (!removed[t]) keep.push_back(tris2[t]); - } - tris2.swap(keep); - - for (const auto & e : poly) tris2.push_back({e.a, e.b, pi}); - } - - // Triángulos finales (sin super-triángulo) - vector final_tris2; final_tris2.reserve(tris2.size()); - for (const auto & T : tris2) { - if (T.a >= iS1 && T.a <= iS3) continue; - if (T.b >= iS1 && T.b <= iS3) continue; - if (T.c >= iS1 && T.c <= iS3) continue; - final_tris2.push_back(T); - } - - // Filtros 3D - const size_t tri_off = all_triangles.size(); - size_t accepted = 0; - - for (const auto & T : final_tris2) { - const int ia = vs[T.a].idx3d; - const int ib = vs[T.b].idx3d; - const int ic = vs[T.c].idx3d; - - const Eigen::Vector3f A(cloud[ia].x, cloud[ia].y, cloud[ia].z); - const Eigen::Vector3f B(cloud[ib].x, cloud[ib].y, cloud[ib].z); - const Eigen::Vector3f C(cloud[ic].x, cloud[ic].y, cloud[ic].z); - - const float area = tri_area(A, B, C); - if (area < min_area) continue; - - const float lAB = edge_len3D(ia, ib); - const float lBC = edge_len3D(ib, ic); - const float lCA = edge_len3D(ic, ia); - if (lAB > max_edge || lBC > max_edge || lCA > max_edge) continue; - - const float dzAB = std::fabs(A.z() - B.z()); - const float dzBC = std::fabs(B.z() - C.z()); - const float dzCA = std::fabs(C.z() - A.z()); - if (std::max({dzAB, dzBC, dzCA}) > max_dz) continue; - - if (neigh_xy < std::numeric_limits::infinity()) { - const auto & A3 = cloud[ia]; - const auto & B3 = cloud[ib]; - const auto & C3 = cloud[ic]; - if (distXY(A3, B3) > neigh_xy || - distXY(B3, C3) > neigh_xy || - distXY(C3, A3) > neigh_xy) { - continue; - } - } - - Eigen::Vector3f n = (B - A).cross(C - A); - const float nn = n.norm(); - if (nn < 1e-6f) continue; - n /= nn; - if (n.dot(Eigen::Vector3f::UnitZ()) < cos_max_slope) continue; - - all_triangles.emplace_back(ia, ib, ic); - ++accepted; - } - - band_tri_ranges.emplace_back(tri_off, accepted); - } - - if (all_triangles.empty()) { - return navmap::NavMap(); - } - - // 4) Construcción del NavMap - const std::string frame_id = "map"; - navmap::NavMap core; - if (!build_navmap_from_mesh(cloud, all_triangles, frame_id, out_msg, &core)) { - out_msg = navmap_ros_interfaces::msg::NavMap(); - return navmap::NavMap(); - } - - // Reclasifica surfaces por banda asumiendo que se preserva el orden. - out_msg.surfaces.clear(); - for (const auto & oc : band_tri_ranges) { - const size_t off = oc.first, cnt = oc.second; - if (cnt == 0) continue; - navmap_ros_interfaces::msg::NavMapSurface s; - s.frame_id = frame_id; - s.navcels.resize(cnt); - for (size_t k = 0; k < cnt; ++k) { - s.navcels[k] = static_cast(off + k); - } - out_msg.surfaces.push_back(std::move(s)); - } - - navmap::NavMap nm = from_msg(out_msg); - return nm; -} - -// Mantiene solo las 'max_surfaces' superficies más grandes (por nº de navcels), en orden descendente. -// max_surfaces <= 0 => no hace nada. static void keep_top_surfaces_by_size(navmap_ros_interfaces::msg::NavMap & msg, int max_surfaces) { - if (max_surfaces <= 0) return; - if (msg.surfaces.size() <= static_cast(max_surfaces)) return; + if (max_surfaces <= 0) {return;} + if (msg.surfaces.size() <= static_cast(max_surfaces)) {return;} - // Índices 0..S-1 std::vector ids(msg.surfaces.size()); std::iota(ids.begin(), ids.end(), 0); - // Ordenar por tamaño de superficie (desc) std::sort(ids.begin(), ids.end(), [&](size_t a, size_t b){ - return msg.surfaces[a].navcels.size() > msg.surfaces[b].navcels.size(); + return msg.surfaces[a].navcels.size() > msg.surfaces[b].navcels.size(); }); - // Construir nueva lista con las N primeras std::vector kept; kept.reserve(static_cast(max_surfaces)); for (int i = 0; i < max_surfaces; ++i) { @@ -1562,34 +833,28 @@ static void keep_top_surfaces_by_size(navmap_ros_interfaces::msg::NavMap & msg, msg.surfaces.swap(kept); } +// ----------------- Rebuild surfaces by triangle connectivity ----------------- -// --- Reconstrucción de surfaces por conectividad de triángulos --- -// Agrupa triángulos en componentes conectadas por ARISTAS compartidas. -// No aplica ningún límite: conserva todas las superficies, ordenadas por tamaño (desc). -// --- Reconstrucción de surfaces por conectividad (rápida, limitada a triángulos ya conservados) --- static void rebuild_surfaces_by_connectivity(navmap_ros_interfaces::msg::NavMap & msg) { const size_t ntris = msg.navcels_v0.size(); - if (ntris == 0) { msg.surfaces.clear(); return; } + if (ntris == 0) {msg.surfaces.clear(); return;} - // 1) Recolecta el conjunto de triángulos que realmente queremos considerar (los que están en surfaces actuales). - // Si no hay surfaces (caso raro), considera todos. + // Triangles to consider: those referenced by current surfaces, or all if none. std::vector keep(ntris, 0); size_t kept_count = 0; if (!msg.surfaces.empty()) { for (const auto & s : msg.surfaces) { for (uint32_t cid : s.navcels) { - if (cid < ntris && !keep[cid]) { keep[cid] = 1; ++kept_count; } + if (cid < ntris && !keep[cid]) {keep[cid] = 1; ++kept_count;} } } } else { - // Fallback: sin filtro kept_count = ntris; std::fill(keep.begin(), keep.end(), 1); } - if (kept_count == 0) { msg.surfaces.clear(); return; } + if (kept_count == 0) {msg.surfaces.clear(); return;} - // 2) Mapea tri_global -> tri_local (0..K-1) y lista inversa. std::vector glob2loc(ntris, UINT32_MAX); std::vector loc2glob; loc2glob.reserve(kept_count); @@ -1601,35 +866,38 @@ static void rebuild_surfaces_by_connectivity(navmap_ros_interfaces::msg::NavMap } const uint32_t K = static_cast(loc2glob.size()); - // 3) Estructura Union-Find (DSU) + // Union-Find std::vector parent(K), rankv(K, 0); std::iota(parent.begin(), parent.end(), 0); auto findp = [&](uint32_t x) { - while (parent[x] != x) { parent[x] = parent[parent[x]]; x = parent[x]; } - return x; - }; + while (parent[x] != x) {parent[x] = parent[parent[x]]; x = parent[x];} + return x; + }; auto unite = [&](uint32_t a, uint32_t b) { - a = findp(a); b = findp(b); - if (a == b) return; - if (rankv[a] < rankv[b]) std::swap(a, b); - parent[b] = a; - if (rankv[a] == rankv[b]) ++rankv[a]; - }; + a = findp(a); b = findp(b); + if (a == b) {return;} + if (rankv[a] < rankv[b]) {std::swap(a, b);} + parent[b] = a; + if (rankv[a] == rankv[b]) {++rankv[a];} + }; - // 4) Mapa de arista -> tri_local (usa clave 64-bit con (min(v), max(v))) - struct U64Hash { - size_t operator()(const uint64_t &k) const noexcept { return static_cast(k ^ (k >> 33)); } + struct U64Hash + { + size_t operator()(const uint64_t & k) const noexcept + { + return static_cast(k ^ (k >> 33)); + } }; auto edge_key = [](uint32_t a, uint32_t b) -> uint64_t { - const uint64_t mn = (a < b) ? a : b; - const uint64_t mx = (a < b) ? b : a; - return (mn << 32) | mx; - }; + const uint64_t mn = (a < b) ? a : b; + const uint64_t mx = (a < b) ? b : a; + return (mn << 32) | mx; + }; std::unordered_map edge2tri; edge2tri.reserve(static_cast(K) * 3u); - // 5) Una pasada: para cada tri_local, intenta unir con el tri que ya tenía la misma arista + // Single pass: join triangles sharing an edge for (uint32_t tl = 0; tl < K; ++tl) { const uint32_t tg = loc2glob[tl]; const uint32_t v0 = msg.navcels_v0[tg]; @@ -1650,67 +918,48 @@ static void rebuild_surfaces_by_connectivity(navmap_ros_interfaces::msg::NavMap } } - // 6) Recolecta componentes: parent_local -> lista de tri_global - std::unordered_map> comp; // root -> tri_global list + // Collect components + std::unordered_map> comp; comp.reserve(K); for (uint32_t tl = 0; tl < K; ++tl) { uint32_t r = findp(tl); comp[r].push_back(loc2glob[tl]); } - // 7) Pasa a vector y ordénalas por tamaño (desc) std::vector> comps; comps.reserve(comp.size()); - for (auto & kv : comp) comps.push_back(std::move(kv.second)); + for (auto & kv : comp) { + comps.push_back(std::move(kv.second)); + } std::sort(comps.begin(), comps.end(), - [](const auto& A, const auto& B){ return A.size() > B.size(); }); + [](const auto & A, const auto & B){return A.size() > B.size();}); - // 8) Emite las nuevas surfaces (sin recorte aquí; ya hiciste keep_top_surfaces_by_size) const std::string fid = msg.header.frame_id; std::vector out; out.reserve(comps.size()); for (auto & C : comps) { navmap_ros_interfaces::msg::NavMapSurface s; s.frame_id = fid; - s.navcels.assign(C.begin(), C.end()); // índices globales de triángulo + s.navcels.assign(C.begin(), C.end()); out.push_back(std::move(s)); } msg.surfaces.swap(out); } -// ----------------- Variante Local Grow (con trazas + aceleraciones) ----------------- +// ----------------- Local Grow builder (renamed: from_points) ----------------- -navmap::NavMap from_points_local_grow( +navmap::NavMap from_points( const pcl::PointCloud & input_points, navmap_ros_interfaces::msg::NavMap & out_msg, BuildParams P) { - using std::cerr; - out_msg = navmap_ros_interfaces::msg::NavMap(); - auto fmtf = [](float v){ std::ostringstream ss; ss< cloud; if (P.resolution > 0.0f) { const float dz_merge = (P.max_dz > 0.0f ? P.max_dz : 0.30f); @@ -1718,186 +967,178 @@ navmap::NavMap from_points_local_grow( } else { cloud = input_points; } - cerr << "[local_grow] cloud after downsample: " << cloud.size() << " points\n"; if (cloud.size() < 3) { - cerr << "[local_grow] Less than 3 points after downsample. RETURN.\n"; return navmap::NavMap(); } - // KdTree + // Neighborhood structure pcl::KdTreeFLANN kdtree; kdtree.setInputCloud(cloud.makeShared()); - cerr << "[local_grow] KdTree built.\n"; const int N = static_cast(cloud.size()); std::vector used_vertex(N, 0); std::vector triangles; triangles.reserve(N * 2); - // Seeds en orden de Z ascendente + // Seeds in ascending Z std::vector order(N); std::iota(order.begin(), order.end(), 0); std::sort(order.begin(), order.end(), - [&](int a, int b){ return cloud[a].z < cloud[b].z; }); - cerr << "[local_grow] seeds (ordered by Z): " << order.size() << "\n"; + [&](int a, int b){return cloud[a].z < cloud[b].z;}); - // --- Estado global para toda la construcción --- - std::unordered_set tri_set_global; - std::unordered_set edge_set_global; // requerido por try_add_triangle + // Global state + std::unordered_set tri_set_global; + std::unordered_set edge_set_global; std::unordered_map edge_use_count; - auto inc_edge = [&](const EdgeKey& e) -> uint16_t { - auto it = edge_use_count.find(e); - if (it == edge_use_count.end()) { edge_use_count.emplace(e, 1); return 1; } - return ++(it->second); - }; - auto edge_count = [&](const EdgeKey& e) -> uint16_t { - auto it = edge_use_count.find(e); - return (it == edge_use_count.end()) ? 0 : it->second; - }; + auto inc_edge = [&](const EdgeKey & e) -> uint16_t { + auto it = edge_use_count.find(e); + if (it == edge_use_count.end()) {edge_use_count.emplace(e, 1); return 1;} + return ++(it->second); + }; + auto edge_count = [&](const EdgeKey & e) -> uint16_t { + auto it = edge_use_count.find(e); + return (it == edge_use_count.end()) ? 0 : it->second; + }; std::queue frontier; std::unordered_set in_frontier; - auto push_frontier_if_border = [&](const EdgeKey& e) { - if (edge_count(e) == 1) { - if (in_frontier.insert(e).second) { - frontier.emplace(e); + auto push_frontier_if_border = [&](const EdgeKey & e) { + if (edge_count(e) == 1) { + if (in_frontier.insert(e).second) { + frontier.emplace(e); + } } - } - }; - - // Contadores (trazas) - struct RejectCounts { size_t edge_len=0,dz=0,xy=0,dup=0,geom=0,zwin_seed=0,zwin_bfs=0; }; - RejectCounts global_rej{}; - size_t global_tri_fan_attempts = 0, global_tri_fan_accept = 0; - size_t global_tri_bfs_attempts = 0, global_tri_bfs_accept = 0; + }; - auto dist3f = [](const pcl::PointXYZ &A, const pcl::PointXYZ &B){ - const float dx=A.x-B.x, dy=A.y-B.y, dz=A.z-B.z; return std::sqrt(dx*dx+dy*dy+dz*dz); - }; - auto distXY = [](const pcl::PointXYZ &A, const pcl::PointXYZ &B){ - const float dx=A.x-B.x, dy=A.y-B.y; return std::sqrt(dx*dx+dy*dy); - }; + auto dist3f = [](const pcl::PointXYZ & A, const pcl::PointXYZ & B){ + const float dx = A.x - B.x, dy = A.y - B.y, dz = A.z - B.z; + return std::sqrt(dx * dx + dy * dy + dz * dz); + }; + auto distXY = [](const pcl::PointXYZ & A, const pcl::PointXYZ & B){ + const float dx = A.x - B.x, dy = A.y - B.y; return std::sqrt(dx * dx + dy * dy); + }; enum class Phase { FAN, BFS }; - auto precheck = [&](int i, int j, int k, Phase phase, RejectCounts & rej, bool &dup_out)->bool { - TriKey tk = make_tri(i,j,k); - if (tri_set_global.find(tk) != tri_set_global.end()) { rej.dup++; dup_out = true; return false; } - dup_out = false; + struct RejectCounts { size_t edge_len = 0, dz = 0, xy = 0, dup = 0, geom = 0, zwin_seed = 0, + zwin_bfs = 0; }; + auto precheck = [&](int i, int j, int k, Phase phase, RejectCounts & rej, bool & dup_out)->bool { + TriKey tk = make_tri(i, j, k); + if (tri_set_global.find(tk) != tri_set_global.end()) { + rej.dup++; dup_out = true; return false; + } + dup_out = false; - const auto &A = cloud[i], &B = cloud[j], &C = cloud[k]; + const auto & A = cloud[i], & B = cloud[j], & C = cloud[k]; - if (P.max_edge_len > 0.0f) { - const float lAB = dist3f(A,B), lBC = dist3f(B,C), lCA = dist3f(C,A); - if (lAB > P.max_edge_len || lBC > P.max_edge_len || lCA > P.max_edge_len) { rej.edge_len++; return false; } - } + if (P.max_edge_len > 0.0f) { + const float lAB = dist3f(A, B), lBC = dist3f(B, C), lCA = dist3f(C, A); + if (lAB > P.max_edge_len || lBC > P.max_edge_len || lCA > P.max_edge_len) { + rej.edge_len++; return false; + } + } - if (P.max_dz > 0.0f) { - const float dzAB = std::fabs(A.z - B.z); - const float dzBC = std::fabs(B.z - C.z); - const float dzCA = std::fabs(C.z - A.z); - if (std::max({dzAB, dzBC, dzCA}) > P.max_dz) { rej.dz++; return false; } - } + if (P.max_dz > 0.0f) { + const float dzAB = std::fabs(A.z - B.z); + const float dzBC = std::fabs(B.z - C.z); + const float dzCA = std::fabs(C.z - A.z); + if (std::max({dzAB, dzBC, dzCA}) > P.max_dz) {rej.dz++; return false;} + } - if (P.neighbor_radius > 0.0f) { - const float r = P.neighbor_radius; - const float dABxy = distXY(A,B), dBCxy = distXY(B,C), dCAxy = distXY(C,A); - bool bad_xy = (phase == Phase::FAN) ? (dABxy > r || dCAxy > r) : (dABxy > r || dBCxy > r || dCAxy > r); - if (bad_xy) { rej.xy++; return false; } - } + if (P.neighbor_radius > 0.0f) { + const float r = P.neighbor_radius; + const float dABxy = distXY(A, B), dBCxy = distXY(B, C), dCAxy = distXY(C, A); + bool bad_xy = (phase == Phase::FAN) ? (dABxy > r || dCAxy > r) : (dABxy > r || dBCxy > r || + dCAxy > r); + if (bad_xy) {rej.xy++; return false;} + } - return true; - }; + return true; + }; - // Ventanas Z: algo más holgadas hacia arriba para no cortar rampas + // Z windows around the expanding boundary (keeps ramps but avoids jumps) const float z_window_seed = std::max(P.max_dz, 0.35f); - const float z_window_bfs = std::max(P.max_dz, 0.35f); - const float z_window_down = 0.10f; // estricta hacia abajo en BFS std::vector> surf_tri_ranges; // [offset, count] - size_t seed_idx_counter = 0; - for (int seed_idx : order) { - ++seed_idx_counter; - if (used_vertex[seed_idx]) continue; + RejectCounts global_rej{}; // still used to track precheck stats (no logging) - const auto &S = cloud[seed_idx]; - cerr << "\n[local_grow] -- Seed #" << seed_idx_counter - << " (idx=" << seed_idx << ") pos=(" - << fmtf(S.x) << "," << fmtf(S.y) << "," << fmtf(S.z) << ")\n"; + for (int seed_idx : order) { + if (used_vertex[seed_idx]) {continue;} - // Vecinos de la semilla + // Neighborhood of the seed std::vector neigh_seed; { std::vector inds; std::vector dists; if (P.neighbor_radius > 0.0f) { - const int found = kdtree.radiusSearch(S, P.neighbor_radius, inds, dists); - cerr << "[local_grow] radiusSearch: found=" << found << " (including self)\n"; - if (found > 0) for (int id : inds) if (id != seed_idx) neigh_seed.push_back(id); + if (kdtree.radiusSearch(cloud[seed_idx], P.neighbor_radius, inds, dists) > 0) { + for (int id : inds) { + if (id != seed_idx) { + neigh_seed.push_back(id); + } + } + } } else { const int K = std::max(8, P.k_neighbors); - const int found = kdtree.nearestKSearch(S, K, inds, dists); - cerr << "[local_grow] kNN: K=" << K << " found=" << found << " (including self)\n"; - if (found > 0) for (int id : inds) if (id != seed_idx) neigh_seed.push_back(id); + if (kdtree.nearestKSearch(cloud[seed_idx], K, inds, dists) > 0) { + for (int id : inds) { + if (id != seed_idx) { + neigh_seed.push_back(id); + } + } + } } } - cerr << "[local_grow] neigh_seed (raw, excl. self): " << neigh_seed.size() << "\n"; - // Filtros rápidos + // Quick filters if (P.max_edge_len > 0.0f) { - size_t before = neigh_seed.size(); neigh_seed.erase(std::remove_if(neigh_seed.begin(), neigh_seed.end(), - [&](int j){ const auto &Q = cloud[j]; if (!pcl::isFinite(Q)) return true; return dist3f(S,Q) > P.max_edge_len; }), + [&](int j){ + const auto & Q = cloud[j]; if (!pcl::isFinite(Q)) { + return true; + } + return dist3f(cloud[seed_idx], Q) > P.max_edge_len; + }), neigh_seed.end()); - cerr << "[local_grow] neigh_seed after edge_len filter: " << neigh_seed.size() - << " (removed " << (before - neigh_seed.size()) << ")\n"; } { - size_t before = neigh_seed.size(); neigh_seed.erase(std::remove_if(neigh_seed.begin(), neigh_seed.end(), - [&](int j){ return std::fabs(cloud[j].z - S.z) > z_window_seed; }), + [&](int j){return std::fabs(cloud[j].z - cloud[seed_idx].z) > z_window_seed;}), neigh_seed.end()); - cerr << "[local_grow] neigh_seed after Z-window (±"<< fmtf(z_window_seed) << "): " - << neigh_seed.size() << " (removed " << (before - neigh_seed.size()) << ")\n"; - global_rej.zwin_seed += (before - neigh_seed.size()); } if (neigh_seed.size() < 2) { used_vertex[seed_idx] = 1; - cerr << "[local_grow] Insufficient neighbors (<2) after filtering. Seed skipped.\n"; continue; } - // Orden angular en XY + // Angular sort in XY { const auto & Cc = cloud[seed_idx]; std::sort(neigh_seed.begin(), neigh_seed.end(), [&](int a, int b){ - const float ax = cloud[a].x - Cc.x, ay = cloud[a].y - Cc.y; - const float bx = cloud[b].x - Cc.x, by = cloud[b].y - Cc.y; - return std::atan2(ay, ax) < std::atan2(by, bx); + const float ax = cloud[a].x - Cc.x, ay = cloud[a].y - Cc.y; + const float bx = cloud[b].x - Cc.x, by = cloud[b].y - Cc.y; + return std::atan2(ay, ax) < std::atan2(by, bx); }); } const size_t tri_off = triangles.size(); RejectCounts comp_rej{}; - size_t comp_fan_attempts = 0, comp_fan_accept = 0; - size_t comp_bfs_attempts = 0, comp_bfs_accept = 0; + size_t comp_fan_accept = 0; + size_t comp_bfs_accept = 0; - // --- FAN inicial --- - cerr << "[local_grow] Fan: pairs=" << (neigh_seed.size() > 0 ? (neigh_seed.size()-1) : 0) << "\n"; + // Initial fan for (size_t t = 0; t + 1 < neigh_seed.size(); ++t) { const int j = neigh_seed[t]; - const int k = neigh_seed[t+1]; - ++comp_fan_attempts; ++global_tri_fan_attempts; - - bool dup=false; - if (!precheck(seed_idx, j, k, Phase::FAN, comp_rej, dup)) continue; + const int k = neigh_seed[t + 1]; + bool dup = false; + if (!precheck(seed_idx, j, k, Phase::FAN, comp_rej, dup)) {continue;} if (try_add_triangle(seed_idx, j, k, cloud, P, tri_set_global, edge_set_global, triangles)) { - ++comp_fan_accept; ++global_tri_fan_accept; + ++comp_fan_accept; - // Actualiza conteos y frontera + // Update counts and frontier const EdgeKey e0 = make_edge(seed_idx, j); const EdgeKey e1 = make_edge(j, k); const EdgeKey e2 = make_edge(k, seed_idx); @@ -1912,11 +1153,12 @@ navmap::NavMap from_points_local_grow( if (neigh_seed.size() >= 3) { const int j = neigh_seed.back(); const int k = neigh_seed.front(); - ++comp_fan_attempts; ++global_tri_fan_attempts; - bool dup=false; + bool dup = false; if (precheck(seed_idx, j, k, Phase::FAN, comp_rej, dup)) { - if (try_add_triangle(seed_idx, j, k, cloud, P, tri_set_global, edge_set_global, triangles)) { - ++comp_fan_accept; ++global_tri_fan_accept; + if (try_add_triangle(seed_idx, j, k, cloud, P, tri_set_global, edge_set_global, + triangles)) + { + ++comp_fan_accept; const EdgeKey e0 = make_edge(seed_idx, j); const EdgeKey e1 = make_edge(j, k); const EdgeKey e2 = make_edge(k, seed_idx); @@ -1930,81 +1172,78 @@ navmap::NavMap from_points_local_grow( } } - cerr << "[local_grow] Fan result: attempts=" << comp_fan_attempts - << " accepted=" << comp_fan_accept - << " tri_now=" << (triangles.size() - tri_off) << "\n"; if (triangles.size() == tri_off) { used_vertex[seed_idx] = 1; - cerr << "[local_grow] No triangle from fan. Seed skipped.\n"; global_rej.edge_len += comp_rej.edge_len; - global_rej.dz += comp_rej.dz; - global_rej.xy += comp_rej.xy; - global_rej.dup += comp_rej.dup; - global_rej.geom += comp_rej.geom; + global_rej.dz += comp_rej.dz; + global_rej.xy += comp_rej.xy; + global_rej.dup += comp_rej.dup; + global_rej.geom += comp_rej.geom; global_rej.zwin_seed += comp_rej.zwin_seed; - global_rej.zwin_bfs += comp_rej.zwin_bfs; + global_rej.zwin_bfs += comp_rej.zwin_bfs; continue; } - // Marca vértices usados de los nuevos triángulos + // Mark vertices used by the new triangles for (size_t u = tri_off; u < triangles.size(); ++u) { used_vertex[triangles[u][0]] = 1; used_vertex[triangles[u][1]] = 1; used_vertex[triangles[u][2]] = 1; } - cerr << "[local_grow] Frontier initialized: " << in_frontier.size() << " edges\n"; - - // --- BFS sobre aristas de frontera --- - size_t bfs_iters = 0; + // BFS expansion over border edges while (!frontier.empty()) { EdgeKey e = frontier.front(); frontier.pop(); in_frontier.erase(e); - ++bfs_iters; - // Sólo expandimos bordes actuales (conteo==1) - if (edge_count(e) != 1) continue; + // Expand only current border edges (count == 1) + if (edge_count(e) != 1) {continue;} const float z_mid = 0.5f * (cloud[e.a].z + cloud[e.b].z); - // Vecinos de e.a y comprobación rápida con e.b (radio y ventana Z) + // Neighbors of e.a (filtered by radius and Z window vs e.b) std::vector neigh_a; { std::vector inds; std::vector dists; if (P.neighbor_radius > 0.0f) { if (kdtree.radiusSearch(cloud[e.a], P.neighbor_radius, inds, dists) > 0) { - for (int id : inds) if (id != e.a) neigh_a.push_back(id); + for (int id : inds) { + if (id != e.a) { + neigh_a.push_back(id); + } + } } } else { const int K = std::max(8, P.k_neighbors); if (kdtree.nearestKSearch(cloud[e.a], K, inds, dists) > 0) { - for (int id : inds) if (id != e.a) neigh_a.push_back(id); + for (int id : inds) { + if (id != e.a) { + neigh_a.push_back(id); + } + } } } } for (int c : neigh_a) { - if (c == e.a || c == e.b) continue; + if (c == e.a || c == e.b) {continue;} if (P.neighbor_radius > 0.0f) { const float dx = cloud[c].x - cloud[e.b].x; const float dy = cloud[c].y - cloud[e.b].y; - if ((dx*dx + dy*dy) > P.neighbor_radius * P.neighbor_radius) continue; + if ((dx * dx + dy * dy) > P.neighbor_radius * P.neighbor_radius) {continue;} } - const float z_half = std::max(P.max_dz, 0.35f); // o calcula en función de res & slope + const float z_half = std::max(P.max_dz, 0.35f); const float dz = cloud[c].z - z_mid; - if (std::fabs(dz) > z_half) { ++global_rej.zwin_bfs; continue; } - - ++comp_bfs_attempts; ++global_tri_bfs_attempts; + if (std::fabs(dz) > z_half) {++global_rej.zwin_bfs; continue;} - bool dup=false; - if (!precheck(e.a, e.b, c, Phase::BFS, comp_rej, dup)) continue; + bool dup = false; + if (!precheck(e.a, e.b, c, Phase::BFS, comp_rej, dup)) {continue;} if (try_add_triangle(e.a, e.b, c, cloud, P, tri_set_global, edge_set_global, triangles)) { - ++comp_bfs_accept; ++global_tri_bfs_accept; + ++comp_bfs_accept; - // Actualiza conteos: el borde (a,b) deja de ser frontera (pasa a 2) const EdgeKey eab = make_edge(e.a, e.b); const EdgeKey eac = make_edge(e.a, c); const EdgeKey ecb = make_edge(c, e.b); @@ -2013,7 +1252,6 @@ navmap::NavMap from_points_local_grow( inc_edge(eac); inc_edge(ecb); - // Encola SOLO los nuevos bordes de contorno push_frontier_if_border(eac); push_frontier_if_border(ecb); @@ -2022,76 +1260,46 @@ navmap::NavMap from_points_local_grow( comp_rej.geom++; } } - - if (bfs_iters % 512 == 0) { - cerr << "[local_grow] BFS iters=" << bfs_iters - << " frontier_size~" << in_frontier.size() - << " tri_total=" << triangles.size() << "\n"; - } } - // Superficie final para esta semilla + // Component done const size_t tri_cnt = triangles.size() - tri_off; if (tri_cnt > 0) { surf_tri_ranges.emplace_back(tri_off, tri_cnt); - cerr << "[local_grow] Component done: triangles=" << tri_cnt - << " (fan acc=" << comp_fan_accept << "/" << comp_fan_attempts - << ", bfs acc=" << comp_bfs_accept << "/" << comp_bfs_attempts << ")\n"; - // Corte temprano por número máximo de superficies if (P.max_surfaces > 0 && - surf_tri_ranges.size() >= static_cast(P.max_surfaces)) { - cerr << "[local_grow] Reached max_surfaces (" << P.max_surfaces - << "). Early stop of seed loop.\n"; + surf_tri_ranges.size() >= static_cast(P.max_surfaces)) + { break; } } - // Acumular contadores globales + // Accumulate global counters (used only for internal accounting) global_rej.edge_len += comp_rej.edge_len; - global_rej.dz += comp_rej.dz; - global_rej.xy += comp_rej.xy; - global_rej.dup += comp_rej.dup; - global_rej.geom += comp_rej.geom; + global_rej.dz += comp_rej.dz; + global_rej.xy += comp_rej.xy; + global_rej.dup += comp_rej.dup; + global_rej.geom += comp_rej.geom; global_rej.zwin_seed += comp_rej.zwin_seed; - global_rej.zwin_bfs += comp_rej.zwin_bfs; + global_rej.zwin_bfs += comp_rej.zwin_bfs; } - // Resumen global - cerr << "\n[local_grow] ====== SUMMARY ======\n"; - cerr << "[local_grow] total triangles: " << triangles.size() << "\n"; - cerr << "[local_grow] components (before prune): " << surf_tri_ranges.size() << "\n"; - cerr << "[local_grow] Fan: attempts=" << global_tri_fan_attempts - << " accepted=" << global_tri_fan_accept << "\n"; - cerr << "[local_grow] BFS: attempts=" << global_tri_bfs_attempts - << " accepted=" << global_tri_bfs_accept << "\n"; - cerr << "[local_grow] Rejects: edge=" << global_rej.edge_len - << " dz=" << global_rej.dz - << " xy=" << global_rej.xy - << " dup=" << global_rej.dup - << " geom=" << global_rej.geom - << " zwin_seed=" << global_rej.zwin_seed - << " zwin_bfs=" << global_rej.zwin_bfs - << "\n"; - if (triangles.empty()) { - cerr << "[local_grow] No triangles produced. RETURN empty.\n"; return navmap::NavMap(); } - // Construcción NavMap + surfaces + // Build NavMap + surfaces const std::string frame_id = "map"; navmap_ros_interfaces::msg::NavMap msg_tmp; navmap::NavMap core; if (!build_navmap_from_mesh(cloud, triangles, frame_id, msg_tmp, &core)) { - cerr << "[local_grow] build_navmap_from_mesh FAILED. RETURN empty.\n"; return navmap::NavMap(); } - // Construye surfaces a partir de los rangos por componente + // Build surfaces from component ranges msg_tmp.surfaces.clear(); for (const auto & oc : surf_tri_ranges) { - if (oc.second == 0) continue; + if (oc.second == 0) {continue;} navmap_ros_interfaces::msg::NavMapSurface s; s.frame_id = frame_id; s.navcels.resize(oc.second); @@ -2101,18 +1309,13 @@ navmap::NavMap from_points_local_grow( msg_tmp.surfaces.push_back(std::move(s)); } - // Mantener solo top-N superficies (si procede) rebuild_surfaces_by_connectivity(msg_tmp); keep_top_surfaces_by_size(msg_tmp, P.max_surfaces); out_msg = std::move(msg_tmp); - cerr << "[local_grow] Surfaces emitted: " << out_msg.surfaces.size() << "\n"; - cerr << "[local_grow] ====== END from_points_local_grow ======\n\n"; return from_msg(out_msg); } - - // ----------------- ROS PointCloud2 entry ----------------- navmap::NavMap from_pointcloud2( @@ -2123,11 +1326,8 @@ navmap::NavMap from_pointcloud2( pcl::PointCloud input_points; pcl::fromROSMsg(pc2, input_points); - // MODO por defecto: Banded Delaunay - // return from_points(input_points, out_msg, params); - - // Para la variante local (con aceleraciones y trazas): - return from_points_local_grow(input_points, out_msg, params); + // Local grow variant (now named from_points) + return from_points(input_points, out_msg, params); } } // namespace navmap_ros From 12e04ca4e0cb3465ca49eef726b645180a9f1671 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 20 Oct 2025 22:37:22 +0200 Subject: [PATCH 6/7] Remove some comments MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_ros/src/navmap_ros/conversions.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index 96e166b..d04e0b6 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -1266,12 +1266,6 @@ navmap::NavMap from_points( const size_t tri_cnt = triangles.size() - tri_off; if (tri_cnt > 0) { surf_tri_ranges.emplace_back(tri_off, tri_cnt); - - if (P.max_surfaces > 0 && - surf_tri_ranges.size() >= static_cast(P.max_surfaces)) - { - break; - } } // Accumulate global counters (used only for internal accounting) @@ -1326,7 +1320,6 @@ navmap::NavMap from_pointcloud2( pcl::PointCloud input_points; pcl::fromROSMsg(pc2, input_points); - // Local grow variant (now named from_points) return from_points(input_points, out_msg, params); } From a4bc0be47072049fc450692704cdfcc620d13e99 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 20 Oct 2025 22:40:18 +0200 Subject: [PATCH 7/7] Remove unused field MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_ros/include/navmap_ros/conversions.hpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index 91d66b6..068f56f 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -184,9 +184,6 @@ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm); */ struct BuildParams { - /** @brief Seed position (world frame) used by region growing or initial search heuristics. */ - Eigen::Vector3f seed = {0.0, 0.0, 0.0}; - /** @brief Target in-plane sampling resolution (meters) used by voxelization or gridding. */ float resolution = 1.0; @@ -214,7 +211,8 @@ struct BuildParams /** @brief Minimum interior angle (degrees) to avoid sliver triangles. */ float min_angle_deg = 20.0f; // minimum interior angle (deg) to avoid sliver triangles - int max_surfaces = 0; // 0 ó <0 => sin límite. >0 => conservar solo las N mayores + /** @brief Maximun surfaces to keep, ordenred by size. */ + int max_surfaces = 0; // 0 ó <0 => no limits. >0 => Keep only N larger }; /**