diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index 2d35de0..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; @@ -213,6 +210,9 @@ 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 + + /** @brief Maximun surfaces to keep, ordenred by size. */ + int max_surfaces = 0; // 0 ó <0 => no limits. >0 => Keep only N larger }; /** diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index cc333b2..d04e0b6 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 @@ -57,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); @@ -174,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); @@ -203,7 +203,6 @@ navmap::NavMap from_msg(const NavMap & msg) } } - // Derived nm.rebuild_geometry_accels(); return nm; } @@ -279,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 ----------------- @@ -294,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)); @@ -309,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); @@ -331,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()); @@ -339,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;}; @@ -364,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; @@ -376,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()); @@ -391,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; @@ -405,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; @@ -416,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) { @@ -451,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)) { @@ -475,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()); @@ -485,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()); @@ -498,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; @@ -517,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()); @@ -526,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); } @@ -546,7 +532,6 @@ bool build_navmap_from_mesh( // ----------------- Surface from PC2 ----------------- - using Triangle = Eigen::Vector3i; static inline float sqr(float v) {return v * v;} @@ -569,21 +554,8 @@ 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 +// Keys to avoid duplicates (edges/triangles) struct EdgeKey { int a, b; @@ -629,63 +601,22 @@ 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, 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)); @@ -697,43 +628,122 @@ 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) +// ----------------- Downsampling (only what is used) ----------------- + +struct Voxel { - std::vector pts; - pts.reserve(nbrs.size() + 1); - pts.push_back(vpos); - for (auto & it : nbrs) { - pts.push_back(it.second); + int x, y, z; + bool operator==(const Voxel & o) const noexcept + { + return x == o.x && y == o.y && z == o.z; } +}; - 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); +struct VoxelHash +{ + std::size_t operator()(const Voxel & v) const noexcept + { + std::size_t h = 1469598103934665603ull; + auto mix = [&](int k) { + std::size_t x = static_cast(k); + h ^= x + 0x9e3779b97f4a7c15ull + (h << 6) + (h >> 2); + }; + mix(v.x); mix(v.y); mix(v.z); + return h; } +}; - std::sort(ang_idx.begin(), ang_idx.end(), - [](auto & a, auto & b){return a.first < b.first;}); +// 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) +{ + 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;} + + 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); - out_ordered.clear(); - out_ordered.reserve(ang_idx.size()); - for (auto & ai : ang_idx) { - out_ordered.push_back(ai.second); + 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;}); + + 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 (i,j,k) under the given constraints +// ----------------- Triangle acceptance filters ----------------- +// +// Includes: max edge length, max |Δz|, XY neighbor radius, min area, +// min internal angle, and slope limit via n·z. + inline bool try_add_triangle( int i, int j, int k, const pcl::PointCloud & cloud, @@ -750,40 +760,49 @@ 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 + 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;} + } 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); - if (slope > P.max_slope_deg) {return false;} + // Slope limit: n·z >= cos(max_slope) + 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 at each vertex + // 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; } - { - 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); - } + // 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)); @@ -792,569 +811,506 @@ inline bool try_add_triangle( return true; } -struct Voxel -{ - int x, y, z; - bool operator==(const Voxel & o) const noexcept - { - return x == o.x && y == o.y && z == o.z; - } -}; +// ----------------- Keep only largest surfaces ----------------- -struct VoxelHash +static void keep_top_surfaces_by_size(navmap_ros_interfaces::msg::NavMap & msg, int max_surfaces) { - std::size_t operator()(const Voxel & v) const noexcept - { - // Simple, stable hash - std::size_t h = 1469598103934665603ull; // FNV-1a offset - auto mix = [&](int k) { - std::size_t x = static_cast(k); - h ^= x + 0x9e3779b97f4a7c15ull + (h << 6) + (h >> 2); - }; - mix(v.x); mix(v.y); mix(v.z); - return h; - } -}; - -struct VoxelAccum -{ - float sum_x = 0.0f; - float sum_y = 0.0f; - float sum_z = 0.0f; - int count = 0; -}; - + if (max_surfaces <= 0) {return;} + if (msg.surfaces.size() <= static_cast(max_surfaces)) {return;} -// 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)) - }; -} + std::vector ids(msg.surfaces.size()); + std::iota(ids.begin(), ids.end(), 0); -// 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::sort(ids.begin(), ids.end(), [&](size_t a, size_t b){ + return msg.surfaces[a].navcels.size() > msg.surfaces[b].navcels.size(); + }); - 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); + 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)]])); } - return out; + msg.surfaces.swap(kept); } -// 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)) - }; -} +// ----------------- Rebuild surfaces by triangle connectivity ----------------- -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) +static void rebuild_surfaces_by_connectivity(navmap_ros_interfaces::msg::NavMap & msg) { - pcl::PointCloud output; - - if (input_points.empty() || !(resolution > 0.0f)) { - output.width = 0; output.height = 1; output.is_dense = true; - return output; + const size_t ntris = msg.navcels_v0.size(); + if (ntris == 0) {msg.surfaces.clear(); return;} + + // 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;} + } + } + } else { + kept_count = ntris; + std::fill(keep.begin(), keep.end(), 1); } + if (kept_count == 0) {msg.surfaces.clear(); return;} + + 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()); + + // 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; + }; + 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];} + }; - const float r = resolution; - 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); - - // Pass B: grid shifted by half resolution - auto centroids_b = voxel_pass_offset(input_points, r, half, half, half); - - // 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; + 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::vector clusters; - clusters.reserve(centroids_a.size()); + std::unordered_map edge2tri; + edge2tri.reserve(static_cast(K) * 3u); - std::unordered_map, VoxelHash> grid; - grid.reserve(centroids_a.size() + centroids_b.size()); + // 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]; + const uint32_t v1 = msg.navcels_v1[tg]; + const uint32_t v2 = msg.navcels_v2[tg]; - 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;} - } - } - } - } + const uint64_t e01 = edge_key(v0, v1); + const uint64_t e12 = edge_key(v1, v2); + const uint64_t e20 = edge_key(v2, v0); - 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; + for (const uint64_t e : {e01, e12, e20}) { + auto it = edge2tri.find(e); + if (it == edge2tri.end()) { + edge2tri.emplace(e, tl); } 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); + unite(tl, it->second); } - }; - - // Insert centroids from both passes - for (const auto & p : centroids_a) { - try_insert(p); + } } - for (const auto & p : centroids_b) { - try_insert(p); + + // 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]); } - // Emit one point per cluster - output.points.reserve(clusters.size()); - for (const auto & cl : clusters) { - output.points.push_back(accum_centroid(cl)); + 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();}); - output.width = static_cast(output.points.size()); - output.height = 1; - output.is_dense = true; - return output; + 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()); + out.push_back(std::move(s)); + } + msg.surfaces.swap(out); } -pcl::PointCloud -downsample_voxelize_avgZ( +// ----------------- Local Grow builder (renamed: from_points) ----------------- + +navmap::NavMap from_points( const pcl::PointCloud & input_points, - float resolution) + navmap_ros_interfaces::msg::NavMap & out_msg, + BuildParams P) { - pcl::PointCloud output; + out_msg = navmap_ros_interfaces::msg::NavMap(); - if (input_points.empty() || !(resolution > 0.0f)) { - output.width = 0; output.height = 1; output.is_dense = true; - return output; + if (input_points.size() < 3) { + return navmap::NavMap(); } - struct Accum - { - double sum_z{0.0}; - int count{0}; - }; + // Downsample that preserves floors: top-Z per Z-layer in each XY cell + pcl::PointCloud cloud; + if (P.resolution > 0.0f) { + 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; + } + if (cloud.size() < 3) { + return navmap::NavMap(); + } - std::unordered_map voxels; - voxels.reserve(input_points.size() / 2); + // Neighborhood structure + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(cloud.makeShared()); - // Accumulate Z per voxel - for (const auto & pt : input_points) { - if (!pcl::isFinite(pt)) {continue;} + const int N = static_cast(cloud.size()); + std::vector used_vertex(N, 0); + std::vector triangles; + triangles.reserve(N * 2); + + // 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;}); + + // 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; + }; - 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 + 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); + } + } + }; - Voxel v{vx, vy, vz}; - auto & acc = voxels[v]; - acc.sum_z += pt.z; - acc.count += 1; - } + 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); + }; - // Emit one point per voxel - output.points.reserve(voxels.size()); - for (const auto & kv : voxels) { - const Voxel & v = kv.first; - const Accum & acc = kv.second; + enum class Phase { FAN, BFS }; + 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; - // 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); + const auto & A = cloud[i], & B = cloud[j], & C = cloud[k]; - output.emplace_back(cx, cy, cz); - } + 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; + } + } - output.width = static_cast(output.points.size()); - output.height = 1; - output.is_dense = true; + 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;} + } - return output; -} + 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;} + } -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; - } + return true; + }; - pcl::KdTreeFLANN kdtree; - kdtree.setInputCloud(cloud.makeShared()); + // Z windows around the expanding boundary (keeps ramps but avoids jumps) + const float z_window_seed = std::max(P.max_dz, 0.35f); - std::queue frontier; - std::unordered_set seen; - frontier.push(seed_idx); - seen.insert(seed_idx); + std::vector> surf_tri_ranges; // [offset, count] - std::unordered_set tri_set; - std::unordered_set edge_set; + RejectCounts global_rej{}; // still used to track precheck stats (no logging) - std::vector nbr_indices; - std::vector nbr_dists; + for (int seed_idx : order) { + if (used_vertex[seed_idx]) {continue;} - while (!frontier.empty()) { - int v = frontier.front(); frontier.pop(); - const auto & V = cloud[v]; + // Neighborhood of the seed + std::vector neigh_seed; + { + std::vector inds; std::vector dists; + if (P.neighbor_radius > 0.0f) { + 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); + if (kdtree.nearestKSearch(cloud[seed_idx], K, inds, dists) > 0) { + for (int id : inds) { + if (id != seed_idx) { + neigh_seed.push_back(id); + } + } + } + } + } - if (!pcl::isFinite(V)) {continue;} + // Quick filters + if (P.max_edge_len > 0.0f) { + 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(cloud[seed_idx], Q) > P.max_edge_len; + }), + neigh_seed.end()); + } + { + neigh_seed.erase(std::remove_if(neigh_seed.begin(), neigh_seed.end(), + [&](int j){return std::fabs(cloud[j].z - cloud[seed_idx].z) > z_window_seed;}), + neigh_seed.end()); + } - // 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); - } else { - found = kdtree.nearestKSearch(V, P.k_neighbors, nbr_indices, nbr_dists); + if (neigh_seed.size() < 2) { + used_vertex[seed_idx] = 1; + continue; } - 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 (dist3(V, Pn) <= P.max_edge_len) { - candidates.emplace_back(idx, Eigen::Vector3f(Pn.x, Pn.y, Pn.z)); + + // 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 size_t tri_off = triangles.size(); + + RejectCounts comp_rej{}; + size_t comp_fan_accept = 0; + size_t comp_bfs_accept = 0; + + // 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]; + 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; + + // 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); + 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++; } } - 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 - // 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]; - int k = ordered[t + 1]; - - if (try_add_triangle(i, j, k, cloud, P, tri_set, edge_set, tris)) { - if (!seen.count(j)) {frontier.push(j); seen.insert(j);} - if (!seen.count(k)) {frontier.push(k); seen.insert(k);} + if (neigh_seed.size() >= 3) { + const int j = neigh_seed.back(); + const int k = neigh_seed.front(); + 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; + 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++; + } } } - // Optional fan closure - int j0 = ordered.front(), k0 = ordered.back(); - try_add_triangle(v, k0, j0, cloud, P, tri_set, edge_set, tris); - } - return tris; -} + if (triangles.size() == tri_off) { + used_vertex[seed_idx] = 1; + 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.zwin_seed += comp_rej.zwin_seed; + global_rej.zwin_bfs += comp_rej.zwin_bfs; + continue; + } + // 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; + } -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; - - if (input_points.empty() || input_points.size() < 3) { - out_msg = navmap_ros_interfaces::msg::NavMap(); - return navmap::NavMap(); - } + // BFS expansion over border edges + while (!frontier.empty()) { + EdgeKey e = frontier.front(); frontier.pop(); + in_frontier.erase(e); - pcl::PointCloud cloud; - if (params.resolution > 0.0f) { - cloud = downsample_voxelize_avgXYZ(input_points, params.resolution); - } else { - cloud = input_points; - } - if (cloud.size() < 3) { - out_msg = navmap_ros_interfaces::msg::NavMap(); - return navmap::NavMap(); - } + // Expand only current border edges (count == 1) + if (edge_count(e) != 1) {continue;} - struct Pt2 { double x, y; int idx3d; }; - struct Tri2 { int a, b, c; }; + const float z_mid = 0.5f * (cloud[e.a].z + cloud[e.b].z); - 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); - }; + // 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); + } + } + } + } 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); + } + } + } + } + } - 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); - }; + for (int c : neigh_a) { + if (c == e.a || c == e.b) {continue;} - 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}); - } + 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;} + } - 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); - } - } + 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;} - // 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); - } - }; + bool dup = false; + if (!precheck(e.a, e.b, c, Phase::BFS, comp_rej, dup)) {continue;} - std::unordered_map edge_count; - edge_count.reserve(bad.size() * 3); + if (try_add_triangle(e.a, e.b, c, cloud, P, tri_set_global, edge_set_global, triangles)) { + ++comp_bfs_accept; - 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]; - }; + 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); - 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); - } + inc_edge(eab); + inc_edge(eac); + inc_edge(ecb); - // 2.3) Remove bad triangles - vector kept; kept.reserve(tris.size()); - vector removed(tris.size(), 0); - for (int idx : bad) { - removed[idx] = 1; - } - for (int ti = 0; ti < static_cast(tris.size()); ++ti) { - if (!removed[ti]) {kept.push_back(tris[ti]);} + 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++; + } + } } - 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}); + + // Component done + const size_t tri_cnt = triangles.size() - tri_off; + if (tri_cnt > 0) { + surf_tri_ranges.emplace_back(tri_off, tri_cnt); } + + // 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.zwin_seed += comp_rej.zwin_seed; + global_rej.zwin_bfs += comp_rej.zwin_bfs; } - // 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())) - { - continue; - } - final_tris.push_back(t); + if (triangles.empty()) { + return navmap::NavMap(); } - // ---- 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); - }; + // 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)) { + return navmap::NavMap(); + } - 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(); - - vector triangles; - triangles.reserve(final_tris.size()); - - // Debug counters - size_t dropped_area = 0, dropped_edge = 0, dropped_dz = 0, dropped_slope = 0; - - 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; - - 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_area3D(A, B, C); - if (area < min_area) {++dropped_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) {++dropped_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) {++dropped_dz; continue;} - - const Eigen::Vector3f n = tri_normal(A, B, C); - if (n.dot(Eigen::Vector3f::UnitZ()) < cos_max_slope) {++dropped_slope; continue;} - - triangles.emplace_back(ia, ib, ic); + // Build surfaces from component ranges + 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)); } - // ---- 4) Emit message + core -------------------------------------------- - 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; + rebuild_surfaces_by_connectivity(msg_tmp); + keep_top_surfaces_by_size(msg_tmp, P.max_surfaces); + + out_msg = std::move(msg_tmp); + return from_msg(out_msg); } +// ----------------- ROS PointCloud2 entry ----------------- navmap::NavMap from_pointcloud2( const sensor_msgs::msg::PointCloud2 & pc2,