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

navmap_core/include/navmap_core/DistanceField.hpp

@@ -77,6 +77,9 @@ class DistanceField
   /// Whether @p cid is currently a seed cell.
   bool is_seed(NavCelId cid) const {return cid < seed_of_.size() && seed_of_[cid] != kNoSeed;}
 
+  /// Entries of the spatial index of the seeds (a seed has one per bucket it is sampled in).
+  std::size_t index_entries() const;
+
 private:
   static constexpr std::uint32_t kNoSeed = std::numeric_limits<std::uint32_t>::max();
   struct Seed
@@ -102,6 +105,7 @@ class DistanceField
   float max_distance_;
   float bucket_{1.0f};
   std::unordered_map<std::int64_t, std::vector<std::uint32_t>> buckets_;  ///< seed index
+  std::vector<std::int64_t> keys_scratch_;     ///< index_seed()'s buckets, reused
   std::vector<Eigen::Vector3f> centroid_;
   std::vector<Seed> seeds_;                    ///< open-edge seeds first, then point seeds
   std::size_t n_boundary_{0};

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>
@@ -312,8 +313,9 @@ class LayerRegistry
   /**
    * \brief Add a new typed layer or return an existing one with the same name.
    *
-   * If a layer with \p name already exists, it is returned (no resize).
-   * Otherwise a new layer is created with \p nitems elements.
+   * If a layer with \p name already exists with the type \p T, it is returned (no resize).
+   * Otherwise (none, or one of another type) a new layer is created with \p nitems elements; a
+   * layer of another type is replaced, and its data is lost.
    *
    * \tparam T Storage type (e.g., uint8_t, float, double).
    * \param name   Layer name (unique key).
@@ -329,7 +331,7 @@ class LayerRegistry
   {
     auto it = layers_.find(name);
     if (it != layers_.end()) {
-      return std::dynamic_pointer_cast<LayerView<T>>(it->second);
+      if (auto same = std::dynamic_pointer_cast<LayerView<T>>(it->second)) {return same;}
     }
     auto view = std::make_shared<LayerView<T>>(name, nitems, type);
     layers_[name] = view;
@@ -880,7 +882,8 @@ class NavMap
     const std::string & unit = {},
     T default_value = T{})
   {
-    const bool existed = has_layer(name);
+    // a layer of another type is replaced by a new one (add_or_get)
+    const bool existed = std::dynamic_pointer_cast<LayerView<T>>(layers.get(name)) != nullptr;
     auto v = layers.add_or_get<T>(name, navcels.size(), layer_type_tag<T>());
     if (!existed || v->data().size() != navcels.size()) {  // new layers start at default_value
       v->data().assign(navcels.size(), default_value);
@@ -1172,7 +1175,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 +1259,28 @@ class NavMap
         }
       };
 
-    std::vector<char> visited(navcels.size(), 0);
+    // Visited NavCels: a hash set while the area is small (a vector per map would cost O(map) on
+    // every call), a flag per NavCel once it is large (a hash set costs ~50 bytes per NavCel)
+    std::unordered_set<NavCelId> seen;
+    std::vector<char> flags;
+    const std::size_t to_flags = navcels.size() / 32 + 1;
+    auto first_visit = [&](NavCelId c) -> bool {
+        if (!flags.empty()) {
+          if (flags[c]) {return false;}
+          flags[c] = 1;
+          return true;
+        }
+        if (!seen.insert(c).second) {return false;}
+        if (seen.size() > to_flags) {
+          flags.assign(navcels.size(), 0);
+          for (const auto s : seen) {flags[s] = 1;}
+          std::unordered_set<NavCelId>().swap(seen);
+        }
+        return true;
+      };
+    first_visit(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 +1294,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 (first_visit(ncid)) {q.push_back(ncid);}
       }
     }
 

navmap_core/src/navmap_core/DistanceField.cpp

@@ -134,34 +134,50 @@ void DistanceField::splat(std::uint32_t s)
     });
 }
 
+std::size_t DistanceField::index_entries() const
+{
+  std::size_t n = 0;
+  for (const auto & kv : buckets_) {
+    n += kv.second.size();
+  }
+  return n;
+}
+
 std::int64_t DistanceField::bucket_key(int bx, int by, int bz)
 {
   return (static_cast<std::int64_t>(bx) * 73856093) ^ (static_cast<std::int64_t>(by) * 19349663) ^
          (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: the queries widen their range by that (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_)));
+  auto & keys = keys_scratch_;
+  keys.clear();
+  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();}
     }
   }
 }
@@ -268,14 +284,16 @@ void DistanceField::update(
       });
     const Eigen::Vector3f p = seeds_[s].a;
     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)
+    // a segment seed is indexed by samples along it, each within bucket_ / 2 of its points
+    // (index_seed): widen the range by that (a hair more, for the rounding of the samples)
+    const float reach = r2 + 0.51f * bucket_;
+    const Eigen::Vector3f lo = (p.array() - reach).matrix() / bucket_;
+    const Eigen::Vector3f hi = (p.array() + reach).matrix() / 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 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)

navmap_core/src/navmap_core/NavMap.cpp

@@ -144,7 +144,7 @@ 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 is replaced by add_or_get)
       switch (src_base->type()) {
         case LayerType::U8: {
             auto src = std::dynamic_pointer_cast<LayerView<uint8_t>>(src_base);

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,70 @@ TEST(DistanceFieldIncremental, AnUpdateOnlyTouchesItsNeighbourhood)
   }
 }
 
+TEST(DistanceFieldIncremental, LongDiagonalOpenEdgesAreIndexedBySamplesNotBoundingBoxes)
+{
+  // 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, a 16 m edge went into
+  // ~(16 / 0.6)^2 buckets; sampled along it, into ~16 / 0.6 (counted, so that no timing is needed)
+  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 entries = [](const NavMap & nm) {
+      DistanceField df(nm, true, 0.6f);
+      df.compute({});
+      return df.index_entries();
+    };
+  const NavMap fine = build(0.5f), coarse = build(16.0f);
+  const double e_fine = static_cast<double>(entries(fine));
+  const double e_coarse = static_cast<double>(entries(coarse));
+  ASSERT_GT(e_fine, 0.0);
+  EXPECT_LT(e_coarse, 40.0 * e_fine) << "fine " << e_fine << ", coarse " << e_coarse;
+}
+
+TEST(NavMapSetArea, SmallAndLargeAreasMarkExactlyTheNavCelsInsideThem)
+{
+  // A small area is followed with a hash set of visited NavCels, a large one with a flag per
+  // NavCel: both must mark the NavCels whose centroid is inside, and only them
+  NavMap nm;
+  add_grid(nm, 30, 30, 0.5f, [](float, float) {return 0.0f;});
+  nm.rebuild_geometry_accels();
+  nm.add_layer<uint8_t>("area", "", "", 0);
+  for (const float radius : {1.0f, 3.0f, 100.0f}) {
+    nm.layer_clear<uint8_t>("area", 0);
+    ASSERT_TRUE(
+      nm.set_area<uint8_t>(
+        Eigen::Vector3f(7.0f, 7.0f, 0.0f), static_cast<uint8_t>(1), "area",
+        navmap::AreaShape::CIRCULAR, radius));
+    std::size_t marked = 0, expected = 0;
+    for (std::size_t c = 0; c < nm.navcels.size(); ++c) {
+      const auto p = nm.navcel_centroid(static_cast<navmap::NavCelId>(c));
+      const bool inside = std::hypot(p.x() - 7.0f, p.y() - 7.0f) <= radius;
+      marked += nm.layer_get<uint8_t>("area", static_cast<navmap::NavCelId>(c), 0);
+      expected += inside ? 1 : 0;
+      EXPECT_EQ(nm.layer_get<uint8_t>("area", static_cast<navmap::NavCelId>(c), 0), inside ? 1 : 0)
+        << "radius " << radius << ", NavCel " << c;
+    }
+    EXPECT_GT(expected, 0u);
+    EXPECT_EQ(marked, expected);
+  }
+}
+
 // ------------------------------------------------------------------ layer change tracking
 
 TEST(LayerChanges, SingleWritesAreRecordedOnceAndBulkWritesMarkTheLayer)

navmap_core/tests/test_navmap_assign.cpp

@@ -284,3 +284,52 @@ 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);
+}
+
+TEST(NavMapAssign, EveryWriterReplacesALayerOfAnotherType)
+{
+  // layer_set, layer_clear, add_layer and add_or_get all go through LayerRegistry::add_or_get,
+  // which used to return null for a layer of another type (and the callers dereferenced it)
+  NavMap m;
+  fill_one_tri_map(m);   // "occ" is U8
+  m.layer_set<float>("occ", 0, 1.5f);
+  ASSERT_TRUE(m.layers.get("occ"));
+  EXPECT_EQ(m.layers.get("occ")->type(), navmap::LayerType::F32);
+  EXPECT_FLOAT_EQ(m.layer_get<float>("occ", 0, 0.0f), 1.5f);
+
+  m.layer_clear<uint8_t>("occ", 9);
+  EXPECT_EQ(m.layers.get("occ")->type(), navmap::LayerType::U8);
+  EXPECT_EQ(m.layer_get<uint8_t>("occ", 0, 0), 9);
+
+  m.add_layer<double>("occ", "", "", 2.0);
+  EXPECT_EQ(m.layers.get("occ")->type(), navmap::LayerType::F64);
+  EXPECT_DOUBLE_EQ(m.layer_get<double>("occ", 0, 0.0), 2.0);
+
+  auto view = m.layers.add_or_get<int16_t>("occ", m.navcels.size(), layer_type_tag<int16_t>());
+  ASSERT_TRUE(view);
+  EXPECT_EQ(m.layers.get("occ")->type(), navmap::LayerType::I16);
+  // the same type: the layer itself, not a new one
+  EXPECT_EQ(m.layers.add_or_get<int16_t>("occ", m.navcels.size(), layer_type_tag<int16_t>()), view);
+}