Rewrite from_points as a multi-level grid mesher

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<std::uint32_t> navcel_surface_;
   void build_navcel_surface_index_();

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<NavCelId>::max();
+  struct EdgeUse
   {
-    size_t operator()(const EdgeKey & k) const
-    {
-      return static_cast<size_t>(k.a) * 73856093u ^
-             static_cast<size_t>(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<EdgeKey, std::pair<NavCelId, int>, EdgeHash, EdgeEq> E;
+  std::vector<EdgeUse> 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;
   }
 }
 

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}
+)

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 <gtest/gtest.h>
+
+#include <algorithm>
+#include <cstdint>
+#include <limits>
+#include <vector>
+
+#include "navmap_core/NavMap.hpp"
+
+using navmap::NavCelId;
+using navmap::NavMap;
+
+namespace
+{
+
+constexpr NavCelId kNone = std::numeric_limits<NavCelId>::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<float>(k);
+    const auto apex = nm.add_vertex({0.5f, a, 0.1f * static_cast<float>(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<NavCelId>{1});
+  EXPECT_EQ(nm.navcel_neighbors(1), std::vector<NavCelId>{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<NavCelId>{t3});
+  EXPECT_EQ(nm.navcel_neighbors(t3), std::vector<NavCelId>{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<uint32_t>(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));
+}

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<pcl::PointXYZ> & input_points,
   navmap_ros_interfaces::msg::NavMap & out_msg,