lib: utils: interpolation: Add bilinear interpolation

This allows to treat a std::map<KeyType<std::map<KeyType, ValueType>> as
a set of x-y coordinates, and bilinearly interpolate a z-value given
four x/y pairs.

2 files changed, 186 insertions, 1 deletions

diff --git a/host/lib/include/uhdlib/utils/interpolation.hpp b/host/lib/include/uhdlib/utils/interpolation.hpp
index 6105dedc9..7d768eb7a 100644
--- a/host/lib/include/uhdlib/utils/interpolation.hpp
+++ b/host/lib/include/uhdlib/utils/interpolation.hpp
@@ -11,9 +11,43 @@
 #include <uhd/utils/math.hpp>
 #include <uhd/utils/interpolation.hpp>
 #include <map>
+#include <utility>
 
 namespace uhd { namespace math {
 
+//! Return a pair of iterators before and after a key within a map.
+//
+// Complexity: That of std::map::lower_bound (logarithmic).
+//
+// If \p key is lower or greater than the range of \p data then both the
+// returned iterators will point to the first or last item in the map, respectively.
+// If the key is found exactly in the map, then the second iterator will point to
+// to that key, and the first iterator will point to the previous item (if
+// possible).
+template <typename map_type>
+std::pair<typename map_type::const_iterator, typename map_type::const_iterator>
+get_bounding_iterators(const map_type& data, const typename map_type::key_type& key)
+{
+    // Get an iterator to the next bigger item
+    auto next_it = data.lower_bound(key);
+    if (next_it == data.end()) {
+        // This means key is larger than our biggest key in data's first
+        // dimension, and thus we return the value of the largest key.
+        next_it--;
+        return {next_it, next_it};
+    }
+    auto prev_it = next_it;
+    // If the next if clause is true, then x2 is already the smallest x value,
+    // and we keep x1 == x2. Otherwise, we make x1 the next smaller available
+    // x value.
+    if (prev_it != data.begin()) {
+        prev_it--;
+    }
+
+    return {prev_it, next_it};
+}
+
+
 //! Linearly interpolate f(x) given f(x0) = y0 and f(x1) = y1
 //
 // This draws a line through the coordinates x0/y0 and x1/y1, and then returns
@@ -31,6 +65,39 @@ inline InterpType linear_interp(
     return y0 + (x - x0) * (y1 - y0) / (x1 - x0);
 }
 
+//! Bi-Linearly interpolate f(x, y) given f(xi, yk) = zik and for i, k == 0, 0
+//
+// This does one linear interpolation in x-direction and one in y-direction to
+// return the z-value for the given x-value and y-value.
+//
+// \throws uhd::runtime_error if x0 == x1, or y0 == y1 since that doesn't allow
+//         us to interpolate.
+template <typename InterpType>
+inline InterpType bilinear_interp(InterpType x,
+    InterpType y,
+    InterpType x0,
+    InterpType y0,
+    InterpType x1,
+    InterpType y1,
+    InterpType z00,
+    InterpType z01,
+    InterpType z10,
+    InterpType z11)
+{
+    if (x0 == x1) {
+        throw uhd::runtime_error("bilinear_interp(): x0 and x1 must differ!");
+    }
+    if (y0 == y1) {
+        throw uhd::runtime_error("bilinear_interp(): y0 and y1 must differ!");
+    }
+
+    return linear_interp(y,
+        y0,
+        linear_interp(x, x0, z00, x1, z10),
+        y1,
+        linear_interp(x, x0, z01, x1, z11));
+}
+
 /*! Like std::at, but will interpolate the data between two points
  *
  * Most often, this is used with a std::map (keys need to be sorted!). Unlike a
@@ -134,4 +201,54 @@ typename map_type::mapped_type at_lin_interp(
         data, key, &uhd::math::linear_interp<typename map_type::mapped_type>);
 }
 
+//! Like std::map::at, except it will do a bilinear interpolation in two dimensions
+//
+// Example:
+// ~~~{.cpp}
+// std::map<double, std::map<double, double>> data;
+// data[1.0][1.0] = 0.0;
+// data[1.0][2.0] = 1.0;
+// data[2.0][1.0] = 1.0;
+// data[2.0][2.0] = 2.0;
+// std::cout << at_bilin_interp(data, 1.5, 1.5) << std::endl; // prints 1.0
+// ~~~
+//
+// This treats the double-map as a set of x/y/z coordinates, and returns the
+// value from the map that corresponds to a bilinear interpolation on those
+// coordinates.
+//
+// For x- or y-values greater than the maximum key, or smaller than the minimum
+// key of \p data, we return the value for the closest available key.
+template <typename doublemap_type>
+typename doublemap_type::mapped_type::mapped_type at_bilin_interp(
+    const doublemap_type& data, const typename doublemap_type::key_type& key_x,
+    const typename doublemap_type::mapped_type::key_type& key_y)
+{
+    // Find x1 and x2 coordinates. They are the x-values closest to key_x.
+    const auto x_iters = get_bounding_iterators(data, key_x);
+    const auto x1 = x_iters.first->first;
+    const auto x2 = x_iters.second->first;
+    // x-boundary condition
+    if (x1 == x2) {
+        return at_lin_interp(x_iters.first->second, key_y);
+    }
+    // Find y1 and y2 coordinates. They are the y-values closest to key_y.
+    const auto y_iters =
+        get_bounding_iterators(x_iters.first->second, key_y);
+    const auto y1 = y_iters.first->first;
+    const auto y2 = y_iters.second->first;
+    // y-boundary condition
+    if (y1 == y2) {
+        return linear_interp(key_x, x1, data.at(x1).at(y1), x2, data.at(x2).at(y1));
+    }
+
+    // Find z values
+    const auto z11 = data.at(x1).at(y1);
+    const auto z12 = data.at(x1).at(y2);
+    const auto z21 = data.at(x2).at(y1);
+    const auto z22 = data.at(x2).at(y2);
+
+    return bilinear_interp(key_x, key_y, x1, y1, x2, y2, z11, z12, z21, z22);
+}
+
 }} // namespace uhd::math
diff --git a/host/tests/interpolation_test.cpp b/host/tests/interpolation_test.cpp
index b3314c34a..4873f2214 100644
--- a/host/tests/interpolation_test.cpp
+++ b/host/tests/interpolation_test.cpp
@@ -6,6 +6,45 @@
 
 #include <uhdlib/utils/interpolation.hpp>
 #include <boost/test/unit_test.hpp>
+#include <string>
+
+BOOST_AUTO_TEST_CASE(test_get_bounding_iterators)
+{
+    using std::string;
+    using namespace uhd::math;
+
+    const std::map<double, string> data{{1.0, "leviathan wakes"}, {2.0, "calibans war"}};
+
+    const auto test1 = get_bounding_iterators(data, 1.1);
+    BOOST_CHECK_EQUAL(test1.first->first, 1.0);
+    BOOST_CHECK_EQUAL(test1.second->first, 2.0);
+    BOOST_CHECK_EQUAL(test1.first->second, "leviathan wakes");
+    BOOST_CHECK_EQUAL(test1.second->second, "calibans war");
+
+    const auto test2 = get_bounding_iterators(data, 0.5);
+    BOOST_CHECK_EQUAL(test2.first->first, 1.0);
+    BOOST_CHECK_EQUAL(test2.second->first, 1.0);
+    BOOST_CHECK_EQUAL(test2.first->second, "leviathan wakes");
+    BOOST_CHECK_EQUAL(test2.second->second, "leviathan wakes");
+
+    const auto test3 = get_bounding_iterators(data, 1e6);
+    BOOST_CHECK_EQUAL(test3.first->first, 2.0);
+    BOOST_CHECK_EQUAL(test3.second->first, 2.0);
+    BOOST_CHECK_EQUAL(test3.first->second, "calibans war");
+    BOOST_CHECK_EQUAL(test3.second->second, "calibans war");
+
+    const auto test4 = get_bounding_iterators(data, 2.0);
+    BOOST_CHECK_EQUAL(test4.first->first, 1.0);
+    BOOST_CHECK_EQUAL(test4.second->first, 2.0);
+    BOOST_CHECK_EQUAL(test4.first->second, "leviathan wakes");
+    BOOST_CHECK_EQUAL(test4.second->second, "calibans war");
+
+    const auto test5 = get_bounding_iterators(data, 1.0);
+    BOOST_CHECK_EQUAL(test5.first->first, 1.0);
+    BOOST_CHECK_EQUAL(test5.second->first, 1.0);
+    BOOST_CHECK_EQUAL(test5.first->second, "leviathan wakes");
+    BOOST_CHECK_EQUAL(test5.second->second, "leviathan wakes");
+}
 
 BOOST_AUTO_TEST_CASE(test_interp)
 {
@@ -20,7 +59,7 @@ BOOST_AUTO_TEST_CASE(test_interp)
 BOOST_AUTO_TEST_CASE(test_map_interp)
 {
     using namespace uhd::math;
-    std::map<double, double> test_data{{1.0, 1.0}, {2.0, 2.0}, {3.0, 3.0}};
+    const std::map<double, double> test_data{{1.0, 1.0}, {2.0, 2.0}, {3.0, 3.0}};
 
     BOOST_CHECK_EQUAL(at_nearest(test_data, 1.1), 1.0);
     BOOST_CHECK_EQUAL(at_nearest(test_data, 1.9), 2.0);
@@ -33,3 +72,32 @@ BOOST_AUTO_TEST_CASE(test_map_interp)
     BOOST_CHECK_EQUAL(at_lin_interp(test_data, 137.0), 3.0);
 }
 
+BOOST_AUTO_TEST_CASE(test_map_bilinear_interp)
+{
+    using namespace uhd::math;
+    // clang-format off
+    std::map<double, std::map<double, double>> test_data{
+        {1.0, {{1.0, 0.0}, {2.0, 1.0}}},
+        {2.0, {{1.0, 1.0}, {2.0, 2.0}}}
+    };
+    // clang-format on
+
+    BOOST_CHECK_CLOSE(at_bilin_interp(test_data, 1.5, 1.5), 1.0, 1e-6);
+    // Move y out of bounds, keep x in bounds
+    BOOST_CHECK_CLOSE(at_bilin_interp(test_data, 1.5, -17), 0.5, 1e-6);
+    BOOST_CHECK_CLOSE(at_bilin_interp(test_data, 1.5, 123.4), 1.5, 1e-6);
+    // Move x out of bounds, keep y in bounds
+    BOOST_CHECK_CLOSE(at_bilin_interp(test_data, -1e5, 1.5), 0.5, 1e-6);
+    BOOST_CHECK_CLOSE(at_bilin_interp(test_data, 1e9, 1.5), 1.5, 1e-6);
+    // Move x and y out of bounds
+    BOOST_CHECK_CLOSE(at_bilin_interp(test_data, -1e5, -1e6), 0.0, 1e-6);
+    BOOST_CHECK_CLOSE(at_bilin_interp(test_data, -1e5, 1e6), 1.0, 1e-6);
+    BOOST_CHECK_CLOSE(at_bilin_interp(test_data, 1e5, -1e6), 1.0, 1e-6);
+    BOOST_CHECK_CLOSE(at_bilin_interp(test_data, 1e5, 1e6), 2.0, 1e-6);
+    // Boundary conditions
+    BOOST_CHECK_EQUAL(at_bilin_interp(test_data, 1.0, 1.0), 0.0);
+    BOOST_CHECK_EQUAL(at_bilin_interp(test_data, 2.0, 2.0), 2.0);
+    BOOST_CHECK_EQUAL(at_bilin_interp(test_data, 1.0, 1.5), 0.5);
+    BOOST_CHECK_EQUAL(at_bilin_interp(test_data, 1.5, 2.0), 1.5);
+}
+