From b940640d51389dadb12f9dceefcb8f6e0a99d216 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 00:29:30 +0200 Subject: [PATCH] Rewrite from_points as a multi-level grid mesher MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit from_points grew triangles from seed fans and border edges and produced overlapping, non-manifold meshes. It now triangulates a grid of XY cells: - Points of a cell are split into levels separated by more than the clearance (min_clearance, default 1 m), so stacked floors are separate vertices. A level's height is a robust plane fit of its upper face at the cell centre; a slab seen from above and below keeps its upper face, and a level that is not a plane (wall, furniture) keeps a high top, as an obstacle. - Sparse upper faces and cells that only saw the lower face of a slab are recovered from the neighbouring cells, without lifting spikes, potholes or boxes. - Small enclosed holes (max_hole_area) are filled from their border. - Neighbouring levels are linked by mutual closest height within the step and slope limits, and every quad gets at most the two triangles of one diagonal per level: no overlap, at most two triangles per edge. - Only used vertices are kept; max_surfaces and min_surface_area drop the triangles and vertices of the removed surfaces. A "support" layer counts the points behind each triangle (0 if interpolated). build_adjacency resets the neighbors, leaves non-manifold edges open and counts them (nonmanifold_edges()). Fixes #30 Signed-off-by: Francisco Martín Rico --- navmap_core/include/navmap_core/NavMap.hpp | 8 +- navmap_core/src/navmap_core/NavMap.cpp | 59 +- navmap_core/tests/CMakeLists.txt | 5 + navmap_core/tests/test_adjacency.cpp | 165 +++ navmap_ros/include/navmap_ros/conversions.hpp | 55 +- navmap_ros/src/navmap_ros/conversions.cpp | 1155 ++++++++--------- navmap_ros/tests/CMakeLists.txt | 8 + navmap_ros/tests/test_from_points.cpp | 760 +++++++++++ 8 files changed, 1565 insertions(+), 650 deletions(-) create mode 100644 navmap_core/tests/test_adjacency.cpp create mode 100644 navmap_ros/tests/test_from_points.cpp diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index 086b59c..8df6396 100644 --- a/navmap_core/include/navmap_core/NavMap.hpp +++ b/navmap_core/include/navmap_core/NavMap.hpp @@ -506,11 +506,15 @@ class NavMap /** * \brief Build topological adjacency between neighboring NavCels. * - * Two triangles are neighbors if they share an undirected edge. + * Two triangles are neighbors if they share an undirected edge and no other + * triangle uses it; edges shared by more than two (non-manifold) stay open. * Called by \ref rebuild_geometry_accels(). */ void build_adjacency(); + /// \brief Edges shared by more than two NavCels in the last adjacency build. + std::size_t nonmanifold_edges() const {return nonmanifold_edges_;} + /** * \brief Mark a vertex as updated (reserved for future cache invalidation). * \param pid Vertex id (unused for now). @@ -1126,6 +1130,8 @@ class NavMap const Eigen::Vector3f & o, const Eigen::Vector3f & d, float t_max, RayHit & out) const; + std::size_t nonmanifold_edges_{0}; + // Owning surface per NavCel, built in rebuild_geometry_accels(). std::vector navcel_surface_; void build_navcel_surface_index_(); diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index b69f9cb..cad726c 100644 --- a/navmap_core/src/navmap_core/NavMap.cpp +++ b/navmap_core/src/navmap_core/NavMap.cpp @@ -58,6 +58,7 @@ NavMap::NavMap(const NavMap & other) layers = other.layers; layer_meta = other.layer_meta; navcel_surface_ = other.navcel_surface_; + nonmanifold_edges_ = other.nonmanifold_edges_; geometry_dirty_ = true; geometry_fp_ = 0; @@ -182,6 +183,7 @@ NavMap & NavMap::operator=(const NavMap & other) layers = other.layers; layer_meta = other.layer_meta; navcel_surface_ = other.navcel_surface_; + nonmanifold_edges_ = other.nonmanifold_edges_; geometry_dirty_ = true; return *this; @@ -199,6 +201,7 @@ NavMap & NavMap::operator=(NavMap && other) noexcept layers = std::move(other.layers); layer_meta = std::move(other.layer_meta); navcel_surface_ = std::move(other.navcel_surface_); + nonmanifold_edges_ = other.nonmanifold_edges_; geometry_fp_ = other.geometry_fp_; geometry_dirty_ = other.geometry_dirty_; @@ -230,42 +233,42 @@ static inline AABB triangle_aabb( void NavMap::build_adjacency() { - struct EdgeKey { PointId a; PointId b; }; - struct EdgeHash + constexpr auto kNone = std::numeric_limits::max(); + struct EdgeUse { - size_t operator()(const EdgeKey & k) const - { - return static_cast(k.a) * 73856093u ^ - static_cast(k.b) * 19349663u; - } - }; - struct EdgeEq - { - bool operator()(const EdgeKey & x, const EdgeKey & y) const - { - return x.a == y.a && x.b == y.b; - } + PointId a, b; + NavCelId cid; + uint8_t e; }; - - std::unordered_map, EdgeHash, EdgeEq> E; + std::vector uses; + uses.reserve(navcels.size() * 3); for (NavCelId cid = 0; cid < navcels.size(); ++cid) { auto & c = navcels[cid]; - for (int e = 0; e < 3; ++e) { + for (uint8_t e = 0; e < 3; ++e) { + c.neighbor[e] = kNone; PointId a = c.v[e]; PointId b = c.v[(e + 1) % 3]; if (a > b) {std::swap(a, b);} - EdgeKey key{a, b}; - auto it = E.find(key); - if (it == E.end()) { - E.emplace(key, std::make_pair(cid, e)); - } else { - auto pair = it->second; - auto cid2 = pair.first; - auto e2 = pair.second; - navcels[cid].neighbor[e] = cid2; - navcels[cid2].neighbor[e2] = cid; - } + uses.push_back({a, b, cid, e}); + } + } + std::sort( + uses.begin(), uses.end(), [](const EdgeUse & x, const EdgeUse & y) { + return x.a != y.a ? x.a < y.a : x.b < y.b; + }); + + // Link only edges shared by exactly two cells; leave non-manifold edges open. + nonmanifold_edges_ = 0; + for (std::size_t k = 0; k < uses.size(); ) { + std::size_t end = k + 1; + while (end < uses.size() && uses[end].a == uses[k].a && uses[end].b == uses[k].b) {++end;} + if (end - k == 2) { + navcels[uses[k].cid].neighbor[uses[k].e] = uses[k + 1].cid; + navcels[uses[k + 1].cid].neighbor[uses[k + 1].e] = uses[k].cid; + } else if (end - k > 2) { + ++nonmanifold_edges_; } + k = end; } } diff --git a/navmap_core/tests/CMakeLists.txt b/navmap_core/tests/CMakeLists.txt index 3b42222..9c45368 100644 --- a/navmap_core/tests/CMakeLists.txt +++ b/navmap_core/tests/CMakeLists.txt @@ -42,3 +42,8 @@ ament_add_gtest(test_navmap_properties test_navmap_properties.cpp) target_link_libraries(test_navmap_properties ${PROJECT_NAME} ) + +ament_add_gtest(test_adjacency test_adjacency.cpp) +target_link_libraries(test_adjacency + ${PROJECT_NAME} +) diff --git a/navmap_core/tests/test_adjacency.cpp b/navmap_core/tests/test_adjacency.cpp new file mode 100644 index 0000000..de454c0 --- /dev/null +++ b/navmap_core/tests/test_adjacency.cpp @@ -0,0 +1,165 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "navmap_core/NavMap.hpp" + +using navmap::NavCelId; +using navmap::NavMap; + +namespace +{ + +constexpr NavCelId kNone = std::numeric_limits::max(); + +// n triangles sharing the edge (0,1), each with its own apex. +NavMap fan_on_edge(int n) +{ + NavMap nm; + const std::size_t s = nm.create_surface("map"); + nm.add_vertex({0.0f, 0.0f, 0.0f}); + nm.add_vertex({1.0f, 0.0f, 0.0f}); + for (int k = 0; k < n; ++k) { + const float a = 0.5f + static_cast(k); + const auto apex = nm.add_vertex({0.5f, a, 0.1f * static_cast(k)}); + nm.add_navcel_to_surface(s, nm.add_navcel(0, 1, apex)); + } + nm.rebuild_geometry_accels(); + return nm; +} + +void expect_symmetric(const NavMap & nm) +{ + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + for (NavCelId n : nm.navcel_neighbors(cid)) { + const auto back = nm.navcel_neighbors(n); + EXPECT_NE(std::find(back.begin(), back.end(), cid), back.end()); + } + } +} + +} // namespace + +TEST(NavMap_Adjacency, TwoCellsOnAnEdgeAreNeighbors) +{ + const NavMap nm = fan_on_edge(2); + EXPECT_EQ(nm.nonmanifold_edges(), 0u); + EXPECT_EQ(nm.navcel_neighbors(0), std::vector{1}); + EXPECT_EQ(nm.navcel_neighbors(1), std::vector{0}); +} + +TEST(NavMap_Adjacency, NonManifoldEdgeStaysOpenAndIsCounted) +{ + for (int n : {3, 4, 6}) { + const NavMap nm = fan_on_edge(n); + EXPECT_EQ(nm.nonmanifold_edges(), 1u) << n; + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + EXPECT_TRUE(nm.navcel_neighbors(cid).empty()) << n << " cell " << cid; + } + } +} + +TEST(NavMap_Adjacency, OnlyTheNonManifoldEdgeIsAffected) +{ + // Unit square split in two, plus a third triangle on the diagonal. + NavMap nm; + const std::size_t s = nm.create_surface("map"); + const auto a = nm.add_vertex({0, 0, 0}); + const auto b = nm.add_vertex({1, 0, 0}); + const auto c = nm.add_vertex({1, 1, 0}); + const auto d = nm.add_vertex({0, 1, 0}); + const auto e = nm.add_vertex({0.5f, 0.5f, 1.0f}); + const auto f = nm.add_vertex({2, 0, 0}); + const NavCelId t0 = nm.add_navcel(a, b, c); + const NavCelId t1 = nm.add_navcel(a, c, d); + const NavCelId t2 = nm.add_navcel(a, c, e); // third cell on edge a-c + const NavCelId t3 = nm.add_navcel(b, f, c); // regular neighbor of t0 on b-c + for (auto t : {t0, t1, t2, t3}) { + nm.add_navcel_to_surface(s, t); + } + nm.rebuild_geometry_accels(); + + EXPECT_EQ(nm.nonmanifold_edges(), 1u); + EXPECT_EQ(nm.navcel_neighbors(t0), std::vector{t3}); + EXPECT_EQ(nm.navcel_neighbors(t3), std::vector{t0}); + EXPECT_TRUE(nm.navcel_neighbors(t1).empty()); + EXPECT_TRUE(nm.navcel_neighbors(t2).empty()); + expect_symmetric(nm); +} + +TEST(NavMap_Adjacency, RebuildClearsStaleNeighbors) +{ + NavMap nm = fan_on_edge(2); + ASSERT_EQ(nm.navcel_neighbors(0).size(), 1u); + // Detach the second cell: move it onto its own vertices. + const auto p = nm.add_vertex({5, 5, 0}); + const auto q = nm.add_vertex({6, 5, 0}); + nm.navcels[1].v[0] = p; + nm.navcels[1].v[1] = q; + nm.rebuild_geometry_accels(); + EXPECT_TRUE(nm.navcel_neighbors(0).empty()); + EXPECT_TRUE(nm.navcel_neighbors(1).empty()); +} + +TEST(NavMap_Adjacency, CountSurvivesCopyAndResetsOnFix) +{ + NavMap nm = fan_on_edge(3); + const NavMap copy(nm); + EXPECT_EQ(copy.nonmanifold_edges(), 1u); + NavMap assigned; + assigned = nm; + EXPECT_EQ(assigned.nonmanifold_edges(), 1u); + + // Remove the third cell's use of the edge and rebuild. + const auto p = nm.add_vertex({9, 9, 0}); + nm.navcels[2].v[0] = p; + nm.rebuild_geometry_accels(); + EXPECT_EQ(nm.nonmanifold_edges(), 0u); + expect_symmetric(nm); +} + +TEST(NavMap_Adjacency, LargeGridIsManifoldAndSymmetric) +{ + NavMap nm; + const std::size_t s = nm.create_surface("map"); + const int n = 60; + for (int j = 0; j <= n; ++j) { + for (int i = 0; i <= n; ++i) { + nm.add_vertex({1.0f * i, 1.0f * j, 0.0f}); + } + } + auto v = [&](int i, int j) {return static_cast(j * (n + 1) + i);}; + for (int j = 0; j < n; ++j) { + for (int i = 0; i < n; ++i) { + nm.add_navcel_to_surface(s, nm.add_navcel(v(i, j), v(i + 1, j), v(i + 1, j + 1))); + nm.add_navcel_to_surface(s, nm.add_navcel(v(i, j), v(i + 1, j + 1), v(i, j + 1))); + } + } + nm.rebuild_geometry_accels(); + EXPECT_EQ(nm.nonmanifold_edges(), 0u); + expect_symmetric(nm); + std::size_t links = 0; + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + links += nm.navcel_neighbors(cid).size(); + } + // Interior edges: 3 n^2 - 2 n, each linked from both sides. + EXPECT_EQ(links, 2u * (3u * n * n - 2u * n)); +} diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index 1fbc813..c0b6519 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -244,10 +244,12 @@ nav_msgs::msg::OccupancyGrid to_occupancy_grid( nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm); /** - * @brief Parameters controlling NavMap construction from unorganized points. + * @brief Parameters controlling NavMap construction from point clouds. * - * These parameters guide neighborhood search, local meshing, and basic geometric filtering - * used by the point-cloud based builders. + * from_points() uses resolution, max_dz, max_slope_deg, min_area, min_clearance, + * max_surfaces, min_surface_area and max_hole_area. max_edge_len, neighbor_radius, k_neighbors, + * use_radius and min_angle_deg are kept for API compatibility and ignored. + * For voxelized clouds, use a resolution of at least twice the voxel size. */ struct BuildParams { @@ -278,32 +280,45 @@ 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 + /** @brief Vertical free space (meters) that separates two stacked surfaces in a cell. */ + float min_clearance = 1.0f; + + /** @brief Maximum number of surfaces to keep, largest first (0 = all). */ + int max_surfaces = 0; + + /** @brief Surfaces smaller than this area (square meters) are dropped (0 = keep all). */ + float min_surface_area = 0.0f; + + /** @brief Enclosed empty regions up to this area (square meters) are filled (0 = none). */ + float max_hole_area = 1.0f; }; /** - * @brief Build a NavMap surface from a PCL point cloud. + * @brief Build a NavMap from an unorganized point cloud (multi-level grid triangulation). * * @param[in] input_points Point set in world coordinates (`pcl::PointXYZ`). * @param[out] out_msg Output transport message mirroring the created NavMap. * @param[in] params Meshing and filtering parameters (see ::BuildParams). - * @return The constructed `navmap::NavMap`. + * @return The constructed `navmap::NavMap` (empty if nothing could be meshed). * * @details - * Typical steps implemented by this builder include: - * - Optional downsampling according to @p params.resolution. - * - Local neighborhood discovery using either radius (@p params.use_radius) or k-NN. - * - Edge and face filtering based on @p params.max_edge_len, @p params.min_area and - * @p params.min_angle_deg. - * - Optional slope gating using @p params.max_slope_deg with respect to the vertical axis. - * - Creation of a single surface with shared vertices and triangle indices. - * The function also fills @p out_msg with the compact ROS representation of the resulting map. - * - * @note Input is treated as an unorganized cloud. If normals or intensities are present, - * they are ignored by this overload. - * @throw std::runtime_error If meshing fails due to inconsistent parameters or empty input. - */ + * - Points are binned in XY cells of @p params.resolution. In each cell, points closer + * than max(min_clearance, max_dz) in z form one level. Its height at the cell centre comes + * from a robust plane fit of its upper face (points above a gap of max_dz / 2, e.g. a slab + * seen from above and below); a face whose spread around the plane exceeds max_dz is + * clutter and keeps a high top (an obstacle). Stacked floors give several vertices per cell. + * - Enclosed empty regions up to max_hole_area are filled by interpolating the levels + * present around most of their border. + * - Vertices of neighbouring cells are linked to the closest level within + * max_dz + tan(max_slope_deg) * distance, only when the choice is mutual. + * - Each grid quad gets at most the two triangles of one diagonal per connected level, + * so every edge has at most two triangles (manifold), and no triangle steeper than + * max_slope_deg is created. + * - Surfaces are the connected components; max_surfaces and min_surface_area drop the + * smaller ones together with their triangles and vertices. + * - Adds per-triangle layers "elevation" (mean vertex height) and "support" (points behind + * the weakest vertex, up to 255; 0 = interpolated over a hole). Frame id is "map". + */ navmap::NavMap from_points( const pcl::PointCloud & input_points, navmap_ros_interfaces::msg::NavMap & out_msg, diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index 2a96eea..0ae1cf4 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -16,6 +16,7 @@ #include "navmap_ros/conversions.hpp" #include +#include #include #include #include @@ -23,6 +24,10 @@ #include #include #include +#include +#include +#include +#include #include "geometry_msgs/msg/pose.hpp" #include "std_msgs/msg/header.hpp" @@ -36,7 +41,6 @@ #include "pcl/point_cloud.h" #include "pcl/point_types.h" -#include "pcl/kdtree/kdtree_flann.h" namespace navmap_ros { @@ -579,313 +583,90 @@ bool build_navmap_from_mesh( return true; } -// ----------------- Surface from PC2 ----------------- - using Triangle = Eigen::Vector3i; -static inline float sqr(float v) {return v * v;} -static inline float dist3(const pcl::PointXYZ & a, const pcl::PointXYZ & b) -{ - return std::sqrt(sqr(a.x - b.x) + sqr(a.y - b.y) + sqr(a.z - b.z)); -} - -static inline float clamp01(float x) -{ - if (x < 0.0f) {return 0.0f;} - if (x > 1.0f) {return 1.0f;} - return x; -} - -static inline float tri_area( - const Eigen::Vector3f & a, - const Eigen::Vector3f & b, - const Eigen::Vector3f & c) -{ - return 0.5f * ((b - a).cross(c - a)).norm(); -} - -// Keys to avoid duplicates (edges/triangles) -struct EdgeKey -{ - int a, b; - bool operator==(const EdgeKey & o) const noexcept {return a == o.a && b == o.b;} -}; -struct EdgeHasher -{ - std::size_t operator()(const EdgeKey & e) const noexcept - { - return (static_cast(e.a) << 32) ^ static_cast(e.b); - } -}; -static inline EdgeKey make_edge(int i, int j) -{ - if (i < j) {return {i, j};} - return {j, i}; -} +// ----------------- Keep only largest surfaces ----------------- -struct TriKey -{ - int a, b, c; - bool operator==(const TriKey & o) const noexcept - { - return a == o.a && b == o.b && c == o.c; - } -}; -struct TriHasher +// Keeps only the triangles listed in some surface and the vertices they use. +static void compact_to_surfaces(navmap_ros_interfaces::msg::NavMap & msg) { - std::size_t operator()(const TriKey & t) const noexcept - { - std::size_t h = 1469598103934665603ull; - auto mix = [&](int k){ - h ^= static_cast(k) + 0x9e3779b97f4a7c15ull + (h << 6) + (h >> 2); - }; - mix(t.a); mix(t.b); mix(t.c); - return h; + const std::size_t ntris = msg.navcels_v0.size(); + std::vector tri_map(ntris, UINT32_MAX); + uint32_t nt = 0; + for (auto & s : msg.surfaces) { + for (auto & cid : s.navcels) { + if (cid >= ntris) {continue;} + if (tri_map[cid] == UINT32_MAX) {tri_map[cid] = nt++;} + cid = tri_map[cid]; + } } -}; -static inline TriKey make_tri(int i, int j, int k) -{ - int v[3] = {i, j, k}; - std::sort(v, v + 3); - return {v[0], v[1], v[2]}; -} - -inline void triangle_angles_deg( - const Eigen::Vector3f & A, - const Eigen::Vector3f & B, - const Eigen::Vector3f & C, - float & angA, float & angB, float & angC) -{ - // side lengths opposite to vertices A/B/C - float a = (B - C).norm(); - float b = (C - A).norm(); - float c = (A - B).norm(); - - const float eps = 1e-12f; - a = std::max(a, eps); - b = std::max(b, eps); - c = std::max(c, eps); - - 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)); - return std::acos(cosv) * 180.0f / static_cast(M_PI); + if (nt == ntris) {return;} + + const std::size_t nverts = msg.positions_x.size(); + std::vector vmap(nverts, UINT32_MAX); + uint32_t nv = 0; + std::vector v0(nt), v1(nt), v2(nt); + auto remap_v = [&](uint32_t v) { + if (vmap[v] == UINT32_MAX) {vmap[v] = nv++;} + return vmap[v]; }; - - angA = angle_from(a, b, c); - angB = angle_from(b, c, a); - angC = angle_from(c, a, b); -} - -// ----------------- Downsampling (only what is used) ----------------- - -struct Voxel -{ - int x, y, z; - bool operator==(const Voxel & o) const noexcept - { - return x == o.x && y == o.y && z == o.z; + for (std::size_t t = 0; t < ntris; ++t) { + if (tri_map[t] == UINT32_MAX) {continue;} + v0[tri_map[t]] = remap_v(msg.navcels_v0[t]); + v1[tri_map[t]] = remap_v(msg.navcels_v1[t]); + v2[tri_map[t]] = remap_v(msg.navcels_v2[t]); } -}; - -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; - } -}; - -// 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); - - 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; - } + msg.navcels_v0.swap(v0); + msg.navcels_v1.swap(v1); + msg.navcels_v2.swap(v2); + + auto compact = [](auto & data, const std::vector & map, uint32_t n) { + if (data.size() != map.size()) {return;} + std::remove_reference_t out(n); + for (std::size_t k = 0; k < map.size(); ++k) { + if (map[k] != UINT32_MAX) {out[map[k]] = data[k];} } - } - flush_cluster(); + data.swap(out); + }; + compact(msg.positions_x, vmap, nv); + compact(msg.positions_y, vmap, nv); + compact(msg.positions_z, vmap, nv); + if (msg.has_vertex_rgba) { + compact(msg.colors_r, vmap, nv); + compact(msg.colors_g, vmap, nv); + compact(msg.colors_b, vmap, nv); + compact(msg.colors_a, vmap, nv); + } + for (auto & layer : msg.layers) { + compact(layer.data_u8, tri_map, nt); + compact(layer.data_f32, tri_map, nt); + compact(layer.data_f64, tri_map, nt); } - - output.width = static_cast(output.points.size()); - output.height = 1; - output.is_dense = true; - 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. - -// max_edge_len is meant as a broad sanity cap on 3D edge length, separate -// from neighbor_radius (the search radius used to find candidate points) -// and max_dz (the per-edge vertical cap). When max_edge_len is set close to -// neighbor_radius -- as in BuildParams' own defaults, both 2.0 m -- it stops -// being a broad cap and starts spuriously rejecting valid edges: the -// candidate search is a 3D sphere of radius neighbor_radius, so any real -// terrain roughness (a few cm of Z noise, far under max_slope_deg/max_dz) -// shifts which points land near that sphere's boundary and can nudge a -// perfectly legitimate edge's 3D length a hair past max_edge_len, leaving a -// hole exactly where the terrain is *not* too steep to be navigable. The -// true worst-case length of an edge that already satisfies both the XY -// search radius and the vertical max_dz cap is the hypotenuse of the two, -// so max_edge_len should never be tighter than that combination -- this -// widens it to match whenever the caller's value would otherwise clip -// geometrically valid edges. See easynav_gis_tool.md for the reproduction. -inline float effective_max_edge_len(const BuildParams & P) -{ - if (P.max_edge_len <= 0.0f || P.neighbor_radius <= 0.0f) {return P.max_edge_len;} - const float floor = std::sqrt( - P.neighbor_radius * P.neighbor_radius + P.max_dz * P.max_dz); - return std::max(P.max_edge_len, floor); } -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) +// Drops surfaces smaller than min_area (square meters) with their triangles and vertices. +static void drop_small_surfaces(navmap_ros_interfaces::msg::NavMap & msg, float min_area) { - 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); - const float max_edge_len = effective_max_edge_len(P); - if (max_edge_len > 0.0f && - (dAB > max_edge_len || dBC > max_edge_len || dCA > 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) < std::max(P.min_area, 1e-9f)) {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 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; - } - - // Canonical orientation (normal facing +Z) - if (n.dot(Eigen::Vector3f::UnitZ()) < 0.0f) { - std::swap(j, k); + if (!(min_area > 0.0f)) {return;} + auto area_of = [&](uint32_t t) { + auto p = [&](uint32_t v) { + return Eigen::Vector3f(msg.positions_x[v], msg.positions_y[v], msg.positions_z[v]); + }; + const Eigen::Vector3f a = p(msg.navcels_v0[t]); + return 0.5f * (p(msg.navcels_v1[t]) - a).cross(p(msg.navcels_v2[t]) - a).norm(); + }; + std::vector kept; + for (auto & s : msg.surfaces) { + float area = 0.0f; + for (uint32_t t : s.navcels) { + area += area_of(t); + } + if (area >= min_area) {kept.push_back(std::move(s));} } - - 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; + msg.surfaces.swap(kept); + compact_to_surfaces(msg); } -// ----------------- Keep only largest surfaces ----------------- - static void keep_top_surfaces_by_size(navmap_ros_interfaces::msg::NavMap & msg, int max_surfaces) { if (max_surfaces <= 0) {return;} @@ -894,7 +675,7 @@ static void keep_top_surfaces_by_size(navmap_ros_interfaces::msg::NavMap & msg, std::vector ids(msg.surfaces.size()); std::iota(ids.begin(), ids.end(), 0); - std::sort(ids.begin(), ids.end(), [&](size_t a, size_t b){ + std::stable_sort(ids.begin(), ids.end(), [&](size_t a, size_t b){ return msg.surfaces[a].navcels.size() > msg.surfaces[b].navcels.size(); }); @@ -904,6 +685,7 @@ static void keep_top_surfaces_by_size(navmap_ros_interfaces::msg::NavMap & msg, kept.push_back(std::move(msg.surfaces[ids[static_cast(i)]])); } msg.surfaces.swap(kept); + compact_to_surfaces(msg); } // ----------------- Rebuild surfaces by triangle connectivity ----------------- @@ -1019,367 +801,538 @@ static void rebuild_surfaces_by_connectivity(navmap_ros_interfaces::msg::NavMap msg.surfaces.swap(out); } -// ----------------- Local Grow builder (renamed: from_points) ----------------- +// ----------------- Multi-level grid builder (from_points) ----------------- -navmap::NavMap from_points( - const pcl::PointCloud & input_points, - navmap_ros_interfaces::msg::NavMap & out_msg, - BuildParams P) +namespace { - out_msg = navmap_ros_interfaces::msg::NavMap(); - if (input_points.size() < 3) { - return navmap::NavMap(); +// Height cluster of one XY cell: a vertex candidate of the multi-level grid. +struct GridNode +{ + int32_t i, j; + float z; + uint32_t support; // points behind the node; 0 if interpolated + float alt_z; // height of a sparse upper face, if any (NaN otherwise) + float z_min; // lowest point of the level (its lower face, if any) +}; + +struct BinnedPoint +{ + uint64_t key; + float z, x, y; // x, y relative to the cell centre +}; + +float level_height_top(const BinnedPoint * pts, std::size_t n, float step); + +// Height of one level at its cell centre. The level may hold several faces closer than the +// clearance (a slab seen from above and below): the support is its upper face, the top group +// of points above a gap. A top group too small to trust on its own (a sparse face, or a spike) +// is returned in alt_z, for the neighbours to decide. +float level_height(const BinnedPoint * pts, std::size_t n, float step, float & alt_z) +{ + alt_z = std::numeric_limits::quiet_NaN(); + const float layer_gap = 0.5f * step; + for (std::size_t k = n - 1; k >= 1; --k) { + if (pts[k].z - pts[k - 1].z <= layer_gap) {continue;} + if (n - k >= std::max(3, n / 10)) { + return level_height_top(pts + k, n - k, step); + } + if (std::isnan(alt_z)) {alt_z = pts[k + (n - k - 1) / 2].z;} // median of the small group } + return level_height_top(pts, n, step); +} - // 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; +// Robust plane fit at the cell centre; a cluttered level keeps a high top (an obstacle). +float level_height_top(const BinnedPoint * pts, std::size_t n, float step) +{ + if (n < 3) { + float z = 0.0f; + for (std::size_t k = 0; k < n; ++k) { + z += pts[k].z; + } + return z / static_cast(n); } - if (cloud.size() < 3) { - return navmap::NavMap(); + // Points come sorted by z: a vertical spread beyond a step that a plane cannot explain + // means clutter (wall, bush, furniture), so keep the 90th percentile as an obstacle top. + auto fit = [&](const std::vector & use, Eigen::Vector3f & abc) { + Eigen::Matrix3f A = Eigen::Matrix3f::Zero(); + Eigen::Vector3f b = Eigen::Vector3f::Zero(); + for (std::size_t k = 0; k < n; ++k) { + if (!use[k]) {continue;} + const Eigen::Vector3f r(pts[k].x, pts[k].y, 1.0f); + A += r * r.transpose(); + b += r * pts[k].z; + } + if (std::fabs(A.determinant()) < 1e-9f) {return false;} + abc = A.ldlt().solve(b); + return abc.allFinite(); + }; + const float top90 = pts[std::min(n - 1, (n * 9) / 10)].z; + std::vector use(n, 1); + Eigen::Vector3f abc; + if (!fit(use, abc)) { + // Collinear in XY (thin wall, scan line): no plane; tall means clutter. + return pts[n - 1].z - pts[0].z > step ? top90 : pts[(n - 1) / 2].z; + } + // One trimming pass: drop residuals beyond 3 robust sigmas (spikes, outliers). + std::vector res(n); + for (std::size_t k = 0; k < n; ++k) { + res[k] = pts[k].z - (abc[0] * pts[k].x + abc[1] * pts[k].y + abc[2]); + } + std::vector absr(n); + for (std::size_t k = 0; k < n; ++k) { + absr[k] = std::fabs(res[k]); + } + std::nth_element(absr.begin(), absr.begin() + n / 2, absr.end()); + const float thr = std::max(0.02f, 3.0f * 1.4826f * absr[n / 2]); + std::size_t kept = 0; + for (std::size_t k = 0; k < n; ++k) { + use[k] = std::fabs(res[k]) <= thr; + kept += use[k]; } + if (kept >= 3) {fit(use, abc);} - // Neighborhood structure - pcl::KdTreeFLANN kdtree; - kdtree.setInputCloud(cloud.makeShared()); + // Clutter test on the inliers' spread around the plane (10th to 90th percentile). + std::vector in; + in.reserve(n); + for (std::size_t k = 0; k < n; ++k) { + if (use[k]) {in.push_back(pts[k].z - (abc[0] * pts[k].x + abc[1] * pts[k].y + abc[2]));} + } + std::sort(in.begin(), in.end()); + const float spread = in[std::min(in.size() - 1, (in.size() * 9) / 10)] - in[in.size() / 10]; + return spread > step ? top90 : abc[2]; +} - const int N = static_cast(cloud.size()); - std::vector used_vertex(N, 0); - std::vector triangles; - triangles.reserve(N * 2); +inline uint64_t cell_key(int32_t i, int32_t j) +{ + return (static_cast(static_cast(i)) << 32) | static_cast(j); +} - // 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;}); +// 8-neighbourhood; kOpposite[d] is the direction back. +constexpr int kDirs[8][2] = {{1, 0}, {1, 1}, {0, 1}, {-1, 1}, {-1, 0}, {-1, -1}, {0, -1}, {1, -1}}; +constexpr int kOpposite[8] = {4, 5, 6, 7, 0, 1, 2, 3}; - // Global state - std::unordered_set tri_set_global; - std::unordered_set edge_set_global; - std::unordered_map edge_use_count; +inline int dir_index(int dx, int dy) +{ + for (int d = 0; d < 8; ++d) { + if (kDirs[d][0] == dx && kDirs[d][1] == dy) {return d;} + } + return -1; +} - 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; - }; +} // namespace - 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); - } +navmap::NavMap from_points( + const pcl::PointCloud & input_points, + navmap_ros_interfaces::msg::NavMap & out_msg, + BuildParams P) +{ + out_msg = navmap_ros_interfaces::msg::NavMap(); + const float res = P.resolution; + if (!(res > 0.0f) || input_points.size() < 3) { + return navmap::NavMap(); + } + const float step = P.max_dz > 0.0f ? P.max_dz : 0.30f; + // Points closer than this in z are the same surface (thickness, sampling, clutter). + const float gap = std::max(step, P.min_clearance); + const float tan_slope = std::tan(P.max_slope_deg * static_cast(M_PI) / 180.0f); + const float cos_slope = std::cos(P.max_slope_deg * static_cast(M_PI) / 180.0f); + + // 1) Bin finite points by XY cell, sorted by (cell, z). + float min_x = std::numeric_limits::infinity(); + float min_y = std::numeric_limits::infinity(); + for (const auto & p : input_points) { + if (pcl::isFinite(p)) {min_x = std::min(min_x, p.x); min_y = std::min(min_y, p.y);} + } + if (!std::isfinite(min_x)) { + return navmap::NavMap(); + } + // Half-cell origin shift: samples on a res-spaced lattice fall on cell centres, not borders. + min_x -= 0.5f * res; + min_y -= 0.5f * res; + std::vector binned; + binned.reserve(input_points.size()); + for (const auto & p : input_points) { + if (!pcl::isFinite(p)) {continue;} + const auto i = static_cast(std::floor((p.x - min_x) / res)); + const auto j = static_cast(std::floor((p.y - min_y) / res)); + binned.push_back( + {cell_key(i, j), p.z, p.x - (min_x + (i + 0.5f) * res), p.y - (min_y + (j + 0.5f) * res)}); + } + std::sort( + binned.begin(), binned.end(), [](const BinnedPoint & a, const BinnedPoint & b) { + return a.key != b.key ? a.key < b.key : a.z < b.z; + }); + + // 2) Split each cell into levels separated by more than the clearance gap. + std::vector nodes; + std::unordered_map> cells; // key -> [first, count) + cells.reserve(binned.size() / 4 + 1); + for (std::size_t k = 0; k < binned.size(); ) { + const uint64_t key = binned[k].key; + const auto i = static_cast(key >> 32); + const auto j = static_cast(key & 0xffffffffu); + const auto first = static_cast(nodes.size()); + std::size_t level_begin = k; + for (++k; ; ++k) { + const bool cell_end = k == binned.size() || binned[k].key != key; + if (cell_end || binned[k].z - binned[k - 1].z > gap) { + const std::size_t n = k - level_begin; + float alt_z; + const float z = level_height(&binned[level_begin], n, step, alt_z); + nodes.push_back({i, j, z, static_cast(n), alt_z, binned[level_begin].z}); + level_begin = k; } - }; + if (cell_end) {break;} + } + cells.emplace(key, std::make_pair(first, static_cast(nodes.size()) - first)); + } - 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); - }; + // 2a) Sparse upper faces. A slab seen mostly from below leaves cells whose upper face got + // few points (alt_z) or none, and their own height falls on the lower face. + { + const float tol = 0.5f * step; + auto neighbours_with = [&](const GridNode & nd, auto && pred) { + int votes = 0; + for (const auto & d : kDirs) { + const auto it = cells.find(cell_key(nd.i + d[0], nd.j + d[1])); + if (it == cells.end()) {continue;} + for (uint32_t m = it->second.first; m < it->second.first + it->second.second; ++m) { + if (pred(nodes[m])) {++votes; break;} + } + } + return votes; + }; + // Pass 1: a sparse upper face is the support when a neighbour stands on it (its main + // height) and two or more neighbours see it. Spikes over a floor never have a neighbour + // whose main height is theirs. + std::vector> lifted; + for (std::size_t n = 0; n < nodes.size(); ++n) { + const auto & nd = nodes[n]; + if (std::isnan(nd.alt_z)) {continue;} + const int on_it = neighbours_with( + nd, [&](const GridNode & nb) {return std::fabs(nb.z - nd.alt_z) <= tol;}); + const int seen = neighbours_with( + nd, [&](const GridNode & nb) { + return std::fabs(nb.z - nd.alt_z) <= tol || + (!std::isnan(nb.alt_z) && std::fabs(nb.alt_z - nd.alt_z) <= tol); + }); + if (on_it >= 1 && seen >= 2) {lifted.emplace_back(n, nd.alt_z);} + } + for (const auto & [n, z] : lifted) { + nodes[n].z = z; + } - 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; }; - const float eff_max_edge_len = effective_max_edge_len(P); - 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; + // Pass 2: a cell that only saw the lower face takes the upper face of the neighbours whose + // lower face is at its height. It needs a level below in its column (where the sensor + // was), so a pothole or a box never qualifies. + std::vector> inferred; + for (std::size_t n = 0; n < nodes.size(); ++n) { + const auto & nd = nodes[n]; + bool level_below = false; + const auto & own = cells.at(cell_key(nd.i, nd.j)); + for (uint32_t m = own.first; m < own.first + own.second; ++m) { + level_below = level_below || nodes[m].z < nd.z_min - gap; } - dup_out = false; - - const auto & A = cloud[i], & B = cloud[j], & C = cloud[k]; - - if (eff_max_edge_len > 0.0f) { - const float lAB = dist3f(A, B), lBC = dist3f(B, C), lCA = dist3f(C, A); - if (lAB > eff_max_edge_len || lBC > eff_max_edge_len || lCA > eff_max_edge_len) { - rej.edge_len++; return false; + if (!level_below) {continue;} + const int own_votes = neighbours_with( + nd, [&](const GridNode & nb) {return std::fabs(nb.z - nd.z) <= tol;}); + std::vector above; + for (const auto & d : kDirs) { + const auto it = cells.find(cell_key(nd.i + d[0], nd.j + d[1])); + if (it == cells.end()) {continue;} + for (uint32_t m = it->second.first; m < it->second.first + it->second.second; ++m) { + const auto & nb = nodes[m]; + if (std::fabs(nb.z_min - nd.z) <= tol && nb.z > nd.z + tol && nb.z - nd.z <= gap) { + above.push_back(nb.z); + break; + } } } - - 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 (above.size() >= 3 && static_cast(above.size()) > own_votes) { + std::nth_element(above.begin(), above.begin() + above.size() / 2, above.end()); + inferred.emplace_back(n, above[above.size() / 2]); } + } + for (const auto & [n, z] : inferred) { + nodes[n].z = z; + nodes[n].support = 0; // height inferred, not observed + } + } - 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;} + // 2b) Fill small enclosed holes (cells without any point) from the levels around them. + const auto max_hole_cells = static_cast(std::floor(P.max_hole_area / (res * res))); + if (max_hole_cells > 0) { + std::unordered_set visited; + std::vector>> filled; + std::vector seeds; + for (const auto & kv : cells) { + const auto i = static_cast(kv.first >> 32); + const auto j = static_cast(kv.first & 0xffffffffu); + for (int d = 0; d < 8; d += 2) { + seeds.push_back(cell_key(i + kDirs[d][0], j + kDirs[d][1])); } - - return true; - }; - - // Z windows around the expanding boundary (keeps ramps but avoids jumps) - const float z_window_seed = std::max(P.max_dz, 0.35f); - - std::vector> surf_tri_ranges; // [offset, count] - - RejectCounts global_rej{}; // still used to track precheck stats (no logging) - - for (int seed_idx : order) { - if (used_vertex[seed_idx]) {continue;} - - // 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); - } - } + } + std::sort(seeds.begin(), seeds.end()); + for (const uint64_t seed : seeds) { + if (cells.count(seed) || visited.count(seed)) {continue;} + // Flood the empty region (4-connected); give up once it is larger than a hole. + std::vector hole{seed}, stack{seed}; + visited.insert(seed); + bool too_big = false; + while (!stack.empty() && !too_big) { + const uint64_t c = stack.back(); + stack.pop_back(); + const auto ci = static_cast(c >> 32); + const auto cj = static_cast(c & 0xffffffffu); + for (int d = 0; d < 8; d += 2) { + const uint64_t nk = cell_key(ci + kDirs[d][0], cj + kDirs[d][1]); + if (cells.count(nk) || visited.count(nk)) {continue;} + visited.insert(nk); + hole.push_back(nk); + stack.push_back(nk); + if (hole.size() > max_hole_cells) {too_big = true; break;} } - } 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 (too_big) {continue;} + + // Border cells (8-neighbours with data) and their levels. + std::unordered_set in_hole(hole.begin(), hole.end()); + std::unordered_set border; + for (const uint64_t c : hole) { + const auto ci = static_cast(c >> 32); + const auto cj = static_cast(c & 0xffffffffu); + for (const auto & dd : kDirs) { + const uint64_t nk = cell_key(ci + dd[0], cj + dd[1]); + if (!in_hole.count(nk) && cells.count(nk)) {border.insert(nk);} + } + } + std::vector border_nodes; + for (const uint64_t b : border) { + const auto & r = cells.at(b); + for (uint32_t n = r.first; n < r.first + r.second; ++n) { + if (nodes[n].support > 0) {border_nodes.push_back(n);} + } + } + std::sort( + border_nodes.begin(), border_nodes.end(), + [&](uint32_t a, uint32_t b) {return nodes[a].z < nodes[b].z;}); + + std::vector> zs(hole.size()); + for (std::size_t g = 0; g < border_nodes.size(); ) { + // One level: border nodes whose heights chain within the clearance gap. + std::size_t e = g + 1; + while (e < border_nodes.size() && + nodes[border_nodes[e]].z - nodes[border_nodes[e - 1]].z <= gap) + { + ++e; + } + std::unordered_set level_cells; + for (std::size_t q = g; q < e; ++q) { + level_cells.insert(cell_key(nodes[border_nodes[q]].i, nodes[border_nodes[q]].j)); + } + // Only levels present around most of the hole (not a single wall or ledge). + if (level_cells.size() * 4 >= border.size() * 3) { + for (std::size_t h = 0; h < hole.size(); ++h) { + const auto hi = static_cast(hole[h] >> 32); + const auto hj = static_cast(hole[h] & 0xffffffffu); + double wsum = 0.0, zsum = 0.0; + for (std::size_t q = g; q < e; ++q) { + const auto & nd = nodes[border_nodes[q]]; + const double d2 = static_cast(nd.i - hi) * (nd.i - hi) + + static_cast(nd.j - hj) * (nd.j - hj); + wsum += 1.0 / d2; + zsum += nd.z / d2; } + zs[h].push_back(static_cast(zsum / wsum)); } } + g = e; + } + for (std::size_t h = 0; h < hole.size(); ++h) { + if (!zs[h].empty()) {filled.emplace_back(hole[h], std::move(zs[h]));} + } + } + for (auto & [key, levels] : filled) { + const auto first = static_cast(nodes.size()); + for (const float z : levels) { + nodes.push_back( + {static_cast(key >> 32), static_cast(key & 0xffffffffu), z, 0, + std::numeric_limits::quiet_NaN(), z}); } + cells.emplace(key, std::make_pair(first, static_cast(levels.size()))); } + } - // Quick filters - if (eff_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) > eff_max_edge_len; - }), - neigh_seed.end()); + // 3) Link nodes of neighbouring cells: closest height within step + slope, mutual. + auto partner_in = [&](const GridNode & a, int d) -> int64_t { + const auto it = cells.find(cell_key(a.i + kDirs[d][0], a.j + kDirs[d][1])); + if (it == cells.end()) {return -1;} + const float dist = res * ((kDirs[d][0] != 0 && kDirs[d][1] != 0) ? std::sqrt(2.0f) : 1.0f); + const float max_dz = step + tan_slope * dist; + int64_t best = -1; + float best_dz = std::numeric_limits::infinity(); + for (uint32_t n = it->second.first; n < it->second.first + it->second.second; ++n) { + const float dz = std::fabs(nodes[n].z - a.z); + if (dz <= max_dz && dz < best_dz) {best_dz = dz; best = n;} + } + return best; + }; + std::vector> link(nodes.size()); + for (std::size_t n = 0; n < nodes.size(); ++n) { + for (int d = 0; d < 8; ++d) { + link[n][d] = partner_in(nodes[n], d); } - { - 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()); + } + for (std::size_t n = 0; n < nodes.size(); ++n) { + for (int d = 0; d < 8; ++d) { + const int64_t m = link[n][d]; + if (m >= 0 && link[m][kOpposite[d]] != static_cast(n)) {link[n][d] = -1;} } + } + auto linked = [&](int64_t a, int64_t b, int dx, int dy) { + return a >= 0 && b >= 0 && link[a][dir_index(dx, dy)] == b; + }; - if (neigh_seed.size() < 2) { - used_vertex[seed_idx] = 1; - continue; + // 4) Triangulate each quad: per connected sheet, the triangles of one diagonal. + std::vector quads; + quads.reserve(cells.size() * 4); + for (const auto & kv : cells) { + const auto i = static_cast(kv.first >> 32); + const auto j = static_cast(kv.first & 0xffffffffu); + for (int di = -1; di <= 0; ++di) { + for (int dj = -1; dj <= 0; ++dj) { + quads.push_back(cell_key(i + di, j + dj)); + } } + } + std::sort(quads.begin(), quads.end()); + quads.erase(std::unique(quads.begin(), quads.end()), quads.end()); - // 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); - }); - } + auto vtx = [&](int64_t n) { + return Eigen::Vector3f( + min_x + (nodes[n].i + 0.5f) * res, min_y + (nodes[n].j + 0.5f) * res, nodes[n].z); + }; + auto good_triangle = [&](int64_t a, int64_t b, int64_t c) { + const Eigen::Vector3f n = (vtx(b) - vtx(a)).cross(vtx(c) - vtx(a)); + const float nn = n.norm(); + return 0.5f * nn >= P.min_area && nn > 0.0f && n.z() / nn >= cos_slope; + }; - const size_t tri_off = triangles.size(); - - RejectCounts comp_rej{}; - - // 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)) { - // 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++; + std::vector> tris; + tris.reserve(nodes.size() * 2); + // Corners: 0=(i,j) 1=(i+1,j) 2=(i+1,j+1) 3=(i,j+1). + constexpr int kCorner[4][2] = {{0, 0}, {1, 0}, {1, 1}, {0, 1}}; + for (const uint64_t q : quads) { + const auto qi = static_cast(q >> 32); + const auto qj = static_cast(q & 0xffffffffu); + + std::vector> members; // (corner, node) + for (int c = 0; c < 4; ++c) { + const auto it = cells.find(cell_key(qi + kCorner[c][0], qj + kCorner[c][1])); + if (it == cells.end()) {continue;} + for (uint32_t n = it->second.first; n < it->second.first + it->second.second; ++n) { + members.emplace_back(c, n); } } - 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)) - { - 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 (members.size() < 3) {continue;} + + std::vector seen(members.size(), 0); + for (std::size_t s = 0; s < members.size(); ++s) { + if (seen[s]) {continue;} + // Sheet: nodes of this quad connected through links. + std::array corner{-1, -1, -1, -1}; + bool ambiguous = false; + std::vector stack{s}; + seen[s] = 1; + while (!stack.empty()) { + const auto [c, n] = members[stack.back()]; + stack.pop_back(); + if (corner[c] >= 0 && corner[c] != n) {ambiguous = true;} + corner[c] = n; + for (std::size_t t = 0; t < members.size(); ++t) { + const auto [c2, n2] = members[t]; + if (seen[t] || c2 == c) {continue;} + if (linked(n, n2, kCorner[c2][0] - kCorner[c][0], kCorner[c2][1] - kCorner[c][1])) { + seen[t] = 1; + stack.push_back(t); + } } } - } - - 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; - } - - // BFS expansion over border edges - while (!frontier.empty()) { - EdgeKey e = frontier.front(); frontier.pop(); - in_frontier.erase(e); - - // 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); - - // Candidate completion points: union of the neighbors of *both* - // e.a and e.b, not just e.a. Searching only from e.a misses any - // point that is close to e.b but just outside radius of e.a -- which - // happens routinely once the edge itself is close to - // neighbor_radius long (the common case for a diagonal grid edge), - // and was a real source of unexplained holes on otherwise-navigable - // terrain: it forced neighbor_radius to be inflated well past the - // true point spacing just to paper over this asymmetry. Geometric - // validity of the resulting triangle is still fully enforced below - // by precheck()/try_add_triangle(), so widening the candidate pool - // here does not admit any edge/angle/slope violation. See - // easynav_gis_tool.md. - std::vector neigh_candidates; - { - std::unordered_set seen; - auto collect = [&](const pcl::PointXYZ & from) { - std::vector inds; std::vector dists; - if (P.neighbor_radius > 0.0f) { - if (kdtree.radiusSearch(from, P.neighbor_radius, inds, dists) > 0) { - for (int id : inds) { - if (id != e.a && id != e.b && seen.insert(id).second) { - neigh_candidates.push_back(id); - } - } - } - } else { - const int K = std::max(8, P.k_neighbors); - if (kdtree.nearestKSearch(from, K, inds, dists) > 0) { - for (int id : inds) { - if (id != e.a && id != e.b && seen.insert(id).second) { - neigh_candidates.push_back(id); - } - } - } - } - }; - collect(cloud[e.a]); - collect(cloud[e.b]); + if (ambiguous) {continue;} + + const int64_t v00 = corner[0], v10 = corner[1], v11 = corner[2], v01 = corner[3]; + auto ok = [&](int64_t a, int64_t b, int64_t c) { + if (a < 0 || b < 0 || c < 0) {return false;} + auto lk = [&](int64_t x, int64_t y) { + const auto & nx = nodes[x]; + const auto & ny = nodes[y]; + return linked(x, y, ny.i - nx.i, ny.j - nx.j); + }; + return lk(a, b) && lk(b, c) && lk(c, a) && good_triangle(a, b, c); + }; + const bool a1 = ok(v00, v10, v11), a2 = ok(v00, v11, v01); + const bool b1 = ok(v00, v10, v01), b2 = ok(v10, v11, v01); + const int na = a1 + a2, nb = b1 + b2; + bool use_a = na >= nb; + if (na == 2 && nb == 2) { + // Both diagonals possible: the one with the smaller height change. + use_a = std::fabs(nodes[v00].z - nodes[v11].z) <= std::fabs(nodes[v10].z - nodes[v01].z); } - - for (int c : neigh_candidates) { - 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;} - - 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)) { - 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); - - 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++; - } + if (use_a) { + if (a1) {tris.push_back({v00, v10, v11});} + if (a2) {tris.push_back({v00, v11, v01});} + } else { + if (b1) {tris.push_back({v00, v10, v01});} + if (b2) {tris.push_back({v10, v11, v01});} } } - - // 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; } - - if (triangles.empty()) { + if (tris.empty()) { return navmap::NavMap(); } - // Build NavMap + surfaces + // 5) Vertices: only nodes used by some triangle. + std::vector vid(nodes.size(), -1); + pcl::PointCloud verts; + std::vector triangles; + triangles.reserve(tris.size()); + for (const auto & t : tris) { + Triangle out; + for (int k = 0; k < 3; ++k) { + if (vid[t[k]] < 0) { + vid[t[k]] = static_cast(verts.size()); + const Eigen::Vector3f p = vtx(t[k]); + verts.push_back(pcl::PointXYZ(p.x(), p.y(), p.z())); + } + out[k] = static_cast(vid[t[k]]); + } + triangles.push_back(out); + } + 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)) { + if (!build_navmap_from_mesh(verts, triangles, frame_id, msg_tmp, nullptr)) { return navmap::NavMap(); } - - // 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); + { + // Points behind each triangle (its weakest vertex); 0 = interpolated over a hole. + navmap_ros_interfaces::msg::NavMapLayer support; + support.name = "support"; + support.type = navmap_ros_interfaces::msg::NavMapLayer::U8; + support.data_u8.reserve(tris.size()); + for (const auto & t : tris) { + const uint32_t m = std::min({nodes[t[0]].support, nodes[t[1]].support, nodes[t[2]].support}); + support.data_u8.push_back(static_cast(std::min(m, 255u))); } - msg_tmp.surfaces.push_back(std::move(s)); + msg_tmp.layers.push_back(std::move(support)); } - rebuild_surfaces_by_connectivity(msg_tmp); + drop_small_surfaces(msg_tmp, P.min_surface_area); keep_top_surfaces_by_size(msg_tmp, P.max_surfaces); + if (msg_tmp.navcels_v0.empty()) { + return navmap::NavMap(); + } out_msg = std::move(msg_tmp); return from_msg(out_msg); diff --git a/navmap_ros/tests/CMakeLists.txt b/navmap_ros/tests/CMakeLists.txt index 8090151..6ca04b2 100644 --- a/navmap_ros/tests/CMakeLists.txt +++ b/navmap_ros/tests/CMakeLists.txt @@ -7,3 +7,11 @@ ament_add_gtest(test_navmap_io test_navmap_io.cpp) target_link_libraries(test_navmap_io ${PROJECT_NAME} ) + +ament_add_gtest(test_from_points test_from_points.cpp) +target_include_directories(test_from_points PRIVATE ${pcl_conversions_INCLUDE_DIRS}) +target_link_libraries(test_from_points + ${PROJECT_NAME} + ${pcl_conversions_TARGETS} + ${pcl_conversions_LIBRARIES} +) diff --git a/navmap_ros/tests/test_from_points.cpp b/navmap_ros/tests/test_from_points.cpp new file mode 100644 index 0000000..03ec4bb --- /dev/null +++ b/navmap_ros/tests/test_from_points.cpp @@ -0,0 +1,760 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" +#include "pcl_conversions/pcl_conversions.h" + +using navmap::NavCelId; +using navmap::NavMap; +using Cloud = pcl::PointCloud; + +namespace +{ + +constexpr float kRes = 0.5f; + +navmap_ros::BuildParams params(float res = kRes) +{ + navmap_ros::BuildParams p; + p.resolution = res; + p.max_dz = 0.25f; + p.max_slope_deg = 30.0f; + return p; +} + +// Dense samples (0.1 m) of z = f(x, y) over [x0, x1] x [y0, y1]. +void sample( + Cloud & cloud, float x0, float x1, float y0, float y1, + const std::function & f, float noise = 0.0f, unsigned seed = 1) +{ + std::mt19937 rng(seed); + std::uniform_real_distribution N(-noise, noise); + for (float x = x0 + 0.05f; x < x1; x += 0.1f) { + for (float y = y0 + 0.05f; y < y1; y += 0.1f) { + cloud.push_back(pcl::PointXYZ(x, y, f(x, y) + (noise > 0.0f ? N(rng) : 0.0f))); + } + } +} + +NavMap build(const Cloud & cloud, navmap_ros::BuildParams p = params()) +{ + navmap_ros_interfaces::msg::NavMap msg; + return navmap_ros::from_points(cloud, msg, p); +} + +float total_area(const NavMap & nm) +{ + float a = 0.0f; + for (const auto & c : nm.navcels) { + a += c.area; + } + return a; +} + +// Max number of cells of one surface stacked at the same height over any sample point. +int max_same_height_cover(const NavMap & nm, int samples = 1500, unsigned seed = 3) +{ + Eigen::Vector3f lo(1e9f, 1e9f, 1e9f), hi(-1e9f, -1e9f, -1e9f); + for (std::size_t v = 0; v < nm.positions.size(); ++v) { + lo = lo.cwiseMin(nm.positions.at(static_cast(v))); + hi = hi.cwiseMax(nm.positions.at(static_cast(v))); + } + std::mt19937 rng(seed); + std::uniform_real_distribution X(lo.x(), hi.x()), Y(lo.y(), hi.y()); + int worst = 0; + for (int s = 0; s < samples; ++s) { + const Eigen::Vector3f o(X(rng), Y(rng), hi.z() + 10.0f), d(0.0f, 0.0f, -1.0f); + std::map> hits; // surface -> hit heights + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + const auto & c = nm.navcels[cid]; + float t, u, v; + if (navmap::ray_triangle_intersect( + o, d, nm.positions.at(c.v[0]), nm.positions.at(c.v[1]), nm.positions.at(c.v[2]), + t, u, v) && u > 1e-3f && v > 1e-3f && u + v < 1.0f - 1e-3f) + { + hits[nm.surface_of(cid)].push_back(o.z() - t); + } + } + for (auto & [s_idx, zs] : hits) { + std::sort(zs.begin(), zs.end()); + int run = 1; + for (std::size_t k = 1; k < zs.size(); ++k) { + run = (zs[k] - zs[k - 1] < 0.2f) ? run + 1 : 1; + worst = std::max(worst, run); + } + worst = std::max(worst, 1); + } + } + return worst; +} + +// Every edge used by at most two cells and every vertex referenced. +void expect_clean_mesh(const NavMap & nm) +{ + EXPECT_EQ(nm.nonmanifold_edges(), 0u); + std::map, int> edge_uses; + std::vector used(nm.positions.size(), 0); + for (const auto & c : nm.navcels) { + for (int e = 0; e < 3; ++e) { + const auto a = c.v[e], b = c.v[(e + 1) % 3]; + ++edge_uses[{std::min(a, b), std::max(a, b)}]; + used[a] = 1; + } + EXPECT_GT(c.normal.z(), 0.0f); // upward facing + } + for (const auto & [edge, n] : edge_uses) { + EXPECT_LE(n, 2); + } + EXPECT_EQ(std::count(used.begin(), used.end(), 0), 0) << "unused vertices"; + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + EXPECT_NE(nm.surface_of(cid), NavMap::kNoSurface); + } + ASSERT_TRUE(nm.has_layer("elevation")); + EXPECT_EQ(nm.layer_size("elevation"), nm.navcels.size()); +} + +bool connected(const NavMap & nm, NavCelId from, NavCelId to) +{ + std::vector seen(nm.navcels.size(), 0); + std::vector stack{from}; + seen[from] = 1; + while (!stack.empty()) { + const NavCelId c = stack.back(); + stack.pop_back(); + if (c == to) {return true;} + for (NavCelId n : nm.navcel_neighbors(c)) { + if (!seen[n]) {seen[n] = 1; stack.push_back(n);} + } + } + return false; +} + +NavCelId cell_at(const NavMap & nm, float x, float y, float z, float * hit_z = nullptr) +{ + std::size_t s; NavCelId cid; Eigen::Vector3f bary, hit; + EXPECT_TRUE(nm.locate_navcel({x, y, z}, s, cid, bary, &hit)) << x << "," << y << "," << z; + if (hit_z) {*hit_z = hit.z();} + return cid; +} + +auto flat(float h) {return [h](float, float) {return h;};} + +} // namespace + +TEST(FromPoints, FlatPlaneIsAFullManifoldGrid) +{ + Cloud cloud; + sample(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f)); + const NavMap nm = build(cloud); + + // Samples span 9.9 m: 21 x 21 cells (half-cell origin shift) -> 20 x 20 quads. + EXPECT_EQ(nm.navcels.size(), 2u * 20u * 20u); + EXPECT_EQ(nm.positions.size(), 21u * 21u); + EXPECT_NEAR(total_area(nm), 10.0f * 10.0f, 1e-2f); + EXPECT_EQ(nm.surfaces.size(), 1u); + expect_clean_mesh(nm); + EXPECT_EQ(max_same_height_cover(nm), 1); +} + +TEST(FromPoints, NoisyPlaneStaysOneSheet) +{ + Cloud cloud; + sample(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f), 0.04f); + const NavMap nm = build(cloud); + EXPECT_EQ(nm.navcels.size(), 2u * 20u * 20u); + EXPECT_EQ(nm.surfaces.size(), 1u); + expect_clean_mesh(nm); + EXPECT_EQ(max_same_height_cover(nm), 1); +} + +TEST(FromPoints, TwoStackedFloorsAreTwoSurfaces) +{ + Cloud cloud; + sample(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f)); + sample(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(3.0f)); + const NavMap nm = build(cloud); + + EXPECT_EQ(nm.navcels.size(), 2u * 2u * 20u * 20u); + ASSERT_EQ(nm.surfaces.size(), 2u); + expect_clean_mesh(nm); + EXPECT_EQ(max_same_height_cover(nm), 1); + + float z_low, z_high; + const NavCelId low = cell_at(nm, 5.2f, 5.3f, 0.05f, &z_low); + const NavCelId high = cell_at(nm, 5.2f, 5.3f, 3.05f, &z_high); + EXPECT_NEAR(z_low, 0.0f, 1e-4f); + EXPECT_NEAR(z_high, 3.0f, 1e-4f); + EXPECT_NE(nm.surface_of(low), nm.surface_of(high)); + EXPECT_FALSE(connected(nm, low, high)); +} + +TEST(FromPoints, RampConnectsGroundAndUpperFloor) +{ + // Ground y in [0, 12]; over y in [0, 6] a 15 deg ramp (x in [10, 20]) leads to an upper + // floor over the ground (x > 20). The ground under the upper floor is reached from the side. + const float slope = std::tan(15.0f * static_cast(M_PI) / 180.0f); + const float h = slope * 10.0f; + Cloud cloud; + sample(cloud, 0.0f, 30.0f, 0.0f, 12.0f, flat(0.0f)); + sample(cloud, 10.0f, 20.0f, 0.0f, 6.0f, [&](float x, float) {return slope * (x - 10.0f);}); + sample(cloud, 20.0f, 30.0f, 0.0f, 6.0f, flat(h)); + const NavMap nm = build(cloud); + + expect_clean_mesh(nm); + EXPECT_EQ(max_same_height_cover(nm), 1); + + float z; + const NavCelId ground = cell_at(nm, 5.0f, 3.0f, 0.05f, &z); + EXPECT_NEAR(z, 0.0f, 1e-3f); + const NavCelId upper = cell_at(nm, 25.0f, 3.0f, h + 0.05f, &z); + EXPECT_NEAR(z, h, 1e-3f); + const NavCelId below_upper = cell_at(nm, 25.0f, 3.0f, 0.05f, &z); + EXPECT_NEAR(z, 0.0f, 1e-3f); + const NavCelId on_ramp = cell_at(nm, 15.2f, 3.0f, slope * 5.2f + 0.05f, &z); + EXPECT_NEAR(z, slope * 5.2f, 0.15f); + const NavCelId under_ramp = cell_at(nm, 17.0f, 3.0f, 0.05f, &z); + EXPECT_NEAR(z, 0.0f, 1e-3f); + + EXPECT_TRUE(connected(nm, ground, upper)); + EXPECT_TRUE(connected(nm, on_ramp, upper)); + EXPECT_TRUE(connected(nm, ground, below_upper)); // around, by y > 6 + EXPECT_NE(upper, below_upper); + EXPECT_NE(on_ramp, under_ramp); + // No cell joins the upper floor straight down to the ground below it. + for (NavCelId n : nm.navcel_neighbors(upper)) { + EXPECT_GT(nm.navcel_centroid(n).z(), h - 0.5f); + } +} + +TEST(FromPoints, HighStepSplitsLowStepJoins) +{ + for (const auto & [step, expected] : {std::pair{0.6f, 2u}, std::pair{0.1f, 1u}}) { + Cloud cloud; + sample(cloud, 0.0f, 10.0f, 0.0f, 5.0f, flat(0.0f)); + sample(cloud, 10.0f, 20.0f, 0.0f, 5.0f, flat(step)); + const NavMap nm = build(cloud); + EXPECT_EQ(nm.surfaces.size(), expected) << "step " << step; + expect_clean_mesh(nm); + } +} + +TEST(FromPoints, SlopeLimit) +{ + for (const auto & [deg, meshed] : {std::pair{20.0f, true}, std::pair{45.0f, false}}) { + const float s = std::tan(deg * static_cast(M_PI) / 180.0f); + Cloud cloud; + sample(cloud, 0.0f, 10.0f, 0.0f, 5.0f, [s](float x, float) {return s * x;}); + const NavMap nm = build(cloud); + if (meshed) { + EXPECT_EQ(nm.navcels.size(), 2u * 20u * 10u) << deg; + expect_clean_mesh(nm); + } else { + // Nothing inside the steep slope; a partial border cell may leave a rim strip. + for (const auto & c : nm.navcels) { + EXPECT_LE(std::acos(c.normal.z()), 30.0f * static_cast(M_PI) / 180.0f + 1e-4f); + } + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + const float x = nm.navcel_centroid(cid).x(); + EXPECT_TRUE(x<0.6f || x>9.4f) << deg << " centroid x " << x; + } + } + } +} + +TEST(FromPoints, IsolatedOutliersAddNothing) +{ + Cloud cloud; + sample(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f)); + std::mt19937 rng(9); + std::uniform_real_distribution U(0.0f, 10.0f); + for (int k = 0; k < 15; ++k) { + cloud.push_back(pcl::PointXYZ(U(rng), U(rng), 1.5f + 0.1f * k)); + } + const NavMap nm = build(cloud); + EXPECT_EQ(nm.navcels.size(), 2u * 20u * 20u); + EXPECT_EQ(nm.surfaces.size(), 1u); + expect_clean_mesh(nm); +} + +TEST(FromPoints, WallSplitsTheFloor) +{ + Cloud cloud; + sample(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f)); + for (float y = 0.05f; y < 10.0f; y += 0.1f) { + for (float z = 0.0f; z < 2.0f; z += 0.05f) { + cloud.push_back(pcl::PointXYZ(5.1f, y, z)); + } + } + const NavMap nm = build(cloud); + expect_clean_mesh(nm); + const NavCelId left = cell_at(nm, 2.0f, 5.0f, 0.05f); + const NavCelId right = cell_at(nm, 8.0f, 5.0f, 0.05f); + EXPECT_FALSE(connected(nm, left, right)); +} + +TEST(FromPoints, MaxSurfacesDropsTrianglesAndVertices) +{ + Cloud cloud; + sample(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f)); + sample(cloud, 0.0f, 4.0f, 0.0f, 4.0f, flat(3.0f)); // smaller upper floor + auto p = params(); + p.max_surfaces = 1; + navmap_ros_interfaces::msg::NavMap msg; + const NavMap nm = navmap_ros::from_points(cloud, msg, p); + + ASSERT_EQ(nm.surfaces.size(), 1u); + EXPECT_EQ(nm.navcels.size(), 2u * 20u * 20u); // the larger, lower floor + EXPECT_EQ(nm.surfaces[0].navcels.size(), nm.navcels.size()); + expect_clean_mesh(nm); + for (std::size_t v = 0; v < nm.positions.size(); ++v) { + EXPECT_NEAR(nm.positions.z[v], 0.0f, 1e-5f); + } + EXPECT_EQ(msg.navcels_v0.size(), nm.navcels.size()); + EXPECT_EQ(msg.layers[0].data_f32.size(), nm.navcels.size()); +} + +TEST(FromPoints, ElevationLayerIsMeanVertexHeight) +{ + Cloud cloud; + sample(cloud, 0.0f, 10.0f, 0.0f, 5.0f, [](float x, float y) {return 0.1f * x + 0.05f * y;}); + const NavMap nm = build(cloud); + ASSERT_FALSE(nm.navcels.empty()); + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + const auto & c = nm.navcels[cid]; + const float mean = (nm.positions.z[c.v[0]] + nm.positions.z[c.v[1]] + + nm.positions.z[c.v[2]]) / 3.0f; + EXPECT_NEAR(nm.layer_get("elevation", cid), mean, 1e-5f); + } +} + +TEST(FromPoints, PointOrderDoesNotMatter) +{ + Cloud cloud; + sample(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f), 0.03f); + sample(cloud, 2.0f, 8.0f, 2.0f, 8.0f, flat(2.8f), 0.03f, 2); + const NavMap a = build(cloud); + std::mt19937 rng(4); + std::shuffle(cloud.points.begin(), cloud.points.end(), rng); + const NavMap b = build(cloud); + + ASSERT_EQ(a.navcels.size(), b.navcels.size()); + EXPECT_EQ(a.surfaces.size(), b.surfaces.size()); + EXPECT_NEAR(total_area(a), total_area(b), 1e-3f); + auto verts = [](const NavMap & nm) { + std::set> s; + for (std::size_t v = 0; v < nm.positions.size(); ++v) { + s.emplace(nm.positions.x[v], nm.positions.y[v], nm.positions.z[v]); + } + return s; + }; + EXPECT_EQ(verts(a), verts(b)); +} + +TEST(FromPoints, DegenerateInputsGiveEmptyMaps) +{ + const float nan = std::numeric_limits::quiet_NaN(); + Cloud empty, two, nans, flat_ok; + two.push_back(pcl::PointXYZ(0, 0, 0)); + two.push_back(pcl::PointXYZ(1, 0, 0)); + for (int k = 0; k < 10; ++k) { + nans.push_back(pcl::PointXYZ(nan, k, 0)); + } + sample(flat_ok, 0.0f, 5.0f, 0.0f, 5.0f, flat(0.0f)); + + EXPECT_TRUE(build(empty).navcels.empty()); + EXPECT_TRUE(build(two).navcels.empty()); + EXPECT_TRUE(build(nans).navcels.empty()); + EXPECT_TRUE(build(flat_ok, params(0.0f)).navcels.empty()); + EXPECT_TRUE(build(flat_ok, params(-1.0f)).navcels.empty()); + + // A single row of cells has no quads. + Cloud row; + sample(row, 0.0f, 10.0f, 0.0f, 0.2f, flat(0.0f)); + EXPECT_TRUE(build(row).navcels.empty()); +} + +TEST(FromPoints, NonFinitePointsAreIgnored) +{ + Cloud cloud; + sample(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f)); + const float inf = std::numeric_limits::infinity(); + cloud.push_back(pcl::PointXYZ(inf, 1.0f, 0.0f)); + cloud.push_back(pcl::PointXYZ(1.0f, std::numeric_limits::quiet_NaN(), 0.0f)); + const NavMap nm = build(cloud); + EXPECT_EQ(nm.navcels.size(), 2u * 20u * 20u); + expect_clean_mesh(nm); +} + +TEST(FromPoints, PointCloud2EntryMatchesPcl) +{ + Cloud cloud; + sample(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f)); + sample(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(3.0f)); + sensor_msgs::msg::PointCloud2 pc2; + pcl::toROSMsg(cloud, pc2); + navmap_ros_interfaces::msg::NavMap msg; + const NavMap from_pc2 = navmap_ros::from_pointcloud2(pc2, msg, params()); + const NavMap from_pcl = build(cloud); + EXPECT_EQ(from_pc2.navcels.size(), from_pcl.navcels.size()); + EXPECT_EQ(from_pc2.surfaces.size(), from_pcl.surfaces.size()); +} + +TEST(FromPoints, MinClearanceDecidesWhatIsALevel) +{ + // Table top 0.7 m over part of the floor. + Cloud cloud; + sample(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f)); + sample(cloud, 3.0f, 6.0f, 3.0f, 6.0f, flat(0.7f)); + + // Default clearance (1 m): table and floor are one column, the floor under it is lost. + const NavMap merged = build(cloud); + expect_clean_mesh(merged); + float z; + cell_at(merged, 4.5f, 4.5f, 0.05f, &z); + EXPECT_GT(z, 0.5f); // only the table top remains there + + // Low robot (0.5 m): the floor under the table is a level of its own. + auto p = params(); + p.min_clearance = 0.5f; + const NavMap split = build(cloud, p); + expect_clean_mesh(split); + cell_at(split, 4.5f, 4.5f, 0.05f, &z); + EXPECT_NEAR(z, 0.0f, 1e-4f); + cell_at(split, 4.5f, 4.5f, 0.75f, &z); + EXPECT_NEAR(z, 0.7f, 1e-4f); +} + +TEST(FromPoints, VoxelizedCloudGivesOneSheet) +{ + // Gentle slope sampled as voxel centres: x, y, z all quantized to 0.3 m. + Cloud cloud; + for (int i = 0; i < 60; ++i) { + for (int j = 0; j < 30; ++j) { + const float x = 0.3f * i, y = 0.3f * j; + const float z = 0.3f * std::round((0.1f * x) / 0.3f); + cloud.push_back(pcl::PointXYZ(x, y, z)); + cloud.push_back(pcl::PointXYZ(x, y, z - 0.3f)); // surface voxels two deep + } + } + const NavMap nm = build(cloud, params(0.6f)); + expect_clean_mesh(nm); + EXPECT_EQ(nm.surfaces.size(), 1u); + // Covers the 17.7 x 8.7 m footprint, up to one cell of border. + EXPECT_GT(total_area(nm), 17.7f * 8.7f * 0.95f); + EXPECT_LT(total_area(nm), 18.3f * 9.3f * 1.05f); +} + +TEST(FromPoints, LatticeAlignedSamplesFillEveryCell) +{ + // One point per cell exactly on a res-spaced lattice (worst case for binning). + Cloud cloud; + for (int i = 0; i < 40; ++i) { + for (int j = 0; j < 40; ++j) { + cloud.push_back(pcl::PointXYZ(0.3f * i, 0.3f * j, 0.0f)); + } + } + const NavMap nm = build(cloud, params(0.3f)); + EXPECT_EQ(nm.positions.size(), 40u * 40u); + EXPECT_EQ(nm.navcels.size(), 2u * 39u * 39u); + expect_clean_mesh(nm); +} + +TEST(FromPoints, MinSurfaceAreaDropsSmallSurfaces) +{ + Cloud cloud; + sample(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f)); + sample(cloud, 0.0f, 2.0f, 0.0f, 2.0f, flat(3.0f)); // ~2.25 m2 upper patch + auto p = params(); + p.min_surface_area = 5.0f; + const NavMap nm = build(cloud, p); + ASSERT_EQ(nm.surfaces.size(), 1u); + EXPECT_EQ(nm.navcels.size(), 2u * 20u * 20u); + expect_clean_mesh(nm); + + p.min_surface_area = 1000.0f; // nothing survives + EXPECT_TRUE(build(cloud, p).navcels.empty()); +} + +namespace +{ + +// Plane points at random XY positions, `per_m2` points per square meter. +void scatter( + Cloud & cloud, float x0, float x1, float y0, float y1, + const std::function & f, float per_m2, unsigned seed, + const std::function & skip = nullptr) +{ + std::mt19937 rng(seed); + std::uniform_real_distribution X(x0, x1), Y(y0, y1); + const int n = static_cast((x1 - x0) * (y1 - y0) * per_m2); + for (int k = 0; k < n; ++k) { + const float x = X(rng), y = Y(rng); + if (skip && skip(x, y)) {continue;} + cloud.push_back(pcl::PointXYZ(x, y, f(x, y))); + } +} + +bool has_cell_near(const NavMap & nm, float x, float y, float z, float tol_z = 0.1f) +{ + std::size_t s; NavCelId cid; Eigen::Vector3f bary, hit; + return nm.locate_navcel({x, y, z}, s, cid, bary, &hit) && + std::fabs(hit.x() - x) < 1e-3f && std::fabs(hit.z() - z) < tol_z; +} + +uint8_t support_at(const NavMap & nm, float x, float y, float z) +{ + return nm.layer_get("support", cell_at(nm, x, y, z)); +} + +} // namespace + +TEST(FromPoints, SpikesDoNotRaiseTheSurface) +{ + Cloud cloud; + scatter(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f), 200.0f, 5); + std::mt19937 rng(6); + std::uniform_real_distribution U(0.0f, 1.0f); + for (auto & p : cloud.points) { + if (U(rng) < 0.02f) {p.z += 0.2f * U(rng);} // 2% spurious returns above the floor + } + const NavMap nm = build(cloud); + expect_clean_mesh(nm); + for (std::size_t v = 0; v < nm.positions.size(); ++v) { + EXPECT_NEAR(nm.positions.z[v], 0.0f, 0.02f) << "vertex " << v; + } +} + +TEST(FromPoints, SlopeHeightIsTakenAtTheCellCentre) +{ + // Irregular samples of z = 0.2 x + 0.1 y: vertices must lie on the plane, not at cell tops. + Cloud cloud; + scatter(cloud, 0.0f, 10.0f, 0.0f, 10.0f, [](float x, float y) {return 0.2f * x + 0.1f * y;}, + 60.0f, 7); + const NavMap nm = build(cloud); + expect_clean_mesh(nm); + // Plane fit is exact with 3+ points; with fewer (border cells) the error is bounded by + // the slope over half a cell. + std::vector min_support(nm.positions.size(), 255); + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + const auto sup = nm.layer_get("support", cid); + for (auto v : nm.navcels[cid].v) { + min_support[v] = std::min(min_support[v], sup); + } + } + for (std::size_t v = 0; v < nm.positions.size(); ++v) { + const float expected = 0.2f * nm.positions.x[v] + 0.1f * nm.positions.y[v]; + const float tol = min_support[v] >= 3 ? 1e-3f : (0.2f + 0.1f) * 0.5f * kRes; + EXPECT_NEAR(nm.positions.z[v], expected, tol) << "vertex " << v; + } +} + +TEST(FromPoints, IrregularDensityStaysOneCleanSheet) +{ + // Dense near the origin, sparse far away, like an accumulated lidar scan. + Cloud cloud; + std::mt19937 rng(8); + std::uniform_real_distribution R(0.0f, 1.0f), A(0.0f, 6.2831853f); + for (int k = 0; k < 60000; ++k) { + const float r = 15.0f * std::sqrt(R(rng)) * R(rng); // density decays with range + const float a = A(rng); + cloud.push_back(pcl::PointXYZ(r * std::cos(a), r * std::sin(a), 0.0f)); + } + const NavMap nm = build(cloud); + expect_clean_mesh(nm); + EXPECT_EQ(max_same_height_cover(nm), 1); + EXPECT_TRUE(has_cell_near(nm, 0.2f, 0.3f, 0.0f)); + EXPECT_TRUE(has_cell_near(nm, 6.0f, 0.0f, 0.0f)); +} + +TEST(FromPoints, BoxOnTheFloorIsAHole) +{ + Cloud cloud; + auto in_box = [](float x, float y) {return x > 4.0f && x < 6.0f && y > 4.0f && y < 6.0f;}; + scatter(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f), 100.0f, 9, in_box); + scatter(cloud, 4.0f, 6.0f, 4.0f, 6.0f, flat(0.4f), 100.0f, 10); // box top + for (float z = 0.0f; z < 0.4f; z += 0.05f) { // box sides + for (float t = 4.0f; t < 6.0f; t += 0.05f) { + for (const auto & p : {pcl::PointXYZ(4.0f, t, z), pcl::PointXYZ(6.0f, t, z), + pcl::PointXYZ(t, 4.0f, z), pcl::PointXYZ(t, 6.0f, z)}) + { + cloud.push_back(p); + } + } + } + const NavMap nm = build(cloud); + expect_clean_mesh(nm); + // No floor under the box, and the box edge never joins the floor. + EXPECT_FALSE(has_cell_near(nm, 5.0f, 5.0f, 0.0f)); + const NavCelId floor_cell = cell_at(nm, 1.0f, 1.0f, 0.0f); + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + if (nm.navcel_centroid(cid).z() > 0.2f) { + EXPECT_FALSE(connected(nm, cid, floor_cell)); + break; + } + } +} + +TEST(FromPoints, SmallEnclosedHoleIsFilledAndMarked) +{ + Cloud cloud; + auto gap = [](float x, float y) {return x > 4.0f && x < 5.0f && y > 4.0f && y < 5.0f;}; + scatter(cloud, 0.0f, 10.0f, 0.0f, 10.0f, [](float x, float) {return 0.1f * x;}, 100.0f, 11, + gap); + const NavMap nm = build(cloud); // max_hole_area 1 m2 by default + expect_clean_mesh(nm); + EXPECT_EQ(nm.surfaces.size(), 1u); + ASSERT_TRUE(has_cell_near(nm, 4.5f, 4.5f, 0.45f, 0.02f)); + EXPECT_EQ(support_at(nm, 4.5f, 4.5f, 0.45f), 0u); // interpolated + EXPECT_GT(support_at(nm, 1.5f, 1.5f, 0.15f), 0u); // observed +} + +TEST(FromPoints, HoleFillingLimits) +{ + auto plane_with_gap = [](float gx0, float gx1, float gy0, float gy1) { + Cloud c; + scatter(c, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f), 100.0f, 12, + [ = ](float x, float y) {return x > gx0 && x < gx1 && y > gy0 && y < gy1;}); + return c; + }; + // Larger than max_hole_area: stays open. + const NavMap big = build(plane_with_gap(3.0f, 6.0f, 3.0f, 6.0f)); + EXPECT_FALSE(has_cell_near(big, 4.5f, 4.5f, 0.0f)); + + // Filling disabled. + auto p = params(); + p.max_hole_area = 0.0f; + const NavMap off = build(plane_with_gap(4.0f, 5.0f, 4.0f, 5.0f), p); + EXPECT_FALSE(has_cell_near(off, 4.5f, 4.5f, 0.0f)); + + // A notch open to the outside of the data is not a hole. + const NavMap notch = build(plane_with_gap(-1.0f, 1.0f, 4.0f, 5.0f)); + EXPECT_FALSE(has_cell_near(notch, 0.25f, 4.5f, 0.0f)); +} + +TEST(FromPoints, PitWithObservedBottomIsNotFilled) +{ + // A 1 x 1 m pit, 1.5 m deep, whose bottom was seen: never bridged at floor level. + Cloud cloud; + auto pit = [](float x, float y) {return x > 4.0f && x < 5.0f && y > 4.0f && y < 5.0f;}; + scatter(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f), 100.0f, 13, pit); + scatter(cloud, 4.0f, 5.0f, 4.0f, 5.0f, flat(-1.5f), 100.0f, 14); + const NavMap nm = build(cloud); + expect_clean_mesh(nm); + EXPECT_FALSE(has_cell_near(nm, 4.5f, 4.5f, 0.0f)); +} + +TEST(FromPoints, HoleSharedByTwoFloorsIsFilledOnBoth) +{ + Cloud cloud; + auto gap = [](float x, float y) {return x > 4.0f && x < 5.0f && y > 4.0f && y < 5.0f;}; + scatter(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f), 100.0f, 15, gap); + scatter(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(3.0f), 100.0f, 16, gap); + const NavMap nm = build(cloud); + expect_clean_mesh(nm); + ASSERT_EQ(nm.surfaces.size(), 2u); + EXPECT_TRUE(has_cell_near(nm, 4.5f, 4.5f, 0.0f, 0.02f)); + EXPECT_TRUE(has_cell_near(nm, 4.5f, 4.5f, 3.0f, 0.02f)); +} + +TEST(FromPoints, SupportCountsPointsOfTheWeakestVertex) +{ + // Exactly 4 points per cell: every observed triangle has support 4. + Cloud cloud; + for (int i = 0; i < 20; ++i) { + for (int j = 0; j < 20; ++j) { + // Cells are centred on the first sample (half-cell origin shift). + for (const auto & [dx, dy] : {std::pair{0.0f, 0.0f}, std::pair{0.1f, 0.0f}, + std::pair{0.0f, 0.1f}, std::pair{0.1f, 0.1f}}) + { + cloud.push_back(pcl::PointXYZ(0.5f * i + dx, 0.5f * j + dy, 0.0f)); + } + } + } + const NavMap nm = build(cloud); + ASSERT_FALSE(nm.navcels.empty()); + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + EXPECT_EQ(nm.layer_get("support", cid), 4u); + } +} + +TEST(FromPoints, SlabSupportIsItsUpperFace) +{ + // An upper floor seen from above and from below (its ceiling face), for thin and thick slabs: + // the vertex must sit on the upper face, not between both faces. + for (const float thickness : {0.1f, 0.2f, 0.3f}) { + Cloud cloud; + scatter(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f), 40.0f, 21); + scatter(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(2.5f - thickness), 40.0f, 22); // ceiling face + scatter(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(2.5f), 40.0f, 23); // upper face + const NavMap nm = build(cloud); + expect_clean_mesh(nm); + EXPECT_EQ(nm.surfaces.size(), 2u) << thickness; + float z; + cell_at(nm, 5.2f, 5.3f, 2.6f, &z); + // Faces closer than max_dz / 2 cannot be told apart from noise: error up to half the slab. + const float tol = thickness > 0.5f * params().max_dz ? 0.01f : 0.5f * thickness; + EXPECT_NEAR(z, 2.5f, tol) << "thickness " << thickness; + cell_at(nm, 5.2f, 5.3f, 0.1f, &z); + EXPECT_NEAR(z, 0.0f, 0.01f) << "thickness " << thickness; + } +} + +TEST(FromPoints, SparseUpperFaceOfASlabIsRecoveredByItsNeighbours) +{ + // Upper floor at 2.5 m seen mostly from below: its ceiling face is dense, its top face sparse + // (about 2 points per cell, many with none). The floor must come out whole, at the top face. + Cloud cloud; + scatter(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f), 100.0f, 31); + scatter(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(2.21f), 100.0f, 32); // ceiling face + scatter(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(2.5f), 25.0f, 33); // sparse top face + const NavMap nm = build(cloud, params(0.3f)); + expect_clean_mesh(nm); + ASSERT_EQ(nm.surfaces.size(), 2u); + float upper_area = 0.0f; + for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { + if (nm.navcel_centroid(cid).z() > 1.0f) { + upper_area += nm.navcels[cid].area; + EXPECT_NEAR(nm.navcel_centroid(cid).z(), 2.5f, 0.02f); + } + } + EXPECT_GT(upper_area, 0.95f * 9.6f * 9.6f); +} + +TEST(FromPoints, PotholeIsNotLiftedToTheFloor) +{ + // A 0.6 x 0.6 m depression 0.3 m deep: its points are below the floor, like a slab seen from + // below, but the floor around has no lower face there, so it must not be lifted. + Cloud cloud; + auto pothole = [](float x, float y) {return x > 4.5f && x < 5.1f && y > 4.5f && y < 5.1f;}; + scatter(cloud, 0.0f, 10.0f, 0.0f, 10.0f, flat(0.0f), 100.0f, 41, pothole); + scatter(cloud, 4.5f, 5.1f, 4.5f, 5.1f, flat(-0.3f), 100.0f, 42); + const NavMap nm = build(cloud, params(0.3f)); + expect_clean_mesh(nm); + EXPECT_FALSE(has_cell_near(nm, 4.8f, 4.8f, 0.0f, 0.05f)); +}