Do extrapolation on timestamps and offsets before or after we have data
Change-Id: Ieff397a9c77e07dc85236dfc52974f259c37a6b9
diff --git a/aos/network/multinode_timestamp_filter_test.cc b/aos/network/multinode_timestamp_filter_test.cc
index 1c1ca54..07d3712 100644
--- a/aos/network/multinode_timestamp_filter_test.cc
+++ b/aos/network/multinode_timestamp_filter_test.cc
@@ -180,7 +180,7 @@
problem.add_filter(0, &a, 1);
problem.add_filter(1, &b, 0);
- // Solve the problem with infinate precision as a verification and compare the
+ // Solve the problem with infinite precision as a verification and compare the
// result.
{
const std::vector<double> result = problem.SolveDouble();
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index c623cbe..5e4d115 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -499,6 +499,7 @@
(std::get<0>(sample) + std::get<1>(sample)).time_since_epoch())
.count());
}
+ fflush(samples_fp_);
saved_samples_.clear();
}
@@ -520,6 +521,7 @@
NoncausalTimestampFilter::FindTimestamps(monotonic_clock::time_point ta) const {
CHECK_GT(timestamps_size(), 1u);
// Linear search until this is proven to be a measurable slowdown.
+ // If ta is outside our timestamp range, return the closest pair
size_t index = 0;
while (index < timestamps_size() - 2u) {
if (std::get<0>(timestamp(index + 1)) > ta) {
@@ -530,6 +532,36 @@
return std::make_pair(timestamp(index), timestamp(index + 1));
}
+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::duration_cast<chrono::nanoseconds>(dt * kMaxVelocity());
+ } 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::duration_cast<chrono::nanoseconds>(dt * kMaxVelocity());
+ }
+}
+
chrono::nanoseconds NoncausalTimestampFilter::InterpolateOffset(
std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p0,
std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p1,
@@ -596,13 +628,78 @@
(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*/) {
+ // For this version, use the base offset from p0 as the base for the offset
+ return std::get<1>(p0);
+}
+
+double NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
+ 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();
+ } else {
+ // Extrapolate forwards with max (negative) slope
+ return -dt * kMaxVelocity();
+ }
+}
+
+bool NoncausalTimestampFilter::IsOutsideSamples(
+ monotonic_clock::time_point ta_base, double ta) const {
+ DCHECK_GE(ta, 0.0);
+ DCHECK_LT(ta, 1.0);
+ if (timestamps_size() == 1u || ta_base < std::get<0>(timestamp(0)) ||
+ ta_base >= std::get<0>(timestamp(timestamps_size() - 1u))) {
+ return true;
+ }
+
+ return false;
+}
+
+bool NoncausalTimestampFilter::IsAfterSamples(
+ monotonic_clock::time_point ta_base, double ta) const {
+ DCHECK_GE(ta, 0.0);
+ DCHECK_LT(ta, 1.0);
+ if (ta_base >= std::get<0>(timestamp(timestamps_size() - 1u))) {
+ return true;
+ }
+
+ return false;
+}
+
+std::tuple<monotonic_clock::time_point, chrono::nanoseconds>
+NoncausalTimestampFilter::GetReferenceTimestamp(
+ monotonic_clock::time_point ta_base, double ta) const {
+ DCHECK_GE(ta, 0.0);
+ DCHECK_LT(ta, 1.0);
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>
+ reference_timestamp = timestamp(0);
+ if (ta_base >= std::get<0>(timestamp(timestamps_size() - 1u))) {
+ reference_timestamp = timestamp(timestamps_size() - 1u);
+ }
+
+ return reference_timestamp;
+}
+
chrono::nanoseconds NoncausalTimestampFilter::Offset(
monotonic_clock::time_point ta) const {
CHECK_GT(timestamps_size(), 0u);
- if (timestamps_size() == 1u || ta < std::get<0>(timestamp(0))) {
- // 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);
+ if (IsOutsideSamples(ta, 0.)) {
+ // Special case when size = 1 or if we're asked to extrapolate to
+ // times before or after we have data.
+ auto reference_timestamp = GetReferenceTimestamp(ta, 0.);
+ return NoncausalTimestampFilter::ExtrapolateOffset(reference_timestamp, ta);
}
std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
@@ -615,12 +712,14 @@
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 || ta_base < std::get<0>(timestamp(0))) {
- // 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);
+ if (IsOutsideSamples(ta_base, ta)) {
+ // Special case size = 1 or ta_base before first timestamp or
+ // after last timesteamp, so we need to extrapolate out
+ auto reference_timestamp = GetReferenceTimestamp(ta_base, ta);
+ return std::make_pair(NoncausalTimestampFilter::ExtrapolateOffset(
+ reference_timestamp, ta_base, ta),
+ NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
+ reference_timestamp, ta_base, ta));
}
std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
@@ -680,7 +779,7 @@
// 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 equivalent to:
- // 2 * (tb - ta - Offset(ta)) * (-(1 + ma))
+ // - 2 * (tb - ta - Offset(ta)) * (1 + ma)
//
// We can compute this a lot more accurately.
@@ -690,8 +789,20 @@
NormalizeTimestamps(&ta_base, &ta);
NormalizeTimestamps(&tb_base, &tb);
- if (timestamps_size() == 1u || ta_base < std::get<0>(timestamp(0))) {
- return -2.0 * OffsetError(ta_base, ta, tb_base, tb);
+ if (IsOutsideSamples(ta_base, ta)) {
+ double slope = kMaxVelocity();
+ if (IsAfterSamples(ta_base, ta)) {
+ // If we're past the last sample point, use a negative slope
+ slope = -kMaxVelocity();
+ }
+ // This implements the high precision version of the above equation:
+ // - 2 * (tb - ta - Offset(ta)) * (1 + ma)
+ // = -2 * OffsetError(ta,tb) * (1 + ma)
+ // Where for extrapolated data, we have to extrapolate for Offset(ta)
+ // and use +/- kMaxVelocity() for ma
+ // (slope is positive if timepoint is before our first sample, and
+ // negative if the point is after our last sample)
+ return -2.0 * OffsetError(ta_base, ta, tb_base, tb) * (1.0 + slope);
}
std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
@@ -708,6 +819,8 @@
//
// (tb - ta - Offset(ta)) ->
// (tb_base - ta_base - OffsetBase + tb - ta - OffsetRemainder)
+ // NOTE: We don't use OffsetError function here, in order to avoid
+ // extra calls to FindTimestamps
return -2.0 *
(static_cast<double>((tb_base - ta_base -
NoncausalTimestampFilter::InterpolateOffset(
@@ -726,9 +839,21 @@
NormalizeTimestamps(&ta_base, &ta);
NormalizeTimestamps(&tb_base, &tb);
- if (timestamps_size() == 1u || ta_base < std::get<0>(timestamp(0))) {
- return absl::StrFormat("-2. * (t%d - t%d - %d)", node_b, node_a,
- std::get<1>(timestamp(0)).count());
+ if (IsOutsideSamples(ta_base, ta)) {
+ auto reference_timestamp = GetReferenceTimestamp(ta_base, ta);
+ double slope = kMaxVelocity();
+ std::string note = "_";
+ if (IsAfterSamples(ta_base, ta)) {
+ slope = -kMaxVelocity();
+ note = "^";
+ }
+ // d cost / dta ==>
+ // - 2 * (tb - ta - (ta - ref) * ma - ref_offset) * (1 + ma)
+ return absl::StrFormat(
+ "-2. * (t%d - t%d - ((t%d - %d) * %f - %d.) * (1 + %f.)%s", node_b,
+ node_a, node_a,
+ std::get<0>(reference_timestamp).time_since_epoch().count(), slope,
+ std::get<1>(reference_timestamp).count(), slope, note);
}
std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
@@ -786,9 +911,20 @@
NormalizeTimestamps(&ta_base, &ta);
NormalizeTimestamps(&tb_base, &tb);
- if (timestamps_size() == 1u || ta_base < std::get<0>(timestamp(0))) {
- return absl::StrFormat("2. * (t%d - t%d - %d)", node_b, node_a,
- std::get<1>(timestamp(0)).count());
+ if (IsOutsideSamples(ta_base, ta)) {
+ auto reference_timestamp = GetReferenceTimestamp(ta_base, ta);
+ double slope = kMaxVelocity();
+ std::string note = "_";
+ if (IsAfterSamples(ta_base, ta)) {
+ slope = -kMaxVelocity();
+ note = "^";
+ }
+ // d cost / dtb ==> 2 * OffsetError(ta, tb) ==>
+ // 2 * (tb - ta - (ta - ref) * ma - ref_offset)
+ return absl::StrFormat(
+ "2. * (t%d - t%d - ((t%d - %d) * %f + %d)%s", node_b, node_a, node_a,
+ std::get<0>(reference_timestamp).time_since_epoch().count(), slope,
+ std::get<1>(reference_timestamp).count(), note);
}
std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
@@ -800,7 +936,7 @@
// ie
// ((tb - (1 + ma) ta - ba)^2
//
- // d cost/dta =>
+ // d cost/dtb => 2 * OffsetError(ta, tb) ==>
// 2 * ((tb - (1 + ma) ta - ba)
const int64_t rise =
@@ -827,9 +963,20 @@
NormalizeTimestamps(&ta_base, &ta);
NormalizeTimestamps(&tb_base, &tb);
- if (timestamps_size() == 1u || ta_base < std::get<0>(timestamp(0))) {
- return absl::StrFormat("(t%d - t%d - %d) ** 2.", node_b, node_a,
- std::get<1>(timestamp(0)).count());
+ if (IsOutsideSamples(ta_base, ta)) {
+ auto reference_timestamp = GetReferenceTimestamp(ta_base, ta);
+ double slope = kMaxVelocity();
+ std::string note = "_";
+ if (IsAfterSamples(ta_base, ta)) {
+ slope = -kMaxVelocity();
+ note = "^";
+ }
+ // Cost = OffsetError(ta, tb) ** 2 = (tb - ta - Offset(ta, tb)) ** 2 ==>
+ // (tb - ta - ((ta - ref) * ma - ref_offset)
+ return absl::StrFormat(
+ "(t%d - t%d - (t%d - %d.) * %f - %d.)%s ** 2.", node_b, node_a, node_a,
+ std::get<0>(reference_timestamp).time_since_epoch().count(), slope,
+ std::get<1>(reference_timestamp).count(), note);
}
std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
@@ -865,11 +1012,14 @@
aos::monotonic_clock::time_point ta,
aos::monotonic_clock::time_point tb) const {
CHECK_GT(timestamps_size(), 0u);
- if (timestamps_size() == 1u || ta < std::get<0>(timestamp(0))) {
- // Special case size = 1 since the interpolation functions don't need to
- // handle it and the answer is trivial.
+ if (IsOutsideSamples(ta, 0.)) {
+ // Special case size = 1 or ta_base before first timestamp or
+ // after last timestamp, so we need to extrapolate out
+ auto reference_timestamp = GetReferenceTimestamp(ta, 0.);
+
+ // Special case size = 1 or ta before first timestamp, so we extrapolate
const chrono::nanoseconds offset =
- NoncausalTimestampFilter::InterpolateOffset(timestamp(0), ta);
+ NoncausalTimestampFilter::ExtrapolateOffset(reference_timestamp, ta);
if (offset + ta > tb) {
LOG(ERROR) << node_a_->name()->string_view() << " -> "
<< node_b_->name()->string_view() << " "
@@ -1271,6 +1421,7 @@
std::chrono::duration_cast<std::chrono::duration<double>>(
std::get<1>(timestamp))
.count());
+ fflush(fp_);
}
}
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index 49a4d9f..5d577a7 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -14,6 +14,9 @@
namespace aos {
namespace message_bridge {
+// TODO<jim>: Should do something to help with precision, like make it an
+// integer and divide by the value (e.g., / 1000)
+
// Max velocity to clamp the filter to in seconds/second.
inline constexpr double kMaxVelocity() { return 0.001; }
@@ -239,6 +242,16 @@
: node_a_(node_a), node_b_(node_b) {}
~NoncausalTimestampFilter();
+ // Check whether the given timestamp falls within our current samples
+ bool IsOutsideSamples(monotonic_clock::time_point ta_base,
+ double ta) const;
+
+ // Check whether the given timestamp lies after our current samples
+ bool IsAfterSamples(monotonic_clock::time_point ta_base, double ta) const;
+
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>
+ GetReferenceTimestamp(monotonic_clock::time_point ta_base, double ta) const;
+
// 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;
@@ -380,26 +393,27 @@
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p1,
monotonic_clock::time_point ta);
- static std::chrono::nanoseconds InterpolateOffset(
+ static std::chrono::nanoseconds ExtrapolateOffset(
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
- monotonic_clock::time_point /*ta*/) {
- return std::get<1>(p0);
- }
+ monotonic_clock::time_point ta);
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(
+ static std::chrono::nanoseconds ExtrapolateOffset(
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
- monotonic_clock::time_point /*ta_base*/, double /*ta*/) {
- return std::get<1>(p0);
- }
+ monotonic_clock::time_point /*ta_base*/, double /*ta*/);
+
+ static double ExtrapolateOffsetRemainder(
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
+ monotonic_clock::time_point ta_base, double ta);
private:
// Removes the oldest timestamp.
diff --git a/aos/network/timestamp_filter_test.cc b/aos/network/timestamp_filter_test.cc
index 8ea4d57..caecccd 100644
--- a/aos/network/timestamp_filter_test.cc
+++ b/aos/network/timestamp_filter_test.cc
@@ -77,14 +77,14 @@
}
class FilterTest : public ::testing::Test {
- public:
- FlatbufferDetachedBuffer<Node> node_a_buffer =
- JsonToFlatbuffer<Node>("{\"name\": \"test_a\"}");
- const Node *const node_a = &node_a_buffer.message();
+ public:
+ FlatbufferDetachedBuffer<Node> node_a_buffer =
+ JsonToFlatbuffer<Node>("{\"name\": \"test_a\"}");
+ const Node *const node_a = &node_a_buffer.message();
- FlatbufferDetachedBuffer<Node> node_b_buffer =
- JsonToFlatbuffer<Node>("{\"name\": \"test_b\"}");
- const Node *const node_b = &node_b_buffer.message();
+ FlatbufferDetachedBuffer<Node> node_b_buffer =
+ JsonToFlatbuffer<Node>("{\"name\": \"test_b\"}");
+ const Node *const node_b = &node_b_buffer.message();
};
using NoncausalTimestampFilterTest = FilterTest;
@@ -125,7 +125,6 @@
filter.Sample(tb, ob);
filter.Sample(tc, oc);
-
EXPECT_TRUE(filter.Observe());
EXPECT_EQ(*filter.Consume(), std::make_tuple(ta, oa));
@@ -145,7 +144,6 @@
filter.Sample(tb, ob);
filter.Sample(tc, oc);
-
filter.Pop(tb);
EXPECT_TRUE(filter.Observe());
EXPECT_EQ(*filter.Consume(), std::make_tuple(tb, ob));
@@ -656,9 +654,9 @@
ASSERT_EQ(filter.timestamps_size(), 2u);
filter.FreezeUntil(tc);
- EXPECT_DEATH(
- { filter.Sample(tb, ob); },
- "Tried to insert 0.100000000sec before 0.200000000sec, which is a frozen time");
+ EXPECT_DEATH({ filter.Sample(tb, ob); },
+ "Tried to insert 0.100000000sec before 0.200000000sec, which "
+ "is a frozen time");
}
{
@@ -672,9 +670,9 @@
ASSERT_EQ(filter.timestamps_size(), 3u);
filter.FreezeUntil(tb);
- EXPECT_DEATH(
- { filter.Sample(tb, oa); },
- "Tried to insert 0.100000000sec before 0.100000000sec, which is a frozen time");
+ EXPECT_DEATH({ filter.Sample(tb, oa); },
+ "Tried to insert 0.100000000sec before 0.100000000sec, which "
+ "is a frozen time");
EXPECT_DEATH({ filter.Sample(tb + chrono::nanoseconds(1), oa); },
"Can't pop an already frozen sample");
}
@@ -684,11 +682,11 @@
TEST_F(NoncausalTimestampFilterTest, InterpolateOffset) {
const monotonic_clock::time_point e = monotonic_clock::epoch();
- const monotonic_clock::time_point t1 = e + chrono::nanoseconds(0);
+ const monotonic_clock::time_point t1 = e + chrono::nanoseconds(10000);
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 monotonic_clock::time_point t2 = t1 + chrono::nanoseconds(1000);
const chrono::nanoseconds o2 = chrono::nanoseconds(150);
const double o2d = static_cast<double>(o2.count());
@@ -714,73 +712,128 @@
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
- e + chrono::nanoseconds(500)),
+ t1 + 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),
+ t1 + 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),
+ t1 + chrono::nanoseconds(500), 0.0),
25.);
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
- e + chrono::nanoseconds(-200)),
+ t1 + chrono::nanoseconds(-200)),
chrono::nanoseconds(90));
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, -200.),
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, -200.),
o1);
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, -200.),
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, -200.),
-10.);
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
- e + chrono::nanoseconds(200)),
+ t1 + chrono::nanoseconds(200)),
chrono::nanoseconds(110));
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 200.),
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 200.),
o1);
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 200.),
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 200.),
10.);
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
- e + chrono::nanoseconds(800)),
+ t1 + chrono::nanoseconds(800)),
chrono::nanoseconds(140));
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 800.),
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 800.),
o1);
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 800.),
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 800.),
40.);
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
std::make_tuple(t1, o1), std::make_tuple(t2, o2),
- e + chrono::nanoseconds(1200)),
+ t1 + chrono::nanoseconds(1200)),
chrono::nanoseconds(160));
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 1200.),
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 1200.),
o1);
EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffsetRemainder(
- std::make_tuple(t1, o1), std::make_tuple(t2, o2), e, 1200.),
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 1200.),
60.);
- EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
- std::make_tuple(t1, o1), e + chrono::nanoseconds(800)),
+ // Test extrapolation functions before t1 and after t2
+ EXPECT_EQ(
+ NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1), t1),
+ o1);
+
+ // With small offset (<1/kMaxVelocity() sec), low precision can't
+ // tell the difference since the delta is a fraction of a nanosecond
+ EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
+ std::make_tuple(t1, o1), t1 + chrono::nanoseconds(-800)),
o1);
- EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(std::make_tuple(t1, o1),
- e, 800.),
+
+ EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
+ std::make_tuple(t1, o1), e + chrono::nanoseconds(1000)),
+ o1 - chrono::nanoseconds(
+ int64_t((t1 - (e + chrono::nanoseconds(1000))).count() *
+ kMaxVelocity())));
+
+ EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
+ std::make_tuple(t1, o1), t1 + chrono::nanoseconds(-9000)),
+ o1 + chrono::nanoseconds(int64_t(
+ chrono::nanoseconds(-9000).count() * kMaxVelocity())));
+
+ // Test base + double version
+ EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1),
+ e, 0.),
o1);
- EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(
- std::make_tuple(t1, o1), e + chrono::nanoseconds(-300)),
+ EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
+ std::make_tuple(t1, o1), e, 0.),
+ -(t1 - e).count() * kMaxVelocity());
+
+ EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1),
+ t1, 0.),
o1);
- EXPECT_EQ(NoncausalTimestampFilter::InterpolateOffset(std::make_tuple(t1, o1),
- e, -300.),
+ EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
+ std::make_tuple(t1, o1), t1, 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());
+
+ EXPECT_EQ(
+ NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t2, o2), t2),
+ o2);
+
+ 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);
+
+ // Test points past our last sample
+ EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
+ std::make_tuple(t2, o2), t2 + chrono::nanoseconds(10000)),
+ 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());
}
// Tests that FindTimestamps finds timestamps in a sequence.
@@ -856,20 +909,22 @@
// and FindTimestamps correctly.
TEST_F(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
+ // 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);
+ const monotonic_clock::time_point t1 = e + chrono::nanoseconds(1000);
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 monotonic_clock::time_point t2 = e + chrono::microseconds(2000);
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 monotonic_clock::time_point t3 = e + chrono::microseconds(3000);
const chrono::nanoseconds o3 = chrono::nanoseconds(50);
const double o3d = static_cast<double>(o3.count());
+ const monotonic_clock::time_point t4 = e + chrono::microseconds(4000);
+
NoncausalTimestampFilter filter(node_a, node_b);
filter.Sample(t1, o1);
@@ -877,7 +932,17 @@
// 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));
+ // Check if we ask for something away from point that we get an offset
+ // based on the MaxVelocity allowed
+ const double offset_pre = -(t1 - e).count() * kMaxVelocity();
+ EXPECT_EQ(filter.Offset(e),
+ o1 + chrono::nanoseconds(static_cast<int64_t>(offset_pre)));
+ EXPECT_EQ(filter.Offset(e, 0.0), std::make_pair(o1, offset_pre));
+
+ double offset_post = -(t2 - t1).count() * kMaxVelocity();
+ EXPECT_EQ(filter.Offset(t2),
+ o1 + chrono::nanoseconds(static_cast<int64_t>(offset_post)));
+ EXPECT_EQ(filter.Offset(t2, 0.0), std::make_pair(o1, offset_post));
filter.Sample(t2, o2);
filter.Sample(t3, o3);
@@ -898,7 +963,18 @@
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));
+ EXPECT_EQ(filter.Offset(t3, 0.0), std::make_pair(o3, 0.0));
+
+ // Check that we still get same answer for times before our sample data...
+ EXPECT_EQ(filter.Offset(e),
+ o1 + chrono::nanoseconds(static_cast<int64_t>(offset_pre)));
+ EXPECT_EQ(filter.Offset(e, 0.0), std::make_pair(o1, offset_pre));
+ // ... and after
+ offset_post = -(t4 - t3).count() * kMaxVelocity();
+ EXPECT_EQ(
+ filter.Offset(t4).count(),
+ (o3 + chrono::nanoseconds(static_cast<int64_t>(offset_post))).count());
+ EXPECT_EQ(filter.Offset(t4, 0.0), std::make_pair(o3, offset_post));
}
// Tests that the cost function handles a single point line properly, and the
@@ -917,17 +993,33 @@
filter.Sample(t1, o1);
// Spot check some known points.
+ // NOTE that double arg must be 0 <= ta < 1. for Offset and OffsetError
+ // We can handle values outside that range for Cost functions
+ EXPECT_EQ(filter.Offset(t1, 0.0), std::make_pair(o1, 0.0));
+ EXPECT_EQ(filter.Offset(t1, 0.5), std::make_pair(o1, -0.5 * kMaxVelocity()));
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.OffsetError(t1, 0.5, t1 + o1, 0.5), 0.5 * kMaxVelocity());
+ EXPECT_EQ(filter.OffsetError(t1, 0.5, t1 + o1, 0.0),
+ -0.5 + 0.5 * kMaxVelocity());
+
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);
+ EXPECT_EQ(filter.Cost(t1, 1.0, t1 + o1, 1.0),
+ std::pow(-1.0 * kMaxVelocity(), 2.0));
+ EXPECT_EQ(filter.Cost(t1, 1.0, t1 + o1, 0.0),
+ std::pow(1.0 - kMaxVelocity(), 2.0));
+ EXPECT_EQ(filter.Cost(t1, -0.5, t1 + o1, 0.0),
+ std::pow(0.5 + 0.5 * kMaxVelocity(), 2.0));
+ EXPECT_EQ(filter.Cost(t1, 0.5, t1 + o1, 0.0),
+ std::pow(-0.5 + 0.5 * kMaxVelocity(), 2.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}) {
+ // Avoid offsets of 0.0, since this function isn't well behaved there
+ for (double ta_nominal : {-1000.0, 20.0, 1000.0}) {
+ for (double tb_nominal : {-2000.0, 20.0, 2000.0}) {
{
+ // Evaluate functions around (ta (or tb) + kDelta)
const double minus_costa =
filter.Cost(t1, ta_nominal - kDelta, t1 + o1, tb_nominal);
const double plus_costa =
@@ -974,7 +1066,7 @@
// 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(t1, 0.5, t1 + o1, 0.5), -0.00025);
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);
@@ -985,10 +1077,10 @@
// 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
+ // Note: don't test 0 delta 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}) {
+ for (double tb_nominal : {-4000.0, -2000.0, 20.0, 2000.0, 4000.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.
{