Compute both integer and remainder at once in InterpolateOffset
We rarely want one of the two numbers, and there's a lot of duplicated
logic and ideas. Combine them to make life easier.
Change-Id: Ife54a70ea987906a918fc8f207ed95db0dec9d77
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index 4ec9e0a..336987c 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -776,6 +776,17 @@
std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p0,
std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p1,
monotonic_clock::time_point ta) {
+ return InterpolateOffset(p0, p1, ta, 0.0).first;
+}
+
+std::pair<chrono::nanoseconds, double>
+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) {
+ DCHECK_GE(ta, 0.0);
+ DCHECK_LT(ta, 1.0);
+
// 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
@@ -787,47 +798,31 @@
//
// Add (or subtract, integer division rounds towards 0...) 0.5 ((dt / 2) / dt)
// to the numerator to round to the nearest number rather than round down.
- const chrono::nanoseconds time_in = ta - std::get<0>(p0);
- const chrono::nanoseconds dt = std::get<0>(p1) - std::get<0>(p0);
-
- absl::int128 numerator =
- absl::int128(time_in.count()) *
- absl::int128((std::get<1>(p1) - std::get<1>(p0)).count());
- numerator += numerator > 0 ? absl::int128(dt.count() / 2)
- : -absl::int128(dt.count() / 2);
- return std::get<1>(p0) + chrono::nanoseconds(static_cast<int64_t>(
- numerator / absl::int128(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) {
- DCHECK_GE(ta, 0.0);
- DCHECK_LT(ta, 1.0);
-
- return InterpolateOffset(p0, p1, ta_base);
-}
-
-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);
const chrono::nanoseconds doffset = std::get<1>(p1) - std::get<1>(p0);
- // Compute the remainder of the division in InterpolateOffset above, and then
- // use double math to compute it accurately.
absl::int128 numerator =
absl::int128(time_in.count()) * absl::int128(doffset.count());
numerator += numerator > 0 ? absl::int128(dt.count() / 2)
: -absl::int128(dt.count() / 2);
- return static_cast<double>(numerator % absl::int128(dt.count())) /
- dt.count() +
- (numerator > 0 ? -0.5 : 0.5) +
- ta * static_cast<double>(doffset.count()) /
- static_cast<double>(dt.count());
+
+ const chrono::nanoseconds integer =
+ std::get<1>(p0) + chrono::nanoseconds(static_cast<int64_t>(
+ numerator / absl::int128(dt.count())));
+ // Compute the remainder of the division in InterpolateOffset above, and
+ // then use double math to compute it accurately. Since integer math rounds
+ // down, we need to undo the rounding to get the double remainder. Add or
+ // subtract dt/2/dt (0.5) to undo the addition.
+ //
+ // We have good tests which confirm for small offsets this matches nicely. For
+ // large offsets, the 128 bit math will take care of us.
+ const double remainder =
+ static_cast<double>(numerator % absl::int128(dt.count())) / dt.count() +
+ (numerator > 0 ? -0.5 : 0.5) +
+ ta * static_cast<double>(doffset.count()) /
+ static_cast<double>(dt.count());
+ return std::make_pair(integer, remainder);
}
chrono::nanoseconds NoncausalTimestampFilter::BoundOffset(
@@ -973,11 +968,8 @@
// lookup.
return std::make_pair(
points.first,
- std::make_pair(
- NoncausalTimestampFilter::InterpolateOffset(
- points.second.first, points.second.second, ta_base, ta),
- NoncausalTimestampFilter::InterpolateOffsetRemainder(
- points.second.first, points.second.second, ta_base, ta)));
+ NoncausalTimestampFilter::InterpolateOffset(
+ points.second.first, points.second.second, ta_base, ta));
}
std::pair<Pointer, double> NoncausalTimestampFilter::SingleFilter::OffsetError(
@@ -1111,16 +1103,14 @@
std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>>
points = FindTimestamps(other, pointer, ta_base, ta);
- const chrono::nanoseconds offset_base =
+ const std::pair<chrono::nanoseconds, double> offset =
NoncausalTimestampFilter::InterpolateOffset(
points.second.first, points.second.second, ta_base, ta);
- const double offset = NoncausalTimestampFilter::InterpolateOffsetRemainder(
- points.second.first, points.second.second, ta_base, ta);
// See below for why this is a >=
- if (static_cast<double>((offset_base + ta_base - tb_base).count()) >=
- tb - offset - ta) {
+ if (static_cast<double>((offset.first + ta_base - tb_base).count()) >=
+ tb - offset.second - ta) {
LOG(ERROR) << node_names_ << " "
- << TimeString(ta_base, ta, offset_base, offset)
+ << TimeString(ta_base, ta, offset.first, offset.second)
<< " > solution time " << tb_base << ", " << tb;
LOG(ERROR) << "Bracketing times are " << TimeString(points.second.first)
<< " and " << TimeString(points.second.second);
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index a056fce..72ffb4f 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -583,7 +583,7 @@
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
monotonic_clock::time_point ta);
- static std::chrono::nanoseconds InterpolateOffset(
+ static std::pair<std::chrono::nanoseconds, double> 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);
diff --git a/aos/network/timestamp_filter_test.cc b/aos/network/timestamp_filter_test.cc
index 1669e62..b354d0e 100644
--- a/aos/network/timestamp_filter_test.cc
+++ b/aos/network/timestamp_filter_test.cc
@@ -806,20 +806,14 @@
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);
+ std::make_pair(o1, 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),
- o2);
- EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2), t2, 0.0),
- 0.0);
+ std::make_pair(o2, 0.0));
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
@@ -828,11 +822,7 @@
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
t1 + chrono::nanoseconds(500), 0.0),
- o1 + chrono::nanoseconds(25));
- EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2),
- t1 + chrono::nanoseconds(500), 0.0),
- 0.0);
+ std::make_pair(o1 + chrono::nanoseconds(25), 0.0));
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
@@ -841,11 +831,7 @@
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
t1 - chrono::nanoseconds(200), 0.0),
- o1 - chrono::nanoseconds(10));
- EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2),
- t1 - chrono::nanoseconds(200), 0.0),
- 0.0);
+ std::make_pair(o1 - chrono::nanoseconds(10), 0.0));
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
@@ -854,11 +840,7 @@
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
t1 + chrono::nanoseconds(200), 0.0),
- o1 + chrono::nanoseconds(10));
- EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2),
- t1 + chrono::nanoseconds(200), 0.0),
- 0.0);
+ std::make_pair(o1 + chrono::nanoseconds(10), 0.0));
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
@@ -867,11 +849,7 @@
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
t1 + chrono::nanoseconds(800), 0.0),
- o1 + chrono::nanoseconds(40));
- EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2),
- t1 + chrono::nanoseconds(800), 0.0),
- 0.0);
+ std::make_pair(o1 + chrono::nanoseconds(40), 0.0));
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
@@ -880,11 +858,7 @@
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
t1 + chrono::nanoseconds(1200), 0.0),
- o1 + chrono::nanoseconds(60));
- EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2),
- t1 + chrono::nanoseconds(1200), 0.0),
- 0.0);
+ std::make_pair(o1 + chrono::nanoseconds(60), 0.0));
for (int i = -MaxVelocityRatio::den * MaxVelocityRatio::num * 6;
i <
@@ -902,23 +876,18 @@
NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2), ta_base);
- EXPECT_EQ(expected_offset, NoncausalTimestampFilter::InterpolateOffset(
- std::make_tuple(t1, o1),
- std::make_tuple(t2, o2), ta_base, ta));
+ const std::pair<chrono::nanoseconds, double> offset =
+ NoncausalTimestampFilter::InterpolateOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), ta_base, ta);
+ EXPECT_EQ(expected_offset, offset.first);
const double expected_double_offset =
static_cast<double>(o1.count()) +
static_cast<double>(ta_orig) / static_cast<double>((t2 - t1).count()) *
(o2 - o1).count();
- EXPECT_NEAR(
- static_cast<double>(
- NoncausalTimestampFilter::InterpolateOffset(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2), ta_base, ta)
- .count()) +
- NoncausalTimestampFilter::InterpolateOffsetRemainder(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2), ta_base, ta),
- expected_double_offset, 1e-9)
+ EXPECT_NEAR(static_cast<double>(offset.first.count()) + offset.second,
+ expected_double_offset, 1e-9)
<< ": i " << i << " t " << ta_base << " " << ta << " t1 " << t1
<< " o1 " << o1.count() << "ns t2 " << t2 << " o2 " << o2.count()
<< "ns Non-rounded: " << expected_offset.count() << "ns";