navmap_core, navmap_ros: fixes for large maps (DistanceField, set_are…

navmap_core/include/navmap_core/NavMap.hpp

@@ -36,6 +36,7 @@
 #include <vector>
 #include <memory>
 #include <unordered_map>
+#include <unordered_set>
 #include <optional>
 #include <string>
 #include <array>
@@ -1172,7 +1173,11 @@ class NavMap
    * cannot be determined, returns false. Supports U8/F32/F64 layers.
    *
    * Efficiency: BFS from the seed NavCel with XY AABB pruning against the
-   * circumscribed square of the area. Inclusion test by triangle centroid.
+   * circumscribed square of the area. Inclusion test by triangle centroid. The work and the
+   * memory are those of the area, not of the map.
+   *
+   * Every NavCel written is recorded as changed in the layer (as layer_set() does), so that the
+   * incremental consumers of the layer (e.g. an inflation filter) see the new values.
    *
    * \tparam T uint8_t, int16_t, float, or double
    * \param p_world 3D point in world coordinates (projected to ground)
@@ -1252,10 +1257,10 @@ class NavMap
         }
       };
 
-    std::vector<char> visited(navcels.size(), 0);
+    // visited: only the NavCels of the area (a per-map vector cost O(map) on every call)
+    std::unordered_set<NavCelId> visited{start_cid};
     std::deque<NavCelId> q;
     q.push_back(start_cid);
-    visited[static_cast<std::size_t>(start_cid)] = 1;
 
     auto & data = layer->mutable_data();
 
@@ -1269,14 +1274,13 @@ class NavMap
       const Eigen::Vector3f c = navcel_centroid(cid);
       if (inside_area(c)) {
         data[static_cast<std::size_t>(cid)] = value;
+        layer->mark_changed(cid);
       }
 
       for (const auto ncid : navcel_neighbors(cid)) {
         const std::size_t nidx = static_cast<std::size_t>(ncid);
-        if (nidx < navcels.size() && !visited[nidx] && tri_aabb_intersects_box(ncid)) {
-          visited[nidx] = 1;
-          q.push_back(ncid);
-        }
+        if (nidx >= navcels.size() || !tri_aabb_intersects_box(ncid)) {continue;}
+        if (visited.insert(ncid).second) {q.push_back(ncid);}
       }
     }
 

navmap_core/src/navmap_core/DistanceField.cpp

@@ -140,28 +140,34 @@ std::int64_t DistanceField::bucket_key(int bx, int by, int bz)
          (static_cast<std::int64_t>(bz) * 83492791);
 }
 
+// A seed goes into the buckets of points sampled along it every bucket_ metres (one bucket for a
+// point seed), not into every bucket of its bounding box: a long diagonal open edge (the coarse
+// NavCels of a level-of-detail mesh have edges of tens of metres) would otherwise fill hundreds
+// of buckets, and the index cost would grow with the square of its length. Every point of the
+// segment is then within bucket_ / 2 of a sample, so within one bucket of an indexed one: the
+// queries look one bucket further (see update()).
 void DistanceField::index_seed(std::uint32_t s, bool insert)
 {
-  const Eigen::Vector3f lo = seeds_[s].a.cwiseMin(seeds_[s].b) / bucket_;
-  const Eigen::Vector3f hi = seeds_[s].a.cwiseMax(seeds_[s].b) / bucket_;
-  for (int bx = static_cast<int>(std::floor(lo.x())); bx <= static_cast<int>(std::floor(hi.x()));
-    ++bx)
-  {
-    for (int by = static_cast<int>(std::floor(lo.y())); by <= static_cast<int>(std::floor(hi.y()));
-      ++by)
-    {
-      for (int bz =
-        static_cast<int>(std::floor(lo.z())); bz <= static_cast<int>(std::floor(hi.z()));
-        ++bz)
-      {
-        auto & v = buckets_[bucket_key(bx, by, bz)];
-        if (insert) {
-          v.push_back(s);
-        } else {
-          const auto it = std::find(v.begin(), v.end(), s);
-          if (it != v.end()) {*it = v.back(); v.pop_back();}
-        }
-      }
+  const Eigen::Vector3f a = seeds_[s].a, b = seeds_[s].b;
+  const int steps = std::max(1, static_cast<int>(std::ceil((b - a).norm() / bucket_)));
+  std::vector<std::int64_t> keys;
+  keys.reserve(static_cast<std::size_t>(steps) + 1);
+  for (int k = 0; k <= steps; ++k) {
+    const Eigen::Vector3f p = (a + (b - a) * (static_cast<float>(k) / steps)) / bucket_;
+    keys.push_back(
+      bucket_key(
+        static_cast<int>(std::floor(p.x())), static_cast<int>(std::floor(p.y())),
+        static_cast<int>(std::floor(p.z()))));
+  }
+  std::sort(keys.begin(), keys.end());
+  keys.erase(std::unique(keys.begin(), keys.end()), keys.end());
+  for (const auto key : keys) {
+    auto & v = buckets_[key];
+    if (insert) {
+      v.push_back(s);
+    } else {
+      const auto it = std::find(v.begin(), v.end(), s);
+      if (it != v.end()) {*it = v.back(); v.pop_back();}
     }
   }
 }
@@ -270,15 +276,15 @@ void DistanceField::update(
     const float r2 = 2.0f * max_distance_;
     const Eigen::Vector3f lo = (p.array() - r2).matrix() / bucket_;
     const Eigen::Vector3f hi = (p.array() + r2).matrix() / bucket_;
-    for (int bx = static_cast<int>(std::floor(lo.x())); bx <= static_cast<int>(std::floor(hi.x()));
-      ++bx)
+    // one bucket more on every side: a segment seed is indexed by samples along it (index_seed)
+    for (int bx = static_cast<int>(std::floor(lo.x())) - 1;
+      bx <= static_cast<int>(std::floor(hi.x())) + 1; ++bx)
     {
-      for (int by =
-        static_cast<int>(std::floor(lo.y())); by <= static_cast<int>(std::floor(hi.y()));
-        ++by)
+      for (int by = static_cast<int>(std::floor(lo.y())) - 1;
+        by <= static_cast<int>(std::floor(hi.y())) + 1; ++by)
       {
-        for (int bz = static_cast<int>(std::floor(lo.z()));
-          bz <= static_cast<int>(std::floor(hi.z())); ++bz)
+        for (int bz = static_cast<int>(std::floor(lo.z())) - 1;
+          bz <= static_cast<int>(std::floor(hi.z())) + 1; ++bz)
         {
           const auto it = buckets_.find(bucket_key(bx, by, bz));
           if (it == buckets_.end()) {continue;}

navmap_core/src/navmap_core/NavMap.cpp

@@ -144,6 +144,11 @@ NavMap & NavMap::operator=(const NavMap & other)
     for (const auto & name : other.layers.list()) {
       auto src_base = other.layers.get(name);
       if (!src_base) {continue;}
+      // A layer of the same name and another type (re-created by the source): replaced, not
+      // synchronised (add_or_get would give no view for the new type)
+      if (const auto old = layers.get(name); old && old->type() != src_base->type()) {
+        layers.remove(name);
+      }
 
       switch (src_base->type()) {
         case LayerType::U8: {

navmap_core/tests/test_incremental.cpp

@@ -16,6 +16,7 @@
 #include <gtest/gtest.h>
 
 #include <algorithm>
+#include <chrono>
 #include <cmath>
 #include <cstdint>
 #include <functional>
@@ -60,7 +61,7 @@ void add_grid(
 
 std::vector<NavMap> maps()
 {
-  std::vector<NavMap> out(3);
+  std::vector<NavMap> out(4);
   add_grid(
     out[0], 20, 20, 0.5f, [](float, float) {return 0.0f;},
     [](int i, int j) {
@@ -69,6 +70,17 @@ std::vector<NavMap> maps()
   add_grid(out[1], 16, 16, 0.5f, [](float, float) {return 0.0f;});
   add_grid(out[1], 16, 16, 0.5f, [](float, float) {return 3.0f;});
   add_grid(out[2], 30, 6, 0.4f, [](float x, float) {return 0.25f * x;});
+  // Coarse cells (4 m) with holes, turned 30 deg: long diagonal open edges, as the far NavCels of
+  // a level-of-detail mesh (the seed index samples along them, see DistanceField::index_seed)
+  add_grid(
+    out[3], 12, 12, 4.0f, [](float, float) {return 0.0f;},
+    [](int i, int j) {return (i * 7 + j * 3) % 5 == 0;});
+  const float c = std::cos(0.5236f), s = std::sin(0.5236f);
+  for (std::size_t v = 0; v < out[3].positions.x.size(); ++v) {
+    const float x = out[3].positions.x[v], y = out[3].positions.y[v];
+    out[3].positions.x[v] = c * x - s * y;
+    out[3].positions.y[v] = s * x + c * y;
+  }
   for (auto & m : out) {
     m.rebuild_geometry_accels();
   }
@@ -279,6 +291,46 @@ TEST(DistanceFieldIncremental, AnUpdateOnlyTouchesItsNeighbourhood)
   }
 }
 
+TEST(DistanceFieldIncremental, LongDiagonalOpenEdgesCostAboutAsMuchAsShortOnes)
+{
+  // The same mesh (a grid with 20 % holes) with cells of 0.5 m and of 16 m, turned 45 deg: only the
+  // length of the open edges changes. Indexed by their bounding boxes, the 16 m one took ~600
+  // times longer; sampled along them, it is within a small factor.
+  auto build = [](float res) {
+      NavMap nm;
+      std::mt19937 rng(9);
+      std::vector<char> hole(120 * 120);
+      for (auto & h : hole) {
+        h = rng() % 5 == 0;
+      }
+      add_grid(
+        nm, 120, 120, res, [](float, float) {return 0.0f;},
+        [&hole](int i, int j) {return hole[static_cast<std::size_t>(j) * 120 + i] != 0;});
+      const float c = std::cos(0.7854f), s = std::sin(0.7854f);
+      for (std::size_t v = 0; v < nm.positions.x.size(); ++v) {
+        const float x = nm.positions.x[v], y = nm.positions.y[v];
+        nm.positions.x[v] = c * x - s * y;
+        nm.positions.y[v] = s * x + c * y;
+      }
+      nm.rebuild_geometry_accels();
+      return nm;
+    };
+  auto seconds = [](const NavMap & nm) {
+      double best = 1e9;
+      for (int k = 0; k < 3; ++k) {
+        const auto t0 = std::chrono::steady_clock::now();
+        DistanceField df(nm, true, 0.6f);
+        df.compute({});
+        best = std::min(
+          best, std::chrono::duration<double>(std::chrono::steady_clock::now() - t0).count());
+      }
+      return best;
+    };
+  const NavMap fine = build(0.5f), coarse = build(16.0f);
+  const double t_fine = seconds(fine), t_coarse = seconds(coarse);
+  EXPECT_LT(t_coarse, 25.0 * t_fine + 0.01) << "fine " << t_fine << " s, coarse " << t_coarse;
+}
+
 // ------------------------------------------------------------------ layer change tracking
 
 TEST(LayerChanges, SingleWritesAreRecordedOnceAndBulkWritesMarkTheLayer)

navmap_core/tests/test_navmap_assign.cpp

@@ -284,3 +284,26 @@ TEST(NavMapAssign, CopiesNeverShareLayerData)
   EXPECT_EQ(moved.layer_get<uint8_t>("occ", 0), 1);
   EXPECT_FLOAT_EQ(moved.layer_get<float>("extra", 0), 3.0f);
 }
+
+TEST(NavMapAssign, ALayerReCreatedWithAnotherTypeIsReplacedNotDereferencedAsNull)
+{
+  // The source has "occ" as F32 where the destination (same geometry) has it as U8: the copy
+  // is a F32 layer. Before, operator= asked the destination for a F32 view of its U8 layer, got
+  // none and crashed (NavState::set of a map copied, edited and set back).
+  NavMap dst;
+  fill_one_tri_map(dst);
+  NavMap src = dst;
+  src.layers.remove("occ");
+  src.layer_set<float>("occ", 0, 3.5f);
+  dst = src;
+  ASSERT_TRUE(dst.layers.get("occ"));
+  EXPECT_EQ(dst.layers.get("occ")->type(), navmap::LayerType::F32);
+  EXPECT_FLOAT_EQ(dst.layer_get<float>("occ", 0, 0.0f), 3.5f);
+  // and back to U8
+  NavMap again = dst;
+  again.layers.remove("occ");
+  again.layer_set<uint8_t>("occ", 0, 7);
+  dst = again;
+  EXPECT_EQ(dst.layers.get("occ")->type(), navmap::LayerType::U8);
+  EXPECT_EQ(dst.layer_get<uint8_t>("occ", 0, 0), 7);
+}

navmap_core/tests/test_navmap_easy.cpp

@@ -484,3 +484,30 @@ TEST(NavMap_SetArea_Systematic, CircularNearBoundary)
     nm, "obstacles", static_cast<uint8_t>(180),
     [&](const Eigen::Vector3f & cc) {return centroid_inside_circle(cc, cx, cy, r);});
 }
+
+TEST(NavMap_SetArea_Systematic, TheWrittenNavCelsAreRecordedAsChangedForIncrementalConsumers)
+{
+  // An incremental consumer (an inflation filter) reads the layer's changed() list: set_area
+  // records there exactly the NavCels it wrote, as layer_set does
+  NavMap nm;
+  make_grid(nm, /*nx=*/ 20, /*ny=*/ 20);
+  nm.add_layer<uint8_t>("obstacles", "occupancy obstacles", "%", 0);
+  const auto view =
+    std::dynamic_pointer_cast<navmap::LayerView<uint8_t>>(nm.layers.get("obstacles"));
+  ASSERT_TRUE(view);
+  view->clear_changes();
+  const float cx = 0.5f, cy = 0.5f, r = 0.22f;
+  ASSERT_TRUE(nm.set_area<uint8_t>(
+      Eigen::Vector3f(cx, cy, 1.0f), static_cast<uint8_t>(254), "obstacles",
+      navmap::AreaShape::CIRCULAR, r));
+  std::vector<navmap::NavCelId> written;
+  for (navmap::NavCelId c = 0; c < nm.navcels.size(); ++c) {
+    if (nm.layer_get<uint8_t>("obstacles", c, 0) == 254) {written.push_back(c);}
+  }
+  ASSERT_FALSE(written.empty());
+  std::vector<navmap::NavCelId> changed = view->changed();
+  std::sort(changed.begin(), changed.end());
+  changed.erase(std::unique(changed.begin(), changed.end()), changed.end());
+  EXPECT_EQ(changed, written);
+  EXPECT_TRUE(view->has_changes());
+}

navmap_ros/include/navmap_ros/navmap_io.hpp

@@ -61,8 +61,18 @@ struct SaveOptions
   bool binary{true};
   Compression compression{Compression::ZLIB};
   int level{6};  ///< zlib level, 1 (fast) to 9 (small)
+  /// The largest message (serialised CDR bytes) the saver attempts. The CDR serialiser of ROS 2
+  /// (Fast-CDR) cannot hold more than 4 GiB: a bigger map is refused before anything is
+  /// allocated, with std::errc::file_too_large (about 180 million NavCels with one layer).
+  std::uint64_t max_serialized_bytes{(std::uint64_t{1} << 32) - (std::uint64_t{1} << 20)};
 };
 
+/**
+ * @brief A close upper estimate of the CDR size of @p msg (bytes), without serialising it: the
+ *        arrays and their length prefixes, plus a fixed allowance for the headers and strings.
+ */
+std::uint64_t estimated_serialized_size(const navmap_ros_interfaces::msg::NavMap & msg);
+
 /**
  * @brief Save a `NavMap` message to disk using ROS 2 CDR serialization.
  *

navmap_ros/src/navmap_ros/navmap_io.cpp

@@ -53,13 +53,52 @@ inline void set_error(std::error_code * ec, int ev = EIO)
 
 }  // namespace
 
+std::uint64_t estimated_serialized_size(const navmap_ros_interfaces::msg::NavMap & msg)
+{
+  // each sequence: its elements (with alignment) and a 4-byte length; strings and headers: a
+  // generous fixed allowance each
+  auto seq = [](std::size_t n, std::size_t elem) {
+      return static_cast<std::uint64_t>(n) * elem + 4 + 8;
+    };
+  constexpr std::uint64_t kSmall = 512;
+  std::uint64_t n = kSmall;   // header, scalars
+  n += seq(msg.positions_x.size(), 4) + seq(msg.positions_y.size(), 4) +
+    seq(msg.positions_z.size(), 4);
+  n += seq(msg.colors_r.size(), 1) + seq(msg.colors_g.size(), 1) + seq(msg.colors_b.size(), 1) +
+    seq(msg.colors_a.size(), 1);
+  n += seq(msg.navcels_v0.size(), 4) + seq(msg.navcels_v1.size(), 4) +
+    seq(msg.navcels_v2.size(), 4);
+  for (const auto & s : msg.surfaces) {
+    n += kSmall + s.frame_id.size() + seq(s.navcels.size(), 4);
+  }
+  for (const auto & l : msg.layers) {
+    n += kSmall + l.name.size() + l.description.size() + l.unit.size() +
+      seq(l.indices.size(), 4) + seq(l.data_u8.size(), 1) + seq(l.data_f32.size(), 4) +
+      seq(l.data_f64.size(), 8) + seq(l.data_i16.size(), 2);
+  }
+  n += static_cast<std::uint64_t>(msg.links.size()) * kSmall;
+  return n;
+}
+
 bool save_msg_to_file(
   const navmap_ros_interfaces::msg::NavMap & msg,
   const std::string & path,
   const SaveOptions & options,
   std::error_code * ec)
 {
   using MsgT = navmap_ros_interfaces::msg::NavMap;
+  // Beyond what the CDR serialiser holds: refused before allocating gigabytes, with the cause
+  // (it threw a non-std exception from deep inside, reported as a bare I/O error)
+  const std::uint64_t estimate = estimated_serialized_size(msg);
+  if (estimate > options.max_serialized_bytes) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("navmap_io"),
+      "cannot save %s: the map serialises to about %.2f GB, more than the %.2f GB a ROS 2 (CDR) "
+      "message holds; save it in pieces or coarser", path.c_str(), estimate / 1e9,
+      options.max_serialized_bytes / 1e9);
+    set_error(ec, EFBIG);
+    return false;
+  }
   try {
     rclcpp::SerializedMessage serialized;
     rclcpp::Serialization<MsgT> ser;