Add a bunch of interpolation and cost utilities to NoncausalTimestampFilter
We want to build up an optimization problem which penalizes
((ta - tb) - Oa(ta))^2 precisely with doubles. The optimization
problem will use a gradient decent, so we mostly care about the
derivatives of this cost. This means we need a very accurate way
to compute Oa(ta) and derivatives of the cost.
There are tricks we can play long term by subtracting an int64_t
constant from the optimization problem to drop the high order bits out
of all the calculations to make things more accurate.
Change-Id: I0d6cdc9b3c3c2e98086ace30012e5903a298328d
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index 6e401b6..da1125c 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -33,6 +33,15 @@
fprintf(fp, "# time_since_start, sample_ns, offset\n");
}
+void NormalizeTimestamps(monotonic_clock::time_point *ta_base, double *ta) {
+ chrono::nanoseconds ta_digits(static_cast<int64_t>(std::floor(*ta)));
+ *ta_base += ta_digits;
+ *ta -= static_cast<double>(ta_digits.count());
+
+ CHECK_GE(*ta, 0.0);
+ CHECK_LT(*ta, 1.0);
+}
+
} // namespace
void TimestampFilter::Set(aos::monotonic_clock::time_point monotonic_now,
@@ -541,7 +550,7 @@
std::deque<
std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>>
-NoncausalTimestampFilter::Timestamps() {
+NoncausalTimestampFilter::Timestamps() const {
std::deque<
std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>>
result;
@@ -574,6 +583,244 @@
saved_samples_.clear();
}
+std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
+NoncausalTimestampFilter::FindTimestamps(monotonic_clock::time_point ta_base,
+ double ta) const {
+ CHECK_GE(ta, 0.0);
+ CHECK_LT(ta, 1.0);
+
+ // Since ta is less than an integer, and timestamps should be at least 1 ns
+ // apart, we can ignore ta if we make sure that the end of the segment is
+ // strictly > than ta_base.
+ return FindTimestamps(ta_base);
+}
+
+std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
+NoncausalTimestampFilter::FindTimestamps(monotonic_clock::time_point ta) const {
+ CHECK_GT(timestamps_size(), 1u);
+ // Linear search until this is proven to be a measurable slowdown.
+ size_t index = 0;
+ while (index < timestamps_size() - 2u) {
+ if (std::get<0>(timestamp(index + 1)) > ta) {
+ break;
+ }
+ ++index;
+ }
+ return std::make_pair(timestamp(index), timestamp(index + 1));
+}
+
+chrono::nanoseconds NoncausalTimestampFilter::InterpolateOffset(
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p0,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p1,
+ monotonic_clock::time_point ta) {
+ // Given 2 points defining a line and the time along that line, interpolate.
+ //
+ // ta may be massive, but the points will be close, so compute everything
+ // relative to the points.
+ //
+ // There are 2 formulations:
+ // oa = p0.o + (ta - p0.t) * (p1.o - p0.o) / (p1.t - p0.t)
+ // or
+ // oa = p1.o + (p1.t - ta) * (p0.o - p1.o) / (p1.t - p0.t)
+ //
+ // These have different properties with respect to numerical precision as you
+ // get close to p0 and p1. Switch over in the middle of the line segment.
+ const chrono::nanoseconds time_in = ta - std::get<0>(p0);
+ const chrono::nanoseconds time_left = std::get<0>(p1) - ta;
+ const chrono::nanoseconds dt = std::get<0>(p1) - std::get<0>(p0);
+ if (time_in < time_left) {
+ return std::get<1>(p0) +
+ chrono::nanoseconds(static_cast<int64_t>(
+ std::round(static_cast<double>(time_in.count()) *
+ (std::get<1>(p1) - std::get<1>(p0)).count() /
+ static_cast<double>(dt.count()))));
+ } else {
+ return std::get<1>(p1) +
+ chrono::nanoseconds(static_cast<int64_t>(
+ std::round(static_cast<double>(time_left.count()) *
+ (std::get<1>(p0) - std::get<1>(p1)).count() /
+ static_cast<double>(dt.count()))));
+ }
+}
+
+chrono::nanoseconds NoncausalTimestampFilter::InterpolateOffset(
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p0,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds> /*p1*/,
+ monotonic_clock::time_point /*ta_base*/, double /*ta*/) {
+ // For the double variant, we want to split the result up into a large integer
+ // portion, and the rest. We want to do this without introducing numerical
+ // precision problems.
+ //
+ // One way would be to carefully compute the integer portion, and then compute
+ // the double portion in such a way that the two are guarenteed to add up
+ // correctly.
+ //
+ // The simpler way is to simply just use the offset from p0 as the integer
+ // portion, and make the rest be the double portion. It will get us most of
+ // the way there for a lot less work, and we can revisit if this breaks down.
+ //
+ // oa = p0.o + (ta - p0.t) * (p1.o - p0.o) / (p1.t - p0.t)
+ // ^^^^
+ return std::get<1>(p0);
+}
+
+double NoncausalTimestampFilter::InterpolateOffsetRemainder(
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p0,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p1,
+ monotonic_clock::time_point ta_base, double ta) {
+ const chrono::nanoseconds time_in = ta_base - std::get<0>(p0);
+ const chrono::nanoseconds dt = std::get<0>(p1) - std::get<0>(p0);
+
+ // The remainder then is the rest of the equation.
+ //
+ // oa = p0.o + (ta - p0.t) * (p1.o - p0.o) / (p1.t - p0.t)
+ // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+ return static_cast<double>(ta + time_in.count()) /
+ static_cast<double>(dt.count()) *
+ (std::get<1>(p1) - std::get<1>(p0)).count();
+}
+
+chrono::nanoseconds NoncausalTimestampFilter::Offset(
+ monotonic_clock::time_point ta) const {
+ CHECK_GT(timestamps_size(), 0u);
+ if (timestamps_size() == 1u) {
+ // Special case size = 1 since the interpolation functions don't need to
+ // handle it and the answer is trivial.
+ return NoncausalTimestampFilter::InterpolateOffset(timestamp(0), ta);
+ }
+
+ std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
+ points = FindTimestamps(ta);
+ return NoncausalTimestampFilter::InterpolateOffset(points.first,
+ points.second, ta);
+}
+
+std::pair<chrono::nanoseconds, double> NoncausalTimestampFilter::Offset(
+ monotonic_clock::time_point ta_base, double ta) const {
+ CHECK_GT(timestamps_size(), 0u);
+ if (timestamps_size() == 1u) {
+ // Special case size = 1 since the interpolation functions don't need to
+ // handle it and the answer is trivial.
+ return std::make_pair(
+ NoncausalTimestampFilter::InterpolateOffset(timestamp(0), ta_base, ta),
+ 0.0);
+ }
+
+ std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
+ points = FindTimestamps(ta_base, ta);
+ // Return both the integer and double portion together to save a timestamp
+ // lookup.
+ return std::make_pair(NoncausalTimestampFilter::InterpolateOffset(
+ points.first, points.second, ta_base, ta),
+ NoncausalTimestampFilter::InterpolateOffsetRemainder(
+ points.first, points.second, ta_base, ta));
+}
+
+double NoncausalTimestampFilter::OffsetError(
+ aos::monotonic_clock::time_point ta_base, double ta,
+ aos::monotonic_clock::time_point tb_base, double tb) const {
+ NormalizeTimestamps(&ta_base, &ta);
+ NormalizeTimestamps(&tb_base, &tb);
+
+ const auto offset = Offset(ta_base, ta);
+
+ // Compute the integer portion first, and the double portion second. Subtract
+ // the results of each. This handles large offsets without losing precision.
+ return static_cast<double>(((tb_base - ta_base) - offset.first).count()) +
+ ((tb - ta) - offset.second);
+}
+
+double NoncausalTimestampFilter::Cost(aos::monotonic_clock::time_point ta_base,
+ double ta,
+ aos::monotonic_clock::time_point tb_base,
+ double tb) const {
+ NormalizeTimestamps(&ta_base, &ta);
+ NormalizeTimestamps(&tb_base, &tb);
+
+ // Squaring the error throws away half the digits. The optimizer uses the
+ // gradient heavily to compensate, so we don't need to care much about
+ // computing this carefully.
+ return std::pow(OffsetError(ta_base, ta, tb_base, tb), 2.0);
+}
+
+double NoncausalTimestampFilter::DCostDta(
+ aos::monotonic_clock::time_point ta_base, double ta,
+ aos::monotonic_clock::time_point tb_base, double tb) const {
+ // As a reminder, our cost function is:
+ // (OffsetError(ta, tb))^2
+ // ie
+ // ((tb - ta - Offset(ta))^2
+ //
+ // Assuming Offset(ta) = m * ta + ba (linear): this becomes
+ // ((tb - ta - (ma ta + ba))^2
+ // ie
+ // ((tb - (1 + ma) ta - ba)^2
+ //
+ // d cost/dta =>
+ // 2 * (tb - (1 + ma) ta - ba) * (-(1 + ma))
+ //
+ // We don't actually want to compute tb - (1 + ma) ta for numerical precision
+ // reasons. The important digits are small compared to the offset. Given our
+ // original cost above, this is equivilent to:
+ // 2 * (tb - ta - Offset(ta)) * (-(1 + ma))
+ //
+ // We can compute this a lot more accurately.
+
+ // Go find our timestamps for the interpolation.
+ // Rather than lookup timestamps a number of times, look them up here and
+ // inline the implementation of OffsetError.
+ if (timestamps_size() == 1u) {
+ return -2.0 * OffsetError(ta_base, ta, tb_base, tb);
+ }
+
+ NormalizeTimestamps(&ta_base, &ta);
+ NormalizeTimestamps(&tb_base, &tb);
+
+ std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
+ points = FindTimestamps(ta_base, ta);
+
+ const double m =
+ static_cast<double>(
+ (std::get<1>(points.second) - std::get<1>(points.first)).count()) /
+ static_cast<double>(
+ (std::get<0>(points.second) - std::get<0>(points.first)).count());
+ // Subtract the integer offsets first and then handle the double remainder to
+ // keep precision up.
+ //
+ // (tb - ta - Offset(ta)) ->
+ // (tb_base - ta_base - OffsetBase + tb - ta - OffsetRemainder)
+ return -2.0 *
+ (static_cast<double>((tb_base - ta_base -
+ NoncausalTimestampFilter::InterpolateOffset(
+ points.first, points.second, ta_base, ta))
+ .count()) +
+ (tb - ta) -
+ NoncausalTimestampFilter::InterpolateOffsetRemainder(
+ points.first, points.second, ta_base, ta)) *
+ (1.0 + m);
+}
+
+double NoncausalTimestampFilter::DCostDtb(
+ aos::monotonic_clock::time_point ta_base, double ta,
+ aos::monotonic_clock::time_point tb_base, double tb) const {
+ // As a reminder, our cost function is:
+ // (OffsetError(ta, tb))^2
+ //
+ // d cost/dtb =>
+ // 2 * OffsetError(ta, tb) * d/dtb OffsetError(ta, tb)
+ //
+ // OffsetError => (tb - (1 + ma) ta - ba), so
+ // d/dtb OffsetError(ta, tb) = 1
+ //
+ // d cost/dtb => 2 * OffsetError(ta, tb)
+ return 2.0 * OffsetError(ta_base, ta, tb_base, tb);
+}
+
bool NoncausalTimestampFilter::Sample(
aos::monotonic_clock::time_point monotonic_now,
chrono::nanoseconds sample_ns) {
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index 50211fd..ccbf334 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -269,8 +269,14 @@
}
double slope() const { return slope_.get_d(); }
+ std::string DebugString() const {
+ std::stringstream ss;
+ ss << "Offset " << mpq_offset() << " slope " << mpq_slope();
+ return ss.str();
+ }
+
void Debug() const {
- LOG(INFO) << "Offset " << mpq_offset() << " slope " << mpq_slope();
+ LOG(INFO) << DebugString();
}
// Returns the offset at a given time.
@@ -330,9 +336,11 @@
// O(ta) = tb - ta - network_delay. This means that the fastest time a message
// can be delivered is going to be when the offset is the most positive.
//
-// TODO(austin): Figure out how to track when we have used something from this
-// filter, and when we do that, enforce that it can't change anymore. That will
-// help us find when we haven't buffered far enough in the future.
+// All the offset and error calculation functions are designed to be used in an
+// optimization problem with large times but to retain sub-nanosecond precision.
+// To do this, time is treated as both a large integer portion, and a small
+// double portion. All the functions are subtracting the large times in integer
+// precision and handling the remainder with double precision.
class NoncausalTimestampFilter {
public:
~NoncausalTimestampFilter();
@@ -341,6 +349,38 @@
// available, or the only point (assuming 0 slope) if not available.
Line FitLine();
+ // Returns the offset for the point in time, using the timestamps in the deque
+ // to form a polyline used to interpolate.
+ std::chrono::nanoseconds Offset(monotonic_clock::time_point ta) const;
+ std::pair<std::chrono::nanoseconds, double> Offset(
+ monotonic_clock::time_point ta_base, double ta) const;
+
+ // Returns the error between the offset in the provided timestamps, and the
+ // offset at ta.
+ double OffsetError(aos::monotonic_clock::time_point ta_base, double ta,
+ aos::monotonic_clock::time_point tb_base, double tb) const;
+
+ // Returns the cost (OffsetError^2), ie (ob - oa - offset(oa, ob))^2,
+ // calculated accurately.
+ // Since this is designed to be used with a gradient based solver, it isn't
+ // super important if Cost is precise.
+ double Cost(aos::monotonic_clock::time_point ta_base, double ta,
+ aos::monotonic_clock::time_point tb_base, double tb) const;
+
+ // Returns the partial derivitive dcost/dta
+ double DCostDta(aos::monotonic_clock::time_point ta_base, double ta,
+ aos::monotonic_clock::time_point tb_base, double tb) const;
+ // Returns the partial derivitive dcost/dtb
+ double DCostDtb(aos::monotonic_clock::time_point ta_base, double ta,
+ aos::monotonic_clock::time_point tb_base, double tb) const;
+
+ double Convert(double ta) const {
+ return ta +
+ static_cast<double>(
+ Offset(monotonic_clock::epoch(), ta).first.count()) +
+ Offset(monotonic_clock::epoch(), ta).second;
+ }
+
// Adds a new sample to our filtered timestamp list.
// Returns true if adding the sample changed the output from FitLine().
bool Sample(aos::monotonic_clock::time_point monotonic_now,
@@ -353,7 +393,13 @@
// Returns the current list of timestamps in our list.
std::deque<
std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>>
- Timestamps();
+ Timestamps() const;
+
+ std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>
+ timestamp(size_t i) const {
+ return std::make_tuple(std::get<0>(timestamps_[i]),
+ std::get<1>(timestamps_[i]));
+ }
size_t timestamps_size() const { return timestamps_.size(); }
@@ -377,6 +423,42 @@
// going forwards.
void Freeze();
+ // Public for testing.
+ // Assuming that there are at least 2 points in timestamps_, finds the 2
+ // matching points.
+ std::pair<std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>>
+ FindTimestamps(monotonic_clock::time_point ta) const;
+ std::pair<std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>>
+ FindTimestamps(monotonic_clock::time_point ta_base, double ta) const;
+
+ static std::chrono::nanoseconds InterpolateOffset(
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p1,
+ monotonic_clock::time_point ta);
+
+ static std::chrono::nanoseconds InterpolateOffset(
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
+ monotonic_clock::time_point /*ta*/) {
+ return std::get<1>(p0);
+ }
+
+ static std::chrono::nanoseconds InterpolateOffset(
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p1,
+ monotonic_clock::time_point ta_base, double ta);
+ static double InterpolateOffsetRemainder(
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> /*p0*/,
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> /*p1*/,
+ monotonic_clock::time_point ta_base, double ta);
+
+ static std::chrono::nanoseconds InterpolateOffset(
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
+ monotonic_clock::time_point /*ta_base*/, double /*ta*/) {
+ return std::get<1>(p0);
+ }
+
private:
// Removes the oldest timestamp.
void PopFront() {
@@ -421,6 +503,18 @@
NoncausalOffsetEstimator(const Node *node_a, const Node *node_b)
: node_a_(node_a), node_b_(node_b) {}
+ const NoncausalTimestampFilter *GetFilter(const Node *n) {
+ if (n == node_a_) {
+ return &a_;
+ }
+ if (n == node_b_) {
+ return &b_;
+ }
+
+ LOG(FATAL) << "Unknown node";
+ return nullptr;
+ }
+
// Updates the filter based on a sample from the provided node to the other
// node.
void Sample(const Node *node,
diff --git a/aos/network/timestamp_filter_test.cc b/aos/network/timestamp_filter_test.cc
index d7d31ae..f3581d5 100644
--- a/aos/network/timestamp_filter_test.cc
+++ b/aos/network/timestamp_filter_test.cc
@@ -5,6 +5,7 @@
#include "aos/configuration.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/macros.h"
+#include "gmock/gmock.h"
#include "gtest/gtest.h"
namespace aos {
@@ -332,6 +333,371 @@
ASSERT_EQ(filter.Timestamps().size(), 2u);
}
+// Tests that all variants of InterpolateOffset do reasonable things.
+TEST(NoncausalTimestampFilterTest, InterpolateOffset) {
+ const monotonic_clock::time_point e = monotonic_clock::epoch();
+
+ const monotonic_clock::time_point t1 = e + chrono::nanoseconds(0);
+ const chrono::nanoseconds o1 = chrono::nanoseconds(100);
+ const double o1d = static_cast<double>(o1.count());
+
+ const monotonic_clock::time_point t2 = e + chrono::nanoseconds(1000);
+ const chrono::nanoseconds o2 = chrono::nanoseconds(150);
+ const double o2d = static_cast<double>(o2.count());
+
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1),
+ o1);
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 0.0),
+ o1);
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 0.0),
+ 0.0);
+
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t2),
+ o2);
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t2, 0.0),
+ o1);
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t2, 0.0),
+ o2d - o1d);
+
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2),
+ e + chrono::nanoseconds(500)),
+ chrono::nanoseconds(125));
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2),
+ e + chrono::nanoseconds(500), 0.0),
+ o1);
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2),
+ e + chrono::nanoseconds(500), 0.0),
+ 25.);
+
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2),
+ e + chrono::nanoseconds(-200)),
+ chrono::nanoseconds(90));
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, -200.),
+ o1);
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, -200.),
+ -10.);
+
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2),
+ e + chrono::nanoseconds(200)),
+ chrono::nanoseconds(110));
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 200.),
+ o1);
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 200.),
+ 10.);
+
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2),
+ e + chrono::nanoseconds(800)),
+ chrono::nanoseconds(140));
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 800.),
+ o1);
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 800.),
+ 40.);
+
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2),
+ e + chrono::nanoseconds(1200)),
+ chrono::nanoseconds(160));
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 1200.),
+ o1);
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 1200.),
+ 60.);
+
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), e + chrono::nanoseconds(800)),
+ o1);
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(std::make_tuple(t1, o1),
+ e, 800.),
+ o1);
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), e + chrono::nanoseconds(-300)),
+ o1);
+ EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(std::make_tuple(t1, o1),
+ e, -300.),
+ o1);
+}
+
+// Tests that FindTimestamps finds timestamps in a sequence.
+TEST(NoncausalTimestampFilterTest, FindTimestamps) {
+ const monotonic_clock::time_point e = monotonic_clock::epoch();
+ // Note: t1, t2, t3 need to be picked such that the slop is small so filter
+ // doesn't modify the timestamps.
+ const monotonic_clock::time_point t1 = e + chrono::nanoseconds(0);
+ const chrono::nanoseconds o1 = chrono::nanoseconds(100);
+ const monotonic_clock::time_point t2 = e + chrono::microseconds(1000);
+ const chrono::nanoseconds o2 = chrono::nanoseconds(150);
+ const monotonic_clock::time_point t3 = e + chrono::microseconds(2000);
+ const chrono::nanoseconds o3 = chrono::nanoseconds(50);
+
+ NoncausalTimestampFilter filter;
+
+ filter.Sample(t1, o1);
+ filter.Sample(t2, o2);
+ filter.Sample(t3, o3);
+
+ // Try points before, after, and at each of the points in the line.
+ EXPECT_THAT(filter.FindTimestamps(e - chrono::microseconds(10)),
+ ::testing::Pair(::testing::Eq(std::make_tuple(t1, o1)),
+ ::testing::Eq(std::make_tuple(t2, o2))));
+ EXPECT_THAT(filter.FindTimestamps(e - chrono::microseconds(10), 0.9),
+ ::testing::Pair(::testing::Eq(std::make_tuple(t1, o1)),
+ ::testing::Eq(std::make_tuple(t2, o2))));
+
+ EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(0)),
+ ::testing::Pair(::testing::Eq(std::make_tuple(t1, o1)),
+ ::testing::Eq(std::make_tuple(t2, o2))));
+ EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(0), 0.8),
+ ::testing::Pair(::testing::Eq(std::make_tuple(t1, o1)),
+ ::testing::Eq(std::make_tuple(t2, o2))));
+
+ EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(100)),
+ ::testing::Pair(::testing::Eq(std::make_tuple(t1, o1)),
+ ::testing::Eq(std::make_tuple(t2, o2))));
+ EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(100), 0.7),
+ ::testing::Pair(::testing::Eq(std::make_tuple(t1, o1)),
+ ::testing::Eq(std::make_tuple(t2, o2))));
+
+ EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(1000)),
+ ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
+ ::testing::Eq(std::make_tuple(t3, o3))));
+ EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(1000), 0.0),
+ ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
+ ::testing::Eq(std::make_tuple(t3, o3))));
+
+ EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(1500)),
+ ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
+ ::testing::Eq(std::make_tuple(t3, o3))));
+ EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(1500), 0.0),
+ ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
+ ::testing::Eq(std::make_tuple(t3, o3))));
+
+ EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(2000)),
+ ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
+ ::testing::Eq(std::make_tuple(t3, o3))));
+ EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(2000), 0.1),
+ ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
+ ::testing::Eq(std::make_tuple(t3, o3))));
+
+ EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(2500)),
+ ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
+ ::testing::Eq(std::make_tuple(t3, o3))));
+ EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(2500), 0.0),
+ ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
+ ::testing::Eq(std::make_tuple(t3, o3))));
+}
+
+// Tests that Offset returns results indicative of it calling InterpolateOffset
+// and FindTimestamps correctly.
+TEST(NoncausalTimestampFilterTest, Offset) {
+ const monotonic_clock::time_point e = monotonic_clock::epoch();
+ // Note: t1, t2, t3 need to be picked such that the slop is small so filter
+ // doesn't modify the timestamps.
+ const monotonic_clock::time_point t1 = e + chrono::nanoseconds(0);
+ const chrono::nanoseconds o1 = chrono::nanoseconds(100);
+ const double o1d = static_cast<double>(o1.count());
+
+ const monotonic_clock::time_point t2 = e + chrono::microseconds(1000);
+ const chrono::nanoseconds o2 = chrono::nanoseconds(150);
+ const double o2d = static_cast<double>(o2.count());
+
+ const monotonic_clock::time_point t3 = e + chrono::microseconds(2000);
+ const chrono::nanoseconds o3 = chrono::nanoseconds(50);
+ const double o3d = static_cast<double>(o3.count());
+
+ NoncausalTimestampFilter filter;
+
+ filter.Sample(t1, o1);
+
+ // 1 point is handled properly.
+ EXPECT_EQ(filter.Offset(t1), o1);
+ EXPECT_EQ(filter.Offset(t1, 0.0), std::make_pair(o1, 0.0));
+ EXPECT_EQ(filter.Offset(t2, 0.0), std::make_pair(o1, 0.0));
+
+ filter.Sample(t2, o2);
+ filter.Sample(t3, o3);
+
+ EXPECT_EQ(filter.Offset(t1), o1);
+ EXPECT_EQ(filter.Offset(t2), o2);
+ EXPECT_EQ(filter.Offset(t3), o3);
+
+ EXPECT_EQ(filter.Offset(t1, 0.0), std::make_pair(o1, 0.0));
+
+ EXPECT_EQ(filter.Offset(
+ e + (t2.time_since_epoch() + t1.time_since_epoch()) / 2, 0.0),
+ std::make_pair(o1, (o2d - o1d) / 2.));
+
+ EXPECT_EQ(filter.Offset(t2, 0.0), std::make_pair(o2, 0.0));
+
+ EXPECT_EQ(filter.Offset(
+ e + (t2.time_since_epoch() + t3.time_since_epoch()) / 2, 0.),
+ std::make_pair(o2, (o2d + o3d) / 2. - o2d));
+
+ EXPECT_EQ(filter.Offset(t3, 0.0), std::make_pair(o2, o3d - o2d));
+}
+
+// Tests that the cost function handles a single point line properly, and the
+// derivatives are consistent. Do this with a massive offset to ensure that we
+// are subtracting out nominal offsets correctly to retain numerical precision
+// in the result.
+TEST(NoncausalTimestampFilterTest, CostAndSlopeSinglePoint) {
+ const monotonic_clock::time_point e = monotonic_clock::epoch();
+ const monotonic_clock::time_point t1 =
+ e + chrono::nanoseconds(0) + chrono::seconds(10000000000);
+ const chrono::nanoseconds o1 =
+ chrono::nanoseconds(1000) - chrono::seconds(10000000000);
+
+ NoncausalTimestampFilter filter;
+
+ filter.Sample(t1, o1);
+
+ // Spot check some known points.
+ EXPECT_EQ(filter.OffsetError(t1, 0.0, t1 + o1, 0.0), 0.0);
+ EXPECT_EQ(filter.OffsetError(t1, 5.0, t1 + o1, 5.0), 0.0);
+ EXPECT_EQ(filter.Cost(t1, 0.0, t1 + o1, 0.0), 0.0);
+ EXPECT_EQ(filter.Cost(t1, 1.0, t1 + o1, 0.0), 1.0);
+
+ constexpr double kDelta = 10.;
+
+ // Perturb ta and tb so we make sure it works away from 0.
+ for (double ta_nominal : {-1000.0, 0.0, 1000.0}) {
+ for (double tb_nominal : {-2000.0, 0.0, 2000.0}) {
+ {
+ const double minus_costa =
+ filter.Cost(t1, ta_nominal - kDelta, t1 + o1, tb_nominal);
+ const double plus_costa =
+ filter.Cost(t1, ta_nominal + kDelta, t1 + o1, tb_nominal);
+
+ const double minus_costb =
+ filter.Cost(t1, ta_nominal, t1 + o1, tb_nominal - kDelta);
+ const double plus_costb =
+ filter.Cost(t1, ta_nominal, t1 + o1, tb_nominal + kDelta);
+
+ EXPECT_NEAR((plus_costa - minus_costa) / (2.0 * kDelta),
+ filter.DCostDta(t1, ta_nominal, t1 + o1, tb_nominal), 1e-9);
+ EXPECT_NEAR((plus_costb - minus_costb) / (2.0 * kDelta),
+ filter.DCostDtb(t1, ta_nominal, t1 + o1, tb_nominal), 1e-9);
+ }
+ }
+ }
+}
+
+TEST(NoncausalTimestampFilterTest, CostAndSlope) {
+ const monotonic_clock::time_point e = monotonic_clock::epoch();
+ // Note: t1, t2, t3 need to be picked such that the slope is small so filter
+ // doesn't modify the timestamps.
+ const monotonic_clock::time_point t1 =
+ e + chrono::nanoseconds(0) + chrono::seconds(10000000000);
+ const chrono::nanoseconds o1 =
+ chrono::nanoseconds(1000) - chrono::seconds(10000000000);
+
+ const monotonic_clock::time_point t2 =
+ e + chrono::microseconds(1000) + chrono::seconds(10000000000);
+ const chrono::nanoseconds o2 =
+ chrono::nanoseconds(1500) - chrono::seconds(10000000000);
+
+ const monotonic_clock::time_point t3 =
+ e + chrono::microseconds(2000) + chrono::seconds(10000000000);
+ const chrono::nanoseconds o3 =
+ chrono::nanoseconds(500) - chrono::seconds(10000000000);
+
+ NoncausalTimestampFilter filter;
+
+ filter.Sample(t1, o1);
+ filter.Sample(t2, o2);
+ filter.Sample(t3, o3);
+
+ // Spot check some known points.
+ EXPECT_EQ(filter.OffsetError(t1, 0.0, t1 + o1, 0.0), 0.0);
+ EXPECT_EQ(filter.OffsetError(t1, 5.0, t1 + o1, 5.0), -0.0025);
+ EXPECT_EQ(filter.OffsetError(t2, 0.0, t2 + o2, 0.0), 0.0);
+ EXPECT_EQ(filter.OffsetError(t3, 0.0, t3 + o3, 0.0), 0.0);
+
+ EXPECT_EQ(filter.Cost(t1, 0.0, t1 + o1, 0.0), 0.0);
+ EXPECT_EQ(filter.Cost(t2, 0.0, t2 + o2, 0.0), 0.0);
+ EXPECT_EQ(filter.Cost(t3, 0.0, t3 + o3, 0.0), 0.0);
+
+ // Perturb ta and tbd so we make sure it works away from 0.
+ constexpr double kDelta = 10.;
+
+ // Note: don't test 0 offset because that makes the computed slope at t2
+ // wrong.
+ for (double ta_nominal : {-1000.0, 20.0, 1000.0}) {
+ for (double tb_nominal : {-2000.0, 20.0, 2000.0}) {
+ // Check points round each of the 3 points in the polyline. Use 3 points
+ // so if we mess up the point selection code, it shows up.
+ {
+ const double minus_costa =
+ filter.Cost(t1, ta_nominal - kDelta, t1 + o1, tb_nominal);
+ const double plus_costa =
+ filter.Cost(t1, ta_nominal + kDelta, t1 + o1, tb_nominal);
+
+ const double minus_costb =
+ filter.Cost(t1, ta_nominal, t1 + o1, tb_nominal - kDelta);
+ const double plus_costb =
+ filter.Cost(t1, ta_nominal, t1 + o1, tb_nominal + kDelta);
+
+ EXPECT_NEAR((plus_costa - minus_costa) / (2.0 * kDelta),
+ filter.DCostDta(t1, ta_nominal, t1 + o1, tb_nominal), 1e-9);
+ EXPECT_NEAR((plus_costb - minus_costb) / (2.0 * kDelta),
+ filter.DCostDtb(t1, ta_nominal, t1 + o1, tb_nominal), 1e-9);
+ }
+
+ {
+ const double minus_costa =
+ filter.Cost(t2, ta_nominal - kDelta, t2 + o2, tb_nominal);
+ const double plus_costa =
+ filter.Cost(t2, ta_nominal + kDelta, t2 + o2, tb_nominal);
+
+ const double minus_costb =
+ filter.Cost(t2, ta_nominal, t2 + o2, tb_nominal - kDelta);
+ const double plus_costb =
+ filter.Cost(t2, ta_nominal, t2 + o2, tb_nominal + kDelta);
+
+ EXPECT_NEAR((plus_costa - minus_costa) / (2.0 * kDelta),
+ filter.DCostDta(t2, ta_nominal, t2 + o2, tb_nominal), 1e-9);
+ EXPECT_NEAR((plus_costb - minus_costb) / (2.0 * kDelta),
+ filter.DCostDtb(t2, ta_nominal, t2 + o2, tb_nominal), 1e-9);
+ }
+
+ {
+ const double minus_costa =
+ filter.Cost(t3, ta_nominal - kDelta, t3 + o3, tb_nominal);
+ const double plus_costa =
+ filter.Cost(t3, ta_nominal + kDelta, t3 + o3, tb_nominal);
+
+ const double minus_costb =
+ filter.Cost(t3, ta_nominal, t3 + o3, tb_nominal - kDelta);
+ const double plus_costb =
+ filter.Cost(t3, ta_nominal, t3 + o3, tb_nominal + kDelta);
+
+ EXPECT_NEAR((plus_costa - minus_costa) / (2.0 * kDelta),
+ filter.DCostDta(t3, ta_nominal, t3 + o3, tb_nominal), 1e-9);
+ EXPECT_NEAR((plus_costb - minus_costb) / (2.0 * kDelta),
+ filter.DCostDtb(t3, ta_nominal, t3 + o3, tb_nominal), 1e-9);
+ }
+ }
+ }
+}
+
// Run a couple of points through the estimator and confirm it works.
TEST(NoncausalOffsetEstimatorTest, FullEstimator) {
const aos::FlatbufferDetachedBuffer<Node> node_a_buffer =