Use 128 bit math for ExtrapolateOffset
We were being lazy with ExtrapolateOffset and using the point as the
integer part of the result, and putting all of the error in the double.
That gets imprecise when we extrapolate long distances, which is common
at the start of log files.
Instead, use the exact integer math, and use the double to capture the
remainder. This is significantly more precise, since we should never
see values over 1.0 in the double.
Change-Id: If8e2029a941494734f239bf43220ea698c9b998b
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index c1cf612..7d25dee 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -769,37 +769,7 @@
chrono::nanoseconds NoncausalTimestampFilter::ExtrapolateOffset(
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
monotonic_clock::time_point ta) {
- const chrono::nanoseconds dt = ta - std::get<0>(p0);
- if (dt <= std::chrono::nanoseconds(0)) {
- // Extrapolate backwards, using the (positive) MaxVelocity slope
- // We've been asked to extrapolate the offset to a time before our first
- // sample point. To be conservative, we'll return an extrapolated
- // offset that is less than (less tight an estimate of the network delay)
- // than our sample offset, bound by the max slew velocity we allow
- // p0
- // /
- // /
- // ta
- // Since dt < 0, we shift by dt * slope to get that value
- return std::get<1>(p0) +
- chrono::nanoseconds(static_cast<int64_t>(
- (absl::int128(dt.count() - MaxVelocityRatio::den / 2) *
- absl::int128(MaxVelocityRatio::num)) /
- absl::int128(MaxVelocityRatio::den)));
- } else {
- // Extrapolate forwards, using the (negative) MaxVelocity slope
- // Same concept, except going foward past our last (most recent) sample:
- // pN
- // |
- // |
- // ta
- // Since dt > 0, we shift by - dt * slope to get that value
- return std::get<1>(p0) -
- chrono::nanoseconds(static_cast<int64_t>(
- (absl::int128(dt.count() + MaxVelocityRatio::den / 2) *
- absl::int128(MaxVelocityRatio::num)) /
- absl::int128(MaxVelocityRatio::den)));
- }
+ return ExtrapolateOffset(p0, ta, 0.0).first;
}
chrono::nanoseconds NoncausalTimestampFilter::InterpolateOffset(
@@ -868,31 +838,61 @@
(std::get<1>(p1) - std::get<1>(p0)).count();
}
-chrono::nanoseconds NoncausalTimestampFilter::ExtrapolateOffset(
- std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
- monotonic_clock::time_point /*ta_base*/, double /*ta*/) {
- // TODO(austin): 128 bit math again? ...
- // For this version, use the base offset from p0 as the base for the offset
- return std::get<1>(p0);
-}
-
-double NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
+std::pair<chrono::nanoseconds, double> NoncausalTimestampFilter::ExtrapolateOffset(
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
monotonic_clock::time_point ta_base, double ta) {
- // Compute the remainder portion of this offset
- // oa = p0.o +/- ((ta + ta_base) - p0.t)) * kMaxVelocity()
- // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
- // But compute (ta + ta_base - p0.t) as (ta + (ta_base - p0.t))
- // to handle numerical precision
- const chrono::nanoseconds time_in = ta_base - std::get<0>(p0);
- const double dt = static_cast<double>(ta + time_in.count());
- if (dt < 0.0) {
- // Extrapolate backwards with max (positive) slope (which means
- // the returned offset should be negative)
- return dt * kMaxVelocity();
+ DCHECK_GE(ta, 0.0);
+ DCHECK_LT(ta, 1.0);
+ // Since the point (p0) is an integer, we now can guarantee that ta won't put
+ // us on a different side of p0. This is because ta is between 0 and 1, and
+ // always positive. Compute the integer and double portions and return them.
+ const chrono::nanoseconds dt = ta_base - std::get<0>(p0);
+
+ if (dt < std::chrono::nanoseconds(0)) {
+ // Extrapolate backwards, using the (positive) MaxVelocity slope
+ // We've been asked to extrapolate the offset to a time before our first
+ // sample point. To be conservative, we'll return an extrapolated
+ // offset that is less than (less tight an estimate of the network delay)
+ // than our sample offset, bound by the max slew velocity we allow
+ // p0
+ // /
+ // /
+ // ta
+ // Since dt < 0, we shift by dt * slope to get that value
+ //
+ // Take the remainder of the math in ExtrapolateOffset above and compute it
+ // with floating point math. Our tests are good enough to confirm that this
+ // works as designed.
+ const absl::int128 numerator =
+ (absl::int128(dt.count() - MaxVelocityRatio::den / 2) *
+ absl::int128(MaxVelocityRatio::num));
+ return std::make_pair(
+ std::get<1>(p0) + chrono::nanoseconds(static_cast<int64_t>(
+ numerator / absl::int128(MaxVelocityRatio::den))),
+ static_cast<double>(numerator % absl::int128(MaxVelocityRatio::den)) /
+ static_cast<double>(MaxVelocityRatio::den) +
+ 0.5 + ta * kMaxVelocity());
} else {
- // Extrapolate forwards with max (negative) slope
- return -dt * kMaxVelocity();
+ // Extrapolate forwards, using the (negative) MaxVelocity slope
+ // Same concept, except going foward past our last (most recent) sample:
+ // pN
+ // |
+ // |
+ // ta
+ // Since dt > 0, we shift by - dt * slope to get that value
+ //
+ // Take the remainder of the math in ExtrapolateOffset above and compute it
+ // with floating point math. Our tests are good enough to confirm that this
+ // works as designed.
+ const absl::int128 numerator =
+ absl::int128(dt.count() + MaxVelocityRatio::den / 2) *
+ absl::int128(MaxVelocityRatio::num);
+ return std::make_pair(
+ std::get<1>(p0) - chrono::nanoseconds(static_cast<int64_t>(
+ numerator / absl::int128(MaxVelocityRatio::den))),
+ -static_cast<double>(numerator % absl::int128(MaxVelocityRatio::den)) /
+ static_cast<double>(MaxVelocityRatio::den) +
+ 0.5 - ta * kMaxVelocity());
}
}
@@ -931,12 +931,9 @@
std::pair<Pointer,
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>>
reference_timestamp = GetReferenceTimestamp(ta_base, ta);
- return std::make_pair(
- reference_timestamp.first,
- std::make_pair(NoncausalTimestampFilter::ExtrapolateOffset(
- reference_timestamp.second, ta_base, ta),
- NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
- reference_timestamp.second, ta_base, ta)));
+ return std::make_pair(reference_timestamp.first,
+ NoncausalTimestampFilter::ExtrapolateOffset(
+ reference_timestamp.second, ta_base, ta));
}
std::pair<
@@ -1061,25 +1058,22 @@
auto reference_timestamp = GetReferenceTimestamp(ta_base, ta);
// Special case size = 1 or ta before first timestamp, so we extrapolate
- const chrono::nanoseconds offset_base =
+ const std::pair<chrono::nanoseconds, double> offset =
NoncausalTimestampFilter::ExtrapolateOffset(reference_timestamp.second,
ta_base, ta);
- const double offset_remainder =
- NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
- reference_timestamp.second, ta_base, ta);
// We want to do offset + ta > tb, but we need to do it with minimal
// numerical precision problems.
// See below for why this is a >=
- if (static_cast<double>((offset_base + ta_base - tb_base).count()) >=
- tb - ta - offset_remainder) {
+ if (static_cast<double>((offset.first + ta_base - tb_base).count()) >=
+ tb - ta - offset.second) {
LOG(ERROR) << node_names_ << " "
- << TimeString(ta_base, ta, offset_base, offset_remainder)
+ << TimeString(ta_base, ta, offset.first, offset.second)
<< " > solution time "
<< tb_base + chrono::nanoseconds(
static_cast<int64_t>(std::round(tb)))
<< ", " << tb - std::round(tb) << " foo";
- LOG(INFO) << "Remainder " << offset_remainder;
+ LOG(INFO) << "Remainder " << offset.second;
return false;
}
return true;
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index fd430ce..79e3c5b 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -591,11 +591,7 @@
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> /*p1*/,
monotonic_clock::time_point ta_base, double ta);
- static std::chrono::nanoseconds ExtrapolateOffset(
- std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
- monotonic_clock::time_point /*ta_base*/, double /*ta*/);
-
- static double ExtrapolateOffsetRemainder(
+ static std::pair<std::chrono::nanoseconds, double> ExtrapolateOffset(
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
monotonic_clock::time_point ta_base, double ta);
diff --git a/aos/network/timestamp_filter_test.cc b/aos/network/timestamp_filter_test.cc
index 5932e3d..d87e63d 100644
--- a/aos/network/timestamp_filter_test.cc
+++ b/aos/network/timestamp_filter_test.cc
@@ -36,6 +36,28 @@
}
};
+void NormalizeTimestamps(monotonic_clock::time_point *ta_base, double *ta) {
+ double ta_orig = *ta;
+ chrono::nanoseconds ta_digits(static_cast<int64_t>(std::floor(*ta)));
+ *ta_base += ta_digits;
+ *ta -= static_cast<double>(ta_digits.count());
+
+ // Sign, numerical precision wins again.
+ // *ta_base=1000.300249970sec, *ta=-1.35525e-20
+ // We then promptly round this to
+ // *ta_base=1000.300249969sec, *ta=1
+ // The 1.0 then breaks the LT assumption below, so we kersplat.
+ //
+ // Detect this case directly and move the 1.0 back into ta_base.
+ if (*ta == 1.0) {
+ *ta = 0.0;
+ *ta_base += chrono::nanoseconds(1);
+ }
+
+ CHECK_GE(*ta, 0.0) << ta_digits.count() << "ns " << ta_orig;
+ CHECK_LT(*ta, 1.0);
+}
+
// Tests that adding samples tracks more negative offsets down quickly, and
// slowly comes back up.
TEST(TimestampFilterTest, Sample) {
@@ -857,7 +879,17 @@
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 1200.),
60.);
+}
+// Tests that all variants of ExtrapolateOffset do reasonable things.
+TEST_F(NoncausalTimestampFilterTest, ExtrapolateOffset) {
+ const monotonic_clock::time_point e = monotonic_clock::epoch();
+
+ const monotonic_clock::time_point t1 = e + chrono::nanoseconds(10000);
+ const chrono::nanoseconds o1 = chrono::nanoseconds(100);
+
+ const monotonic_clock::time_point t2 = t1 + chrono::nanoseconds(1000);
+ const chrono::nanoseconds o2 = chrono::nanoseconds(150);
// Test extrapolation functions before t1 and after t2
EXPECT_EQ(
NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1), t1),
@@ -889,24 +921,14 @@
// Test base + double version
EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1),
e, 0.),
- o1);
- EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
- std::make_tuple(t1, o1), e, 0.),
- -(t1 - e).count() * kMaxVelocity());
-
+ std::make_pair(chrono::nanoseconds(90), 0.0));
EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1),
t1, 0.),
- o1);
- EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
- std::make_tuple(t1, o1), t1, 0.),
- 0.);
+ std::make_pair(o1, 0.0));
EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1),
- t1, -1000.),
- o1);
- EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
- std::make_tuple(t1, o1), t1, -1000.),
- -1000. * kMaxVelocity());
+ t1, 0.5),
+ std::make_pair(o1, -0.5 * kMaxVelocity()));
EXPECT_EQ(
NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t2, o2), t2),
@@ -914,10 +936,7 @@
EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t2, o2),
t2, 0.0),
- o2);
- EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
- std::make_tuple(t2, o2), t2, 0.0),
- 0.0);
+ std::make_pair(o2, 0.0));
// Test points past our last sample
EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
@@ -925,12 +944,40 @@
chrono::nanoseconds(
static_cast<int64_t>(o2.count() - 10000. * kMaxVelocity())));
- EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t2, o2),
- t2, 100.0),
- o2);
- EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
- std::make_tuple(t2, o2), t2, 100.0),
- -100.0 * kMaxVelocity());
+ EXPECT_EQ(
+ NoncausalTimestampFilter::ExtrapolateOffset(
+ std::make_tuple(t2, o2), t2 + chrono::nanoseconds(10000), 0.5),
+ std::make_pair(o2 - chrono::nanoseconds(10), -0.5 * kMaxVelocity()));
+
+ // Now, test that offset + remainder functions add up to the right answer for
+ // a lot of cases. This is enough to catch all the various rounding cases.
+ for (int i = -MaxVelocityRatio::den * MaxVelocityRatio::num * 6;
+ i < MaxVelocityRatio::den * MaxVelocityRatio::num * 4; ++i) {
+ monotonic_clock::time_point ta_base = t1;
+ const double ta_orig = static_cast<double>(i) / 3.0;
+ double ta = ta_orig;
+
+ NormalizeTimestamps(&ta_base, &ta);
+ CHECK_GE(ta, 0.0);
+ CHECK_LT(ta, 1.0);
+
+ const chrono::nanoseconds expected_offset =
+ NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1),
+ ta_base);
+
+
+ std::pair<chrono::nanoseconds, double> offset =
+ NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1),
+ ta_base, ta);
+
+ EXPECT_EQ(expected_offset, offset.first);
+ EXPECT_NEAR(
+ static_cast<double>(offset.first.count()) + offset.second,
+ static_cast<double>(o1.count()) - std::abs(ta_orig) * kMaxVelocity(),
+ 1e-9)
+ << ": i " << i << " t " << ta_base << " " << ta
+ << " Non-rounded: " << expected_offset.count() << "ns";
+ }
}
// Tests that FindTimestamps finds timestamps in a sequence.
@@ -1211,14 +1258,18 @@
const double offset_pre = -(t1.time - e.time).count() * kMaxVelocity();
EXPECT_EQ(filter.Offset(nullptr, Pointer(), e, 0),
o1 + chrono::nanoseconds(static_cast<int64_t>(offset_pre)));
- EXPECT_EQ(filter.Offset(nullptr, Pointer(), e, 0.0, 0),
- std::make_pair(o1, offset_pre));
+ EXPECT_EQ(
+ filter.Offset(nullptr, Pointer(), e, 0.0, 0),
+ std::make_pair(o1 + chrono::nanoseconds(static_cast<int64_t>(offset_pre)),
+ 0.0));
double offset_post = -(t2.time - t1.time).count() * kMaxVelocity();
EXPECT_EQ(filter.Offset(nullptr, Pointer(), t2, 0),
o1 + chrono::nanoseconds(static_cast<int64_t>(offset_post)));
- EXPECT_EQ(filter.Offset(nullptr, Pointer(), t2, 0.0, 0),
- std::make_pair(o1, offset_post));
+ EXPECT_EQ(
+ filter.Offset(nullptr, Pointer(), t2, 0.0, 0),
+ std::make_pair(
+ o1 + chrono::nanoseconds(static_cast<int64_t>(offset_post)), 0.0));
filter.Sample(t2, o2);
filter.Sample(t3, o3);
@@ -1251,14 +1302,18 @@
// Check that we still get same answer for times before our sample data...
EXPECT_EQ(filter.Offset(nullptr, Pointer(), e, 0),
o1 + chrono::nanoseconds(static_cast<int64_t>(offset_pre)));
- EXPECT_EQ(filter.Offset(nullptr, Pointer(), e, 0.0, 0),
- std::make_pair(o1, offset_pre));
+ EXPECT_EQ(
+ filter.Offset(nullptr, Pointer(), e, 0.0, 0),
+ std::make_pair(o1 + chrono::nanoseconds(static_cast<int64_t>(offset_pre)),
+ 0.0));
// ... and after
offset_post = -(t4.time - t3.time).count() * kMaxVelocity();
EXPECT_EQ(filter.Offset(nullptr, Pointer(), t4, 0),
(o3 + chrono::nanoseconds(static_cast<int64_t>(offset_post))));
- EXPECT_EQ(filter.Offset(nullptr, Pointer(), t4, 0.0, 0),
- std::make_pair(o3, offset_post));
+ EXPECT_EQ(
+ filter.Offset(nullptr, Pointer(), t4, 0.0, 0),
+ std::make_pair(
+ o3 + chrono::nanoseconds(static_cast<int64_t>(offset_post)), 0.0));
}
// Tests that adding duplicates gets correctly deduplicated.