Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions navmap_core/include/navmap_core/Simplify.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#define NAVMAP_CORE__SIMPLIFY_HPP_

#include <cstddef>
#include <functional>
#include <limits>
#include <string>
#include <vector>
Expand All @@ -49,6 +50,12 @@ 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<std::string> ignore_layers;
/// Optional: max_error at each point (m), e.g. growing with the distance to a robot, so the map
/// is exact near it and coarse far away. A collapse uses the smaller of the values at its two
/// vertices; a value that is not finite or is negative stands for max_error.
std::function<float(const Eigen::Vector3f &)> max_error_at;
/// Optional: max_edge_length at each point (m), the same way.
std::function<float(const Eigen::Vector3f &)> max_edge_length_at;
};

/// Sizes before and after simplify().
Expand Down
24 changes: 20 additions & 4 deletions navmap_core/src/navmap_core/Simplify.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -218,10 +218,26 @@ class Simplifier
return out;
}

// The square of a bound at the collapse of u into v: the smaller of what `at` says at both
// vertices, `fallback` where it says nothing usable
double bound2(
const std::function<float(const Eigen::Vector3f &)> & at, uint32_t u, uint32_t v,
double fallback) const
{
if (!at) {return fallback * fallback;}
const float a = at(P_[u].cast<float>()), b = at(P_[v].cast<float>());
const bool ok_a = std::isfinite(a) && a >= 0.0f, ok_b = std::isfinite(b) && b >= 0.0f;
if (!ok_a && !ok_b) {return fallback * fallback;}
const double m = !ok_a ? b : (!ok_b ? a : std::min(a, b));
return m * m;
}

// Whether collapsing u into v keeps layers, border, topology and quality; its quadric cost.
bool valid(uint32_t u, uint32_t v, double & cost) const
{
if (locked_[u] || dead_[u] || dead_[v] || u == v) {return false;}
const double max_err2 = p_.max_error_at ?
bound2(p_.max_error_at, u, v, p_.max_error) : max_err2_;
const auto & fan = VT_[u];
for (const uint32_t t : fan) {
if (cls_[t] != cls_[fan[0]] || surf_[t] != surf_[fan[0]]) {return false;}
Expand All @@ -243,7 +259,7 @@ class Simplifier
if (len2 == 0.0) {return false;}
const double s = (P_[u] - P_[v]).dot(d) / len2;
if (s <= 0.0 || s >= 1.0) {return false;}
if ((P_[v] + s * d - P_[u]).squaredNorm() > max_err2_) {return false;}
if ((P_[v] + s * d - P_[u]).squaredNorm() > max_err2) {return false;}
} else if (!open.empty() || et.size() != 2) {
return false;
}
Expand All @@ -255,7 +271,7 @@ class Simplifier
}
if (common != et.size()) {return false;}
// The NavCels that stay, with v instead of u
const double max_edge2 = static_cast<double>(p_.max_edge_length) * p_.max_edge_length;
const double max_edge2 = bound2(p_.max_edge_length_at, u, v, p_.max_edge_length);
bool any = false;
double dev2 = std::numeric_limits<double>::infinity();
for (const uint32_t t : fan) {
Expand All @@ -279,10 +295,10 @@ class Simplifier
P_[u].cast<float>(), q[0].cast<float>(), q[1].cast<float>(), q[2].cast<float>())));
any = true;
}
if (!any || dev2 > max_err2_) {return false;}
if (!any || dev2 > max_err2) {return false;}
const Eigen::Vector4d h(P_[v].x(), P_[v].y(), P_[v].z(), 1.0);
cost = std::max(0.0, h.dot((Q_[u] + Q_[v]) * h));
return cost <= max_err2_;
return cost <= max_err2;
}

void push_best(uint32_t u)
Expand Down
65 changes: 65 additions & 0 deletions navmap_core/tests/test_simplify.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -412,3 +412,68 @@ TEST(Simplify, DegenerateInputsAreSafe)
}
}
}

// max_error_at: exact where it says 0 (near a robot), simplified where it allows more
TEST(Simplify, TheErrorCanChangeFromPlaceToPlace)
{
const Height bumps = [](float x, float y) {return 0.3f * std::sin(x) * std::cos(0.7f * y);};
NavMap orig;
add_grid(orig, 60, 30, 0.1f, bumps); // x in [0, 6]
orig.rebuild_geometry_accels();
NavMap nm = orig;
SimplifyParams p;
p.max_error = 0.05f;
p.max_error_at = [](const Eigen::Vector3f & q) {return q.x() < 3.0f ? 0.0f : 0.05f;};
navmap::simplify(nm, p);
std::size_t left = 0, right = 0, left0 = 0;
for (std::size_t i = 0; i < nm.positions.size(); ++i) {
(nm.positions.x[i] < 2.9f ? left : right) += 1;
}
for (std::size_t i = 0; i < orig.positions.size(); ++i) {
left0 += orig.positions.x[i] < 2.9f;
}
EXPECT_EQ(left, left0) << "where the error is 0 no vertex moves";
EXPECT_LT(right, orig.positions.size() - left0) << "where it is not, the terrain is simplified";
// what is given as not usable (NaN, negative) stands for max_error
NavMap nm2 = orig;
p.max_error_at = [](const Eigen::Vector3f &) {return -1.0f;};
navmap::simplify(nm2, p);
NavMap nm3 = orig;
p.max_error_at = nullptr;
navmap::simplify(nm3, p);
EXPECT_EQ(nm2.navcels.size(), nm3.navcels.size());
}

// max_edge_length_at: small NavCels in one place, large in another
TEST(Simplify, TheEdgeLengthCanChangeFromPlaceToPlace)
{
NavMap nm;
add_grid(nm, 60, 30, 0.1f, flat); // x in [0, 6]
nm.rebuild_geometry_accels();
SimplifyParams p;
p.max_edge_length_at = [](const Eigen::Vector3f & q) {
return q.x() < 3.0f ? 0.3f : std::numeric_limits<float>::infinity();
};
navmap::simplify(nm, p);
float left = 0.0f, right = 0.0f;
for (const auto & c : nm.navcels) {
float lo = 1e9f, longest = 0.0f;
for (int k = 0; k < 3; ++k) {
const auto a = nm.positions.at(c.v[k]), b = nm.positions.at(c.v[(k + 1) % 3]);
lo = std::min(lo, a.x());
longest = std::max(longest, (a - b).norm());
}
if (lo < 2.5f) {
float hi = 0.0f;
for (int k = 0; k < 3; ++k) {
hi = std::max(hi, nm.positions.at(c.v[k]).x());
}
if (hi < 2.9f) {left = std::max(left, longest);}
} else if (lo > 3.1f) {
right = std::max(right, longest);
}
}
EXPECT_LE(left, 0.3f + 1e-4f);
EXPECT_GT(right, 1.0f);
EXPECT_NEAR(area(nm), 18.0f, 1e-3f);
}
Loading