diff --git a/navmap_core/CMakeLists.txt b/navmap_core/CMakeLists.txt index 0b3ae95..11b0e95 100644 --- a/navmap_core/CMakeLists.txt +++ b/navmap_core/CMakeLists.txt @@ -19,6 +19,7 @@ add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}/Descriptors.cpp src/${PROJECT_NAME}/Combine.cpp src/${PROJECT_NAME}/Simplify.cpp + src/${PROJECT_NAME}/Subdivide.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC $ diff --git a/navmap_core/include/navmap_core/Simplify.hpp b/navmap_core/include/navmap_core/Simplify.hpp index 5270f15..07c53ed 100644 --- a/navmap_core/include/navmap_core/Simplify.hpp +++ b/navmap_core/include/navmap_core/Simplify.hpp @@ -49,6 +49,9 @@ struct SimplifyParams /// Layers left out of the above (e.g. per-cell geometry such as "elevation"). They are carried /// from the NavCel that survives, so recompute them if they depend on its shape. std::vector ignore_layers; + /// Vertices (ids of the input map) that are never removed: other vertices may collapse onto + /// them, they never collapse. E.g. where the borders of maps simplified apart must meet. + std::vector keep_vertices; }; /// Sizes before and after simplify(). diff --git a/navmap_core/include/navmap_core/Subdivide.hpp b/navmap_core/include/navmap_core/Subdivide.hpp new file mode 100644 index 0000000..eb721b4 --- /dev/null +++ b/navmap_core/include/navmap_core/Subdivide.hpp @@ -0,0 +1,89 @@ +// 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. + +/** + * @file Subdivide.hpp + * @brief Splitting NavCels: along straight lines (cut_along_lines) and around points (refine). + * + * Both keep the mesh conforming (every edge is shared whole by the NavCels on both sides: no + * cracks) and carry what the NavCels hold: each new NavCel has the layer values and the surface + * of the NavCel it comes from. New vertices interpolate the position and the vertex colours. + * NavCel ids change; geometry accelerations are rebuilt. + */ + +#ifndef NAVMAP_CORE__SUBDIVIDE_HPP_ +#define NAVMAP_CORE__SUBDIVIDE_HPP_ + +#include +#include +#include + +#include "navmap_core/NavMap.hpp" + +namespace navmap +{ + +/// Sizes before and after a subdivision. +struct SubdivideStats +{ + std::size_t navcels_before{0}, navcels_after{0}; + std::size_t vertices_before{0}, vertices_after{0}; +}; + +/// Lines in the map frame: x = xs[i] and y = ys[j] (m). +struct CutLines +{ + std::vector xs; + std::vector ys; +}; + +/** + * @brief Split the NavCels the lines cross, so that none crosses one. + * + * E.g. to cut a map into tiles along a grid. A vertex within @p snap (m) of a line is moved onto + * it, and the new vertices are put exactly on it: every vertex on a line has exactly the line's + * coordinate. A link keeps its ends on the first piece of each. + */ +SubdivideStats cut_along_lines(NavMap & nm, const CutLines & lines, float snap = 1e-4f); + +/** + * @brief Refine the NavCels under @p points until none of them has an edge longer than + * @p max_edge (m). + * + * Longest-edge bisection: an edge is split at its middle in every NavCel that has it, after + * splitting first, if needed, the longer edges of its neighbours (so the NavCels keep their + * shape and the mesh stays conforming). The refinement spreads a little around each point. + * Points off the map are ignored; NavCels at the end of a link are not split, nor any edge they + * have (the refinement stops short there). + */ +SubdivideStats refine(NavMap & nm, const std::vector & points, float max_edge); + +/// The largest edge (m) a NavCel with these vertices may have; not finite or <= 0: no bound. +using EdgeSizeAt = std::function; + +/** + * @brief Refine until no NavCel has an edge longer than @p max_edge_at says for it. + * + * The same longest-edge bisection, for a region instead of points (e.g. every NavCel within + * some distance of an obstacle, or a size that grows with the distance to a robot): the NavCels + * split are asked again, so the result meets the bound everywhere, in one pass over the map. + * If no NavCel is too large the map is left as it is (its accelerations too). + */ +SubdivideStats refine(NavMap & nm, const EdgeSizeAt & max_edge_at); + +} // namespace navmap + +#endif // NAVMAP_CORE__SUBDIVIDE_HPP_ diff --git a/navmap_core/src/navmap_core/Simplify.cpp b/navmap_core/src/navmap_core/Simplify.cpp index 48e7c3a..298ba43 100644 --- a/navmap_core/src/navmap_core/Simplify.cpp +++ b/navmap_core/src/navmap_core/Simplify.cpp @@ -191,6 +191,9 @@ class Simplifier } } } + for (const PointId x : p_.keep_vertices) { + if (x < nv) {locked_[x] = 1;} + } for (uint32_t u = 0; u < nv; ++u) { if (VT_[u].empty()) {dead_[u] = 1;} } diff --git a/navmap_core/src/navmap_core/Subdivide.cpp b/navmap_core/src/navmap_core/Subdivide.cpp new file mode 100644 index 0000000..90fce4a --- /dev/null +++ b/navmap_core/src/navmap_core/Subdivide.cpp @@ -0,0 +1,560 @@ +// 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 "navmap_core/Subdivide.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace navmap +{ + +namespace +{ + +constexpr uint32_t kNone = std::numeric_limits::max(); +using Tri = std::array; +using Rgba = std::array; + +// Calls f(view) with the typed view of @p base (no-op for an unknown type). +template +void with_view(const std::shared_ptr & base, F && f) +{ + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return;} + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return;} + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return;} + if (auto v = std::dynamic_pointer_cast>(base)) {f(*v); return;} +} + +uint64_t edge_key(uint32_t a, uint32_t b) +{ + return a < b ? (static_cast(a) << 32) | b : (static_cast(b) << 32) | a; +} + +// The vertices of a map being subdivided: positions and (optional) colours, growing +struct Vertices +{ + std::vector p; + std::vector c; + bool colored{false}; + + explicit Vertices(const NavMap & nm) + { + const std::size_t n = nm.positions.size(); + p.resize(n); + for (std::size_t i = 0; i < n; ++i) { + p[i] = nm.positions.at(static_cast(i)); + } + colored = nm.colors && nm.colors->r.size() == n; + if (colored) { + c.resize(n); + for (std::size_t i = 0; i < n; ++i) { + c[i] = {nm.colors->r[i], nm.colors->g[i], nm.colors->b[i], nm.colors->a[i]}; + } + } + } + + // A new vertex at a + t (b - a) + uint32_t add_between(uint32_t a, uint32_t b, float t) + { + p.push_back(p[a] + t * (p[b] - p[a])); + if (colored) { + Rgba m; + for (int k = 0; k < 4; ++k) { + m[k] = static_cast(std::lround(c[a][k] + t * (c[b][k] - c[a][k]))); + } + c.push_back(m); + } + return static_cast(p.size() - 1); + } +}; + +// Replaces the NavCels of @p nm by @p tris, each coming from the old NavCel parent[i]: its layer +// values, its surface; a link goes to the first NavCel coming from each of its ends +SubdivideStats replace( + NavMap & nm, const std::vector & tris, const std::vector & parent, + const Vertices & verts, SubdivideStats st) +{ + const std::size_t old_n = nm.navcels.size(), n = tris.size(); + std::vector surf(old_n, NavMap::kNoSurface); + for (std::size_t s = 0; s < nm.surfaces.size(); ++s) { + for (const NavCelId c : nm.surfaces[s].navcels) { + if (c < old_n) {surf[c] = s;} + } + nm.surfaces[s].navcels.clear(); + } + std::vector cels(n); + std::vector first(old_n, kNone); + for (std::size_t i = 0; i < n; ++i) { + for (int k = 0; k < 3; ++k) { + cels[i].v[k] = tris[i][k]; + } + const NavCelId from = parent[i]; + if (surf[from] != NavMap::kNoSurface) { + nm.surfaces[surf[from]].navcels.push_back(static_cast(i)); + } + if (first[from] == kNone) {first[from] = static_cast(i);} + } + for (const auto & name : nm.list_layers()) { + with_view( + nm.layers.get(name), [&](auto & v) { + auto & d = v.data(); + if (d.size() != old_n) {return;} + std::remove_reference_t out(n); + for (std::size_t i = 0; i < n; ++i) { + out[i] = d[parent[i]]; + } + d.swap(out); + v.mark_all_changed(); + }); + } + const auto links = nm.links(); + nm.clear_links(); + for (const auto & l : links) { + if (l.from < old_n && l.to < old_n && first[l.from] != kNone && first[l.to] != kNone) { + nm.add_link(first[l.from], first[l.to], l.type, l.length, l.bidirectional); + } + } + Positions pos; + pos.x.reserve(verts.p.size()); + pos.y.reserve(verts.p.size()); + pos.z.reserve(verts.p.size()); + for (const auto & q : verts.p) { + pos.x.push_back(q.x()); + pos.y.push_back(q.y()); + pos.z.push_back(q.z()); + } + nm.positions = std::move(pos); + if (verts.colored) { + Colors col; + for (const auto & c : verts.c) { + col.r.push_back(c[0]); + col.g.push_back(c[1]); + col.b.push_back(c[2]); + col.a.push_back(c[3]); + } + nm.colors = std::move(col); + } + nm.navcels = std::move(cels); + nm.rebuild_geometry_accels(); + st.navcels_after = nm.navcels.size(); + st.vertices_after = nm.positions.size(); + return st; +} + +SubdivideStats stats_before(const NavMap & nm) +{ + SubdivideStats st; + st.navcels_before = st.navcels_after = nm.navcels.size(); + st.vertices_before = st.vertices_after = nm.positions.size(); + return st; +} + +} // namespace + +// --------------------------------------------------------------------------------------------- +// cut_along_lines +// --------------------------------------------------------------------------------------------- + +SubdivideStats cut_along_lines(NavMap & nm, const CutLines & lines, float snap) +{ + SubdivideStats st = stats_before(nm); + auto sorted = [](std::vector v) { + v.erase( + std::remove_if(v.begin(), v.end(), [](float x) {return !std::isfinite(x);}), v.end()); + std::sort(v.begin(), v.end()); + v.erase(std::unique(v.begin(), v.end()), v.end()); + return v; + }; + const std::vector xs = sorted(lines.xs), ys = sorted(lines.ys); + if (nm.navcels.empty() || (xs.empty() && ys.empty())) {return st;} + + Vertices verts(nm); + // Vertices (almost) on a line go exactly onto it + auto snap_axis = [&](int axis, const std::vector & ls) { + for (auto & q : verts.p) { + auto it = std::lower_bound(ls.begin(), ls.end(), q[axis] - snap); + if (it != ls.end() && std::abs(*it - q[axis]) <= snap) {q[axis] = *it;} + } + }; + snap_axis(0, xs); + snap_axis(1, ys); + + // Line l: axis 0 (x = xs[l]) for l < xs.size(), else axis 1 (y = ys[l - xs.size()]) + const std::size_t nx = xs.size(); + auto axis_of = [nx](std::size_t l) {return l < nx ? 0 : 1;}; + auto value_of = [&](std::size_t l) {return l < nx ? xs[l] : ys[l - nx];}; + // The vertex where the edge (a, b) crosses line l, the same for the NavCels on both sides + std::vector> crossing(nx + ys.size()); + auto cross = [&](uint32_t a, uint32_t b, std::size_t l) { + const uint64_t key = edge_key(a, b); + auto it = crossing[l].find(key); + if (it != crossing[l].end()) {return it->second;} + const uint32_t u = std::min(a, b), w = std::max(a, b); + const int axis = axis_of(l); + const float c = value_of(l); + const float t = (c - verts.p[u][axis]) / (verts.p[w][axis] - verts.p[u][axis]); + const uint32_t m = verts.add_between(u, w, t); + verts.p[m][axis] = c; + crossing[l].emplace(key, m); + return m; + }; + + std::vector tris; + std::vector parent; + tris.reserve(nm.navcels.size()); + parent.reserve(nm.navcels.size()); + std::vector> polys, next; + std::vector crossing_lines; + for (NavCelId t = 0; t < nm.navcels.size(); ++t) { + const auto & v = nm.navcels[t].v; + // The lines strictly inside the NavCel's bounds, in a fixed order (x lines, then y lines, + // each increasing): the NavCels sharing an edge split it the same way + crossing_lines.clear(); + for (int axis = 0; axis < 2; ++axis) { + float lo = std::numeric_limits::infinity(), hi = -lo; + for (int k = 0; k < 3; ++k) { + lo = std::min(lo, verts.p[v[k]][axis]); + hi = std::max(hi, verts.p[v[k]][axis]); + } + const auto & ls = axis == 0 ? xs : ys; + for (auto it = std::upper_bound(ls.begin(), ls.end(), lo); it != ls.end() && *it < hi; ++it) { + crossing_lines.push_back(static_cast(it - ls.begin()) + (axis == 0 ? 0 : nx)); + } + } + if (crossing_lines.empty()) { + tris.push_back({v[0], v[1], v[2]}); + parent.push_back(t); + continue; + } + polys.assign(1, {v[0], v[1], v[2]}); + for (const std::size_t l : crossing_lines) { + const int axis = axis_of(l); + const float c = value_of(l); + next.clear(); + for (const auto & poly : polys) { + bool neg = false, pos = false; + for (const uint32_t x : poly) { + neg |= verts.p[x][axis] < c; + pos |= verts.p[x][axis] > c; + } + if (!neg || !pos) {next.push_back(poly); continue;} + std::vector below, above; + for (std::size_t i = 0; i < poly.size(); ++i) { + const uint32_t a = poly[i], b = poly[(i + 1) % poly.size()]; + const float sa = verts.p[a][axis] - c, sb = verts.p[b][axis] - c; + if (sa <= 0.0f) {below.push_back(a);} + if (sa >= 0.0f) {above.push_back(a);} + if ((sa < 0.0f && sb > 0.0f) || (sa > 0.0f && sb < 0.0f)) { + const uint32_t m = cross(a, b, l); + below.push_back(m); + above.push_back(m); + } + } + if (below.size() >= 3) {next.push_back(std::move(below));} + if (above.size() >= 3) {next.push_back(std::move(above));} + } + polys.swap(next); + } + // The pieces are convex: a fan from their first vertex + for (const auto & poly : polys) { + for (std::size_t i = 1; i + 1 < poly.size(); ++i) { + tris.push_back({poly[0], poly[i], poly[i + 1]}); + parent.push_back(t); + } + } + } + return replace(nm, tris, parent, verts, st); +} + +// --------------------------------------------------------------------------------------------- +// refine +// --------------------------------------------------------------------------------------------- + +namespace +{ + +class Refiner +{ +public: + Refiner(NavMap & nm, float max_edge) + : nm_(nm), verts_(nm), max2_(static_cast(max_edge) * max_edge) + { + const std::size_t n = nm.navcels.size(); + edges_.reserve(n * 2); + tris_.resize(n); + parent_.resize(n); + fixed_.assign(n, 0); + kids_.assign(n, {}); + for (NavCelId t = 0; t < n; ++t) { + for (int k = 0; k < 3; ++k) { + tris_[t][k] = nm.navcels[t].v[k]; + } + parent_[t] = t; + for (int k = 0; k < 3; ++k) { + edges_[edge_key(tris_[t][k], tris_[t][(k + 1) % 3])].push_back(t); + } + } + for (const auto & l : nm.links()) { + for (const NavCelId c : {l.from, l.to}) { + if (c < n) {fixed_[c] = 1;} + } + } + } + + // Splits around q, starting from the NavCel t that holds it; a point on an edge or a vertex is + // in the NavCels on the other side too, which are refined as well + void around(const Eigen::Vector3f & q, uint32_t t) + { + std::vector todo{holder(q, t)}, done; + while (!todo.empty()) { + uint32_t c = todo.back(); + todo.pop_back(); + if (std::find(done.begin(), done.end(), c) != done.end()) {continue;} + c = refine_one(q, c); + done.push_back(c); + const auto b = bary(q, c); + for (int k = 0; k < 3; ++k) { + if (std::abs(b[(k + 2) % 3]) > 1e-5f) {continue;} // not on the edge (k, k + 1) + for (const uint32_t o : edges_[edge_key(tris_[c][k], tris_[c][(k + 1) % 3])]) { + if (o != c && std::find(done.begin(), done.end(), o) == done.end()) {todo.push_back(o);} + } + } + } + } + + SubdivideStats finish(SubdivideStats st) + { + return replace(nm_, tris_, parent_, verts_, st); + } + +private: + // Refines the NavCel t that holds q until it is small enough; the NavCel holding q then + uint32_t refine_one(const Eigen::Vector3f & q, uint32_t t) + { + for (int guard = 0; guard < 100000; ++guard) { + if (length2(longest(t)) <= max2_) {return t;} + if (!split_edge(terminal_edge(t))) {return t;} + t = holder(q, t); + } + return t; + } + + // Following the longest edges from t, the first that is the longest of the NavCels on both + // sides (or has no other side): the edge to split for t (0 if none is found) + uint64_t terminal_edge(uint32_t t) + { + uint32_t cur = t; + for (int depth = 0; depth < 10000; ++depth) { + const uint64_t e = longest(cur); + const auto & sides = edges_[e]; + uint32_t other = kNone; + for (const uint32_t s : sides) { + if (s != cur) {other = s;} + } + if (other == kNone || sides.size() > 2 || longest(other) == e) {return e;} + cur = other; + } + return 0; + } + +public: + // Splits until no NavCel has an edge longer than size(its vertices), starting with `todo` + void where(const EdgeSizeAt & size, std::vector todo) + { + std::vector sides; + for (std::size_t guard = 0; !todo.empty() && guard < 100000000; ++guard) { + const uint32_t t = todo.back(); + todo.pop_back(); + const auto & v = tris_[t]; + const float m = size(verts_.p[v[0]], verts_.p[v[1]], verts_.p[v[2]]); + if (!std::isfinite(m) || m <= 0.0f) {continue;} + if (length2(longest(t)) <= static_cast(m) * m) {continue;} + const uint64_t e = terminal_edge(t); + if (e == 0) {continue;} + sides = edges_[e]; + if (!split_edge(e)) {continue;} + // the NavCels split (and t, if the split was elsewhere) are looked at again + todo.push_back(t); + for (const uint32_t s : sides) { + todo.push_back(s); + todo.insert(todo.end(), kids_[s].end() - 1, kids_[s].end()); + } + } + } + +private: + double length2(uint64_t e) const + { + const uint32_t a = static_cast(e >> 32), b = static_cast(e); + return static_cast((verts_.p[a] - verts_.p[b]).squaredNorm()); + } + + // The longest edge of t; ties go to the smaller key, so both NavCels of an edge agree + uint64_t longest(uint32_t t) const + { + uint64_t best = 0; + double best_len = -1.0; + for (int k = 0; k < 3; ++k) { + const uint64_t e = edge_key(tris_[t][k], tris_[t][(k + 1) % 3]); + const double len = length2(e); + if (len > best_len || (len == best_len && e < best)) {best = e; best_len = len;} + } + return best; + } + + void unlink(uint64_t e, uint32_t t) + { + auto & v = edges_[e]; + v.erase(std::remove(v.begin(), v.end(), t), v.end()); + } + + // Splits the edge e at its middle in every NavCel that has it + bool split_edge(uint64_t e) + { + const std::vector sides = edges_[e]; + for (const uint32_t t : sides) { + if (fixed_[t]) {return false;} + } + const uint32_t a = static_cast(e >> 32), b = static_cast(e); + const uint32_t m = verts_.add_between(a, b, 0.5f); + for (const uint32_t t : sides) { + // t = (p, q, o) with the edge (p, q) in its winding: t -> (p, m, o), new -> (m, q, o) + int k = 0; + while (edge_key(tris_[t][k], tris_[t][(k + 1) % 3]) != e) {++k;} + const uint32_t p = tris_[t][k], q = tris_[t][(k + 1) % 3], o = tris_[t][(k + 2) % 3]; + const uint32_t t2 = static_cast(tris_.size()); + tris_[t] = {p, m, o}; + tris_.push_back({m, q, o}); + parent_.push_back(parent_[t]); + fixed_.push_back(0); + kids_[t].push_back(t2); + kids_.emplace_back(); + unlink(edge_key(q, o), t); + edges_[edge_key(q, o)].push_back(t2); + edges_[edge_key(p, m)].push_back(t); + edges_[edge_key(m, q)].push_back(t2); + edges_[edge_key(m, o)].push_back(t); + edges_[edge_key(m, o)].push_back(t2); + } + edges_.erase(e); + return true; + } + + // The NavCel holding q now: t, or one of the NavCels split from it since (the most inside) + uint32_t holder(const Eigen::Vector3f & q, uint32_t t) const + { + uint32_t best = t; + float best_in = inside(q, t); + std::vector stack(kids_[t].begin(), kids_[t].end()); + while (!stack.empty()) { + const uint32_t k = stack.back(); + stack.pop_back(); + const float in = inside(q, k); + if (in > best_in) {best = k; best_in = in;} + stack.insert(stack.end(), kids_[k].begin(), kids_[k].end()); + } + return best; + } + + // Barycentric coordinates of q in t, in the plane (NaN for a NavCel with no area there) + std::array bary(const Eigen::Vector3f & q, uint32_t t) const + { + const Eigen::Vector2f a = verts_.p[tris_[t][0]].head<2>(), b = verts_.p[tris_[t][1]].head<2>(), + c = verts_.p[tris_[t][2]].head<2>(), x = q.head<2>(); + const float d = (b.x() - a.x()) * (c.y() - a.y()) - (c.x() - a.x()) * (b.y() - a.y()); + if (d == 0.0f) { + const float nan = std::numeric_limits::quiet_NaN(); + return {nan, nan, nan}; + } + const float u = ((b.x() - x.x()) * (c.y() - x.y()) - (c.x() - x.x()) * (b.y() - x.y())) / d; + const float v = ((c.x() - x.x()) * (a.y() - x.y()) - (a.x() - x.x()) * (c.y() - x.y())) / d; + return {u, v, 1.0f - u - v}; + } + + // How far inside t q is, in the plane: the smallest barycentric coordinate + float inside(const Eigen::Vector3f & q, uint32_t t) const + { + const auto b = bary(q, t); + if (std::isnan(b[0])) {return -std::numeric_limits::infinity();} + return std::min({b[0], b[1], b[2]}); + } + + NavMap & nm_; + Vertices verts_; + const double max2_; + std::vector tris_; + std::vector parent_; + std::vector fixed_; + std::unordered_map> edges_; + std::vector> kids_; // the NavCels split from each one +}; + +} // namespace + +SubdivideStats refine(NavMap & nm, const EdgeSizeAt & max_edge_at) +{ + SubdivideStats st = stats_before(nm); + if (nm.navcels.empty() || !max_edge_at) {return st;} + // The NavCels too large: if none, the map is left as it is + std::vector todo; + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + const auto & v = nm.navcels[c].v; + const Eigen::Vector3f a = nm.positions.at(v[0]), b = nm.positions.at(v[1]), + d = nm.positions.at(v[2]); + const float m = max_edge_at(a, b, d); + if (!std::isfinite(m) || m <= 0.0f) {continue;} + const float longest2 = std::max({(a - b).squaredNorm(), (b - d).squaredNorm(), + (d - a).squaredNorm()}); + if (longest2 > m * m) {todo.push_back(c);} + } + if (todo.empty()) {return st;} + std::reverse(todo.begin(), todo.end()); + Refiner r(nm, 0.0f); + r.where(max_edge_at, std::move(todo)); + return r.finish(st); +} + +SubdivideStats refine(NavMap & nm, const std::vector & points, float max_edge) +{ + SubdivideStats st = stats_before(nm); + if (nm.navcels.empty() || points.empty() || !(max_edge > 0.0f)) {return st;} + // Where the points are, on the map as it is + std::vector> start; + for (const auto & q : points) { + std::size_t s; + NavCelId cid; + Eigen::Vector3f bary; + if (nm.locate_navcel(q, s, cid, bary, nullptr)) {start.emplace_back(q, cid);} + } + if (start.empty()) {return st;} + Refiner r(nm, max_edge); + for (const auto & [q, cid] : start) { + r.around(q, cid); + } + return r.finish(st); +} + +} // namespace navmap diff --git a/navmap_core/tests/CMakeLists.txt b/navmap_core/tests/CMakeLists.txt index 7990c0b..a5020a0 100644 --- a/navmap_core/tests/CMakeLists.txt +++ b/navmap_core/tests/CMakeLists.txt @@ -92,3 +92,8 @@ ament_add_gtest(test_surface_tree test_surface_tree.cpp) target_link_libraries(test_surface_tree ${PROJECT_NAME} ) + +ament_add_gtest(test_subdivide test_subdivide.cpp) +target_link_libraries(test_subdivide + ${PROJECT_NAME} +) diff --git a/navmap_core/tests/test_simplify.cpp b/navmap_core/tests/test_simplify.cpp index 8ae44ce..ca84169 100644 --- a/navmap_core/tests/test_simplify.cpp +++ b/navmap_core/tests/test_simplify.cpp @@ -412,3 +412,31 @@ TEST(Simplify, DegenerateInputsAreSafe) } } } + +// keep_vertices: what the borders of maps simplified apart need to meet +TEST(Simplify, KeptVerticesAreNeverRemoved) +{ + NavMap nm; + add_grid(nm, 20, 20, 0.1f, flat); + nm.rebuild_geometry_accels(); + const std::vector keep = {10, 21 * 10 + 10, 21 * 5 + 3}; // border, inside + std::vector where; + for (const auto v : keep) { + where.push_back(nm.positions.at(v)); + } + NavMap free = nm; + SimplifyParams p; + p.max_error = 0.05f; + navmap::simplify(free, p); + p.keep_vertices = keep; + navmap::simplify(nm, p); + EXPECT_LT(nm.navcels.size(), 100u) << "the rest is simplified"; + for (const auto & w : where) { + bool found = false; + for (std::size_t i = 0; i < nm.positions.size(); ++i) { + found |= (nm.positions.at(static_cast(i)) - w).norm() < 1e-6f; + } + EXPECT_TRUE(found) << w.transpose(); + } + EXPECT_GT(nm.positions.size(), free.positions.size()); +} diff --git a/navmap_core/tests/test_subdivide.cpp b/navmap_core/tests/test_subdivide.cpp new file mode 100644 index 0000000..35f04bc --- /dev/null +++ b/navmap_core/tests/test_subdivide.cpp @@ -0,0 +1,253 @@ +// 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 "navmap_core/NavMap.hpp" +#include "navmap_core/Subdivide.hpp" + +using navmap::NavCelId; +using navmap::NavMap; + +namespace +{ + +// A grid of n x m cells of side `cell`, two NavCels each, height z(x, y), and a layer "column" +// with the column of each cell +NavMap grid(int n, int m, float cell, const std::function & z) +{ + NavMap nm; + const auto s = nm.create_surface("map"); + for (int j = 0; j <= m; ++j) { + for (int i = 0; i <= n; ++i) { + nm.positions.x.push_back(i * cell); + nm.positions.y.push_back(j * cell); + nm.positions.z.push_back(z(i * cell, j * cell)); + } + } + std::vector column; + for (int j = 0; j < m; ++j) { + for (int i = 0; i < n; ++i) { + const uint32_t a = j * (n + 1) + i, b = a + 1, c = a + n + 1, d = c + 1; + nm.add_navcel_to_surface(s, nm.add_navcel(a, b, d)); + nm.add_navcel_to_surface(s, nm.add_navcel(a, d, c)); + column.push_back(static_cast(i)); + column.push_back(static_cast(i)); + } + } + auto layer = nm.add_layer("column"); + layer->set_data(column); + nm.rebuild_geometry_accels(); + return nm; +} + +float flat(float, float) {return 0.0f;} + +double area(const NavMap & nm) +{ + double a = 0.0; + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + const auto p = nm.positions.at(nm.navcels[c].v[0]), q = nm.positions.at(nm.navcels[c].v[1]), + r = nm.positions.at(nm.navcels[c].v[2]); + a += 0.5 * (q - p).cross(r - p).norm(); + } + return a; +} + +// Conforming: no vertex in the middle of an edge (a crack: the NavCels on both sides of the edge +// would not share it whole) +int t_junctions(const NavMap & nm) +{ + std::map, int> edges; + for (const auto & c : nm.navcels) { + for (int k = 0; k < 3; ++k) { + const uint32_t a = c.v[k], b = c.v[(k + 1) % 3]; + edges[{std::min(a, b), std::max(a, b)}]++; + } + } + int bad = 0; + for (const auto & [e, n] : edges) { + const Eigen::Vector3f a = nm.positions.at(e.first), b = nm.positions.at(e.second); + for (uint32_t v = 0; v < nm.positions.size(); ++v) { + if (v == e.first || v == e.second) {continue;} + const Eigen::Vector3f p = nm.positions.at(v); + const float t = (p - a).dot(b - a) / (b - a).squaredNorm(); + if (t > 1e-4f && t < 1.0f - 1e-4f && (a + t * (b - a) - p).norm() < 1e-4f) {++bad;} + } + } + return bad; +} + +float longest_edge(const NavMap & nm, NavCelId c) +{ + float l = 0.0f; + for (int k = 0; k < 3; ++k) { + l = std::max( + l, (nm.positions.at(nm.navcels[c].v[k]) - + nm.positions.at(nm.navcels[c].v[(k + 1) % 3])).norm()); + } + return l; +} + +} // namespace + +TEST(Subdivide, NoNavCelCrossesTheLinesAfterTheCut) +{ + NavMap nm = grid(10, 10, 1.0f, flat); + const double a0 = area(nm); + const auto st = navmap::cut_along_lines(nm, {{2.5f, 5.5f}, {3.3f}}); + EXPECT_GT(st.navcels_after, st.navcels_before); + EXPECT_NEAR(area(nm), a0, 1e-3); + EXPECT_EQ(t_junctions(nm), 0); + for (const auto & c : nm.navcels) { + for (const float x : {2.5f, 5.5f}) { + bool below = false, above = false; + for (const auto v : c.v) { + below |= nm.positions.x[v] < x; + above |= nm.positions.x[v] > x; + } + EXPECT_FALSE(below && above); + } + bool below = false, above = false; + for (const auto v : c.v) { + below |= nm.positions.y[v] < 3.3f; + above |= nm.positions.y[v] > 3.3f; + } + EXPECT_FALSE(below && above); + } + // the pieces keep the values of the NavCel they come from, and the surface has them all + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + EXPECT_EQ( + nm.layer_get("column", c), + static_cast(std::floor(nm.navcel_centroid(c).x()))); + } + EXPECT_EQ(nm.surfaces[0].navcels.size(), nm.navcels.size()); +} + +TEST(Subdivide, VerticesOnALineHaveItsCoordinateExactly) +{ + // a line 0.00005 m from a column of vertices: they go onto it, no sliver is made + NavMap nm = grid(4, 4, 1.0f, [](float x, float y) {return 0.5f * x + 0.25f * y;}); + navmap::cut_along_lines(nm, {{2.00005f, 1.7f}, {}}); + int on = 0; + for (std::size_t v = 0; v < nm.positions.size(); ++v) { + const float x = nm.positions.x[v]; + if (std::abs(x - 1.7f) < 1e-3f) {EXPECT_EQ(x, 1.7f); ++on;} + if (std::abs(x - 2.00005f) < 1e-3f) {EXPECT_EQ(x, 2.00005f);} + // on the plane (a vertex moved onto a line keeps its height: within the snap times the slope) + EXPECT_NEAR(nm.positions.z[v], 0.5f * x + 0.25f * nm.positions.y[v], 1e-4f); + } + EXPECT_GT(on, 4); + EXPECT_EQ(t_junctions(nm), 0); +} + +TEST(Subdivide, TheRefinementIsWhereThePointsAre) +{ + NavMap nm = grid(1, 1, 16.0f, flat); // two NavCels of 16 m legs + const double a0 = area(nm); + navmap::refine(nm, {Eigen::Vector3f(3.0f, 2.0f, 0.0f)}, 1.0f); + EXPECT_NEAR(area(nm), a0, 1e-3); + EXPECT_EQ(t_junctions(nm), 0); + std::size_t s; + NavCelId c; + Eigen::Vector3f bary; + ASSERT_TRUE(nm.locate_navcel(Eigen::Vector3f(3.0f, 2.0f, 0.0f), s, c, bary, nullptr)); + EXPECT_LE(longest_edge(nm, c), 1.0f + 1e-4f); + ASSERT_TRUE(nm.locate_navcel(Eigen::Vector3f(14.0f, 15.0f, 0.0f), s, c, bary, nullptr)); + EXPECT_GT(longest_edge(nm, c), 4.0f) << "far from the point the NavCels stay large"; + EXPECT_LT(nm.navcels.size(), 200u); + EXPECT_EQ(nm.surfaces[0].navcels.size(), nm.navcels.size()); +} + +TEST(Subdivide, RefiningCarriesTheLayersAndIgnoresPointsOffTheMap) +{ + NavMap nm = grid(4, 1, 4.0f, flat); + NavMap same = nm; + navmap::refine(same, {Eigen::Vector3f(-10.0f, -10.0f, 0.0f)}, 0.5f); + EXPECT_EQ(same.navcels.size(), nm.navcels.size()); + + navmap::refine(nm, {Eigen::Vector3f(9.0f, 1.0f, 0.0f), Eigen::Vector3f(1.0f, 3.0f, 0.0f)}, 0.5f); + EXPECT_EQ(t_junctions(nm), 0); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + EXPECT_EQ( + nm.layer_get("column", c), + static_cast(std::floor(nm.navcel_centroid(c).x() / 4.0f))); + } +} + +TEST(Subdivide, NavCelsAtTheEndOfALinkAreNotSplit) +{ + NavMap nm = grid(2, 1, 8.0f, flat); + nm.add_link(0, 3, navmap::LinkType::GENERIC, 1.0f, true); + navmap::refine(nm, {Eigen::Vector3f(1.0f, 4.0f, 0.0f)}, 0.5f); + EXPECT_EQ(t_junctions(nm), 0); + ASSERT_EQ(nm.links().size(), 1u); + const auto & l = nm.links()[0]; + EXPECT_GT(longest_edge(nm, l.from), 8.0f); + EXPECT_GT(longest_edge(nm, l.to), 8.0f); +} + +TEST(Subdivide, APointOnAnEdgeRefinesTheNavCelsOnBothSides) +{ + // (8, 8) is on the diagonal of the square: in both NavCels; then on edges of the splits too + NavMap nm = grid(1, 1, 16.0f, flat); + navmap::refine(nm, {Eigen::Vector3f(8.0f, 8.0f, 0.0f), Eigen::Vector3f(4.0f, 0.0f, 0.0f)}, 1.0f); + EXPECT_EQ(t_junctions(nm), 0); + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + for (const auto & q : {Eigen::Vector2f(8.0f, 8.0f), Eigen::Vector2f(4.0f, 0.0f)}) { + // every NavCel with the point inside or on its border + const Eigen::Vector2f a = nm.positions.at(nm.navcels[c].v[0]).head<2>(); + const Eigen::Vector2f b = nm.positions.at(nm.navcels[c].v[1]).head<2>(); + const Eigen::Vector2f d = nm.positions.at(nm.navcels[c].v[2]).head<2>(); + auto cross = [](Eigen::Vector2f u, Eigen::Vector2f v) {return u.x() * v.y() - u.y() * v.x();}; + const float s1 = cross(b - a, q - a), s2 = cross(d - b, q - b), s3 = cross(a - d, q - d); + const bool in = (s1 >= -1e-5f && s2 >= -1e-5f && s3 >= -1e-5f) || + (s1 <= 1e-5f && s2 <= 1e-5f && s3 <= 1e-5f); + if (in) {EXPECT_LE(longest_edge(nm, c), 1.0f + 1e-4f) << q.transpose();} + } + } +} + +TEST(Subdivide, ARegionIsRefinedAsFineAsItsBoundSays) +{ + // every NavCel within 2 m of (8, 8) no larger than 1 m; the rest as it is + NavMap nm = grid(2, 2, 8.0f, flat); + const Eigen::Vector2f p(8.0f, 8.0f); + auto near = [&](const Eigen::Vector3f & a, const Eigen::Vector3f & b, const Eigen::Vector3f & c) { + const Eigen::Vector2f q = ((a + b + c) / 3.0f).head<2>(); + float reach = 0.0f; + for (const auto * v : {&a, &b, &c}) { + reach = std::max(reach, (v->head<2>() - q).norm()); + } + return (q - p).norm() < 2.0f + reach ? 1.0f : 0.0f; + }; + const auto st = navmap::refine(nm, near); + EXPECT_GT(st.navcels_after, st.navcels_before); + EXPECT_EQ(t_junctions(nm), 0); + bool large = false; + for (NavCelId c = 0; c < nm.navcels.size(); ++c) { + const Eigen::Vector2f q = nm.navcel_centroid(c).head<2>(); + if ((q - p).norm() < 2.0f) {EXPECT_LE(longest_edge(nm, c), 1.0f + 1e-4f);} + large |= longest_edge(nm, c) > 4.0f; + } + EXPECT_TRUE(large) << "far from the point the NavCels stay large"; +}