Add Line and AverageLine for new noncausal filter
This lets us accurately represent the offset between two nodes as a line
(with arbitrary precision for now) and also average 2 lines to be able
to compute the average offset between 2 nodes.
Change-Id: I983fa1eebca128a7a4ad77620e4bafae3996f5bb
diff --git a/aos/network/BUILD b/aos/network/BUILD
index e3014fc..8ea63fd 100644
--- a/aos/network/BUILD
+++ b/aos/network/BUILD
@@ -361,6 +361,7 @@
hdrs = ["timestamp_filter.h"],
deps = [
"//aos/time",
+ "//third_party/gmp",
"@com_google_absl//absl/strings",
],
)
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index cda013a..117b0e6 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -5,6 +5,7 @@
#include "absl/strings/str_cat.h"
#include "aos/time/time.h"
+#include "third_party/gmp/gmpxx.h"
namespace aos {
namespace message_bridge {
@@ -423,5 +424,61 @@
}
}
+Line Line::Fit(
+ const std::tuple<monotonic_clock::time_point, chrono::nanoseconds> a,
+ const std::tuple<monotonic_clock::time_point, chrono::nanoseconds> b) {
+ mpq_class slope =
+ FromInt64((std::get<1>(b) - std::get<1>(a)).count()) /
+ FromInt64((std::get<0>(b) - std::get<0>(a)).count());
+ slope.canonicalize();
+ mpq_class offset =
+ FromInt64(std::get<1>(a).count()) -
+ FromInt64(std::get<0>(a).time_since_epoch().count()) * slope;
+ offset.canonicalize();
+ Line f(offset, slope);
+ return f;
+}
+
+Line AverageFits(Line fa, Line fb) {
+ // tb = Oa(ta) + ta
+ // ta = Ob(tb) + tb
+ // tb - ta = Oa(ta)
+ // tb - ta = -Ob(tb)
+ // Oa(ta) = ma * ta + ba
+ // Ob(tb) = mb * tb + bb
+ //
+ // ta + O(ta, tb) = tb
+ // tb - ta = O(ta, tb)
+ // O(ta, tb) = (Oa(ta) - Ob(tb)) / 2.0
+ // ta + (ma * ta + ba - mb * tb - bb) / 2 = tb
+ // (2 + ma) / 2 * ta + (ba - bb) / 2 = (2 + mb) / 2 * tb
+ // (2 + ma) * ta + (ba - bb) = (2 + mb) * tb
+ // tb = (2 + ma) / (2 + mb) * ta + (ba - bb) / (2 + mb)
+ // ta = (2 + mb) / (2 + ma) * tb + (bb - ba) / (2 + ma)
+ //
+ // ta - tb = (mb - ma) / (2 + ma) * tb + (bb - ba) / (2 + ma)
+ // mb = (mb - ma) / (2 + ma)
+ // bb = (bb - ba) / (2 + ma)
+ //
+ // tb - ta = (ma - mb) / (2 + mb) * tb + (ba - bb) / (2 + mb)
+ // ma = (ma - mb) / (2 + mb)
+ // ba = (ba - bb) / (2 + mb)
+ //
+ // O(ta) = ma * ta + ba
+ // tb = O(ta) + ta
+ // ta = O(tb) + tb
+
+ mpq_class m =
+ (fa.mpq_slope() - fb.mpq_slope()) / (mpq_class(2) + fb.mpq_slope());
+ m.canonicalize();
+
+ mpq_class b =
+ (fa.mpq_offset() - fb.mpq_offset()) / (mpq_class(2) + fb.mpq_slope());
+ b.canonicalize();
+
+ Line f(b, m);
+ return f;
+}
+
} // namespace message_bridge
} // namespace aos
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index 3940f9c..c82f9a6 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -8,6 +8,7 @@
#include "aos/time/time.h"
#include "glog/logging.h"
+#include "third_party/gmp/gmpxx.h"
namespace aos {
namespace message_bridge {
@@ -215,6 +216,94 @@
FILE *rev_fp_ = nullptr;
};
+// Converts a int64_t into a mpq_class. This only uses 32 bit precision
+// internally, so it will work on ARM. This should only be used on 64 bit
+// platforms to test out the 32 bit implementation.
+inline mpq_class FromInt64(int64_t i) {
+ uint64_t absi = std::abs(i);
+ mpq_class bits(static_cast<uint32_t>((absi >> 32) & 0xffffffffu));
+ bits *= mpq_class(0x10000);
+ bits *= mpq_class(0x10000);
+ bits += mpq_class(static_cast<uint32_t>(absi & 0xffffffffu));
+
+ if (i < 0) {
+ return -bits;
+ } else {
+ return bits;
+ }
+}
+
+// Class to hold an affine function for the time offset.
+// O(t) = slope * t + offset
+//
+// This is stored using mpq_class, which stores everything as full rational
+// fractions.
+class Line {
+ public:
+ Line() {}
+
+ // Constructs a line given the offset and slope.
+ Line(mpq_class offset, mpq_class slope) : offset_(offset), slope_(slope) {}
+
+ // TODO(austin): Remove this one.
+ Line(std::chrono::nanoseconds offset, double slope)
+ : offset_(DoFromInt64(offset.count())), slope_(slope) {}
+
+ // Fits a line to 2 points and returns the associated line.
+ static Line Fit(
+ const std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> a,
+ const std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>
+ b);
+
+ // Returns the full precision slopes and offsets.
+ mpq_class mpq_offset() const { return offset_; }
+ mpq_class mpq_slope() const { return slope_; }
+
+ // Returns the rounded offsets and slopes.
+ std::chrono::nanoseconds offset() const {
+ double o = offset_.get_d();
+ return std::chrono::nanoseconds(static_cast<int64_t>(o));
+ }
+ double slope() const { return slope_.get_d(); }
+
+ void Debug() const {
+ LOG(INFO) << "Offset " << mpq_offset() << " slope " << mpq_slope();
+ }
+
+ // Returns the offset at a given time.
+ // TODO(austin): get_d() ie double -> int64 can't be accurate...
+ std::chrono::nanoseconds Eval(monotonic_clock::time_point pt) const {
+ mpq_class result =
+ mpq_class(FromInt64(pt.time_since_epoch().count())) * slope_ + offset_;
+ return std::chrono::nanoseconds(static_cast<int64_t>(result.get_d()));
+ }
+
+ private:
+ static mpq_class DoFromInt64(int64_t i) {
+#if GMP_NUMB_BITS == 32
+ return message_bridge::FromInt64(i);
+#else
+ return i;
+#endif
+ }
+
+ mpq_class offset_;
+ mpq_class slope_;
+};
+
+// Averages 2 fits per the equations below
+//
+// Oa(ta) = fa.slope * ta + fa.offset;
+// tb = Oa(ta) + ta;
+// Ob(tb) = fb.slope * tb + fb.offset;
+// ta = Ob(tb) + tb;
+//
+// This splits the difference between Oa and Ob and solves:
+// tb - ta = (Oa(ta) - Ob(tb)) / 2.0
+// and returns O(ta) such that
+// tb = O(ta) + ta
+Line AverageFits(Line fa, Line fb);
+
} // namespace message_bridge
} // namespace aos
diff --git a/aos/network/timestamp_filter_test.cc b/aos/network/timestamp_filter_test.cc
index 213353a..c834850 100644
--- a/aos/network/timestamp_filter_test.cc
+++ b/aos/network/timestamp_filter_test.cc
@@ -10,6 +10,7 @@
namespace testing {
namespace chrono = std::chrono;
+using aos::monotonic_clock;
// Tests that adding samples tracks more negative offsets down quickly, and
// slowly comes back up.
@@ -72,6 +73,93 @@
EXPECT_EQ(filter.offset(), chrono::microseconds(100500));
}
+// Tests that the FromInt64 function correctly produces a mpq even though it
+// only can use 32 bit numbers.
+TEST(LineTest, Int64) {
+ EXPECT_EQ(FromInt64(0x9710000000ll),
+ mpq_class(0x971) * mpq_class(0x10000000));
+
+ EXPECT_EQ(FromInt64(-0x9710000000ll),
+ mpq_class(-0x971) * mpq_class(0x10000000));
+}
+
+// Tests that we can create a simple line and the methods return sane results.
+TEST(LineTest, SimpleLine) {
+ mpq_class offset(1023);
+ mpq_class slope(1);
+ Line l(offset, slope);
+
+ EXPECT_EQ(l.mpq_offset(), offset);
+ EXPECT_EQ(l.mpq_slope(), slope);
+
+ EXPECT_EQ(l.offset(), chrono::nanoseconds(1023));
+ EXPECT_EQ(l.slope(), 1.0);
+
+ EXPECT_EQ(chrono::nanoseconds(1023 + 100),
+ l.Eval(monotonic_clock::time_point(chrono::nanoseconds(100))));
+}
+
+// Tests that we can fit a line to 2 points and they recover correctly.
+TEST(LineTest, FitLine) {
+ const monotonic_clock::time_point ta(chrono::nanoseconds(1000));
+ const monotonic_clock::time_point tb(chrono::nanoseconds(100001013));
+ Line l = Line::Fit(std::make_tuple(ta, chrono::nanoseconds(100)),
+ std::make_tuple(tb, chrono::nanoseconds(105)));
+
+ EXPECT_EQ(chrono::nanoseconds(100), l.Eval(ta));
+ EXPECT_EQ(chrono::nanoseconds(105), l.Eval(tb));
+}
+
+// Tests that averaging 2 lines results in the correct outcome.
+// Try to compute the correct outcome a couple of different ways to confirm the
+// math is done right.
+TEST(LineTest, AverageFits) {
+ const monotonic_clock::time_point ta(chrono::nanoseconds(1000));
+ const monotonic_clock::time_point tb(chrono::nanoseconds(100001013));
+
+ // Test 2 lines which both diverge and should average back to nothing.
+ {
+ Line l1(mpq_class(999000), mpq_class(1000, 1000));
+ Line l2(-mpq_class(990000), mpq_class(1000, 1000));
+
+ Line a = AverageFits(l1, l2);
+ a.Debug();
+
+ EXPECT_EQ(a.mpq_slope(), mpq_class(0));
+
+ // Confirm some points to make sure everything works.
+ //
+ // tb = ta + O(ta)
+ // tb = Oa(ta) + ta
+ // ta = Ob(tb) + tb
+ // tb - ta = O(ta, tb)
+ // So, if we pick a point at t=x, we can evaluate both functions and should
+ // get back O(x)
+
+ const monotonic_clock::time_point ta(chrono::nanoseconds(1000));
+ const monotonic_clock::time_point tb(chrono::nanoseconds(100001013));
+
+ EXPECT_EQ((l1.Eval(ta) - l2.Eval(a.Eval(ta) + ta)) / 2, a.Eval(ta));
+ EXPECT_EQ((l1.Eval(tb) - l2.Eval(a.Eval(tb) + tb)) / 2, a.Eval(tb));
+ }
+
+ // Test 2 lines which are parallel, so there should be a slope.
+ {
+ Line l1(mpq_class(990000), mpq_class(1000, 1000));
+ Line l2(-mpq_class(990000), -mpq_class(1000, 1000));
+
+ Line a = AverageFits(l1, l2);
+ a.Debug();
+
+ EXPECT_EQ(a.mpq_slope(), mpq_class(2));
+
+ // Confirm some points to make sure everything works.
+
+ EXPECT_EQ((l1.Eval(ta) - l2.Eval(a.Eval(ta) + ta)) / 2, a.Eval(ta));
+ EXPECT_EQ((l1.Eval(tb) - l2.Eval(a.Eval(tb) + tb)) / 2, a.Eval(tb));
+ }
+}
+
} // namespace testing
} // namespace message_bridge
} // namespace aos
diff --git a/third_party/gmp/BUILD b/third_party/gmp/BUILD
index 57cedef..e4f7029 100644
--- a/third_party/gmp/BUILD
+++ b/third_party/gmp/BUILD
@@ -82,6 +82,7 @@
"arm": [
"-Wno-old-style-declaration",
"-Wno-maybe-uninitialized",
+ "-Wno-empty-body",
"-Wno-unused-but-set-variable",
],
"amd64": [],
@@ -909,6 +910,7 @@
cc_library(
name = "gmp",
+ includes = ["."],
visibility = ["//visibility:public"],
deps = [
":cxx",