Merge "Set the slope to 0 before the start in NoncausalTimestampFilter"
diff --git a/aos/network/multinode_timestamp_filter_test.cc b/aos/network/multinode_timestamp_filter_test.cc
index 0244225..1c1ca54 100644
--- a/aos/network/multinode_timestamp_filter_test.cc
+++ b/aos/network/multinode_timestamp_filter_test.cc
@@ -233,9 +233,10 @@
mpq_class tb_mpq =
SolveExact(FitLine(a), FitLine(b), problem.base_clock(0));
- EXPECT_EQ(tb_mpq.get_d(), result[0])
+ EXPECT_NEAR(tb_mpq.get_d(), result[0], 1e-6)
<< std::setprecision(12) << std::fixed << " Expected "
- << tb_mpq.get_d() << " " << tb_mpq << " got " << result[0];
+ << tb_mpq.get_d() << " " << tb_mpq << " got " << result[0]
+ << " difference of " << tb_mpq.get_d() - result[0];
}
}
}
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index 14cb33c..c623cbe 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -599,7 +599,7 @@
chrono::nanoseconds NoncausalTimestampFilter::Offset(
monotonic_clock::time_point ta) const {
CHECK_GT(timestamps_size(), 0u);
- if (timestamps_size() == 1u) {
+ 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);
@@ -615,7 +615,7 @@
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) {
+ 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(
@@ -687,13 +687,13 @@
// 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);
+ if (timestamps_size() == 1u || ta_base < std::get<0>(timestamp(0))) {
+ return -2.0 * OffsetError(ta_base, ta, 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);
@@ -723,14 +723,14 @@
aos::monotonic_clock::time_point ta_base, double ta,
aos::monotonic_clock::time_point tb_base, double tb, size_t node_a,
size_t node_b) const {
- if (timestamps_size() == 1u) {
+ 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());
}
- 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);
@@ -775,6 +775,7 @@
// d/dtb OffsetError(ta, tb) = 1
//
// d cost/dtb => 2 * OffsetError(ta, tb)
+ NormalizeTimestamps(&ta_base, &ta);
return 2.0 * OffsetError(ta_base, ta, tb_base, tb);
}
@@ -782,14 +783,14 @@
aos::monotonic_clock::time_point ta_base, double ta,
aos::monotonic_clock::time_point tb_base, double tb, size_t node_a,
size_t node_b) const {
- if (timestamps_size() == 1u) {
+ 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());
}
- 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);
@@ -823,14 +824,14 @@
aos::monotonic_clock::time_point ta_base, double ta,
aos::monotonic_clock::time_point tb_base, double tb, size_t node_a,
size_t node_b) const {
- if (timestamps_size() == 1u) {
+ 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());
}
- 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);
@@ -864,7 +865,7 @@
aos::monotonic_clock::time_point ta,
aos::monotonic_clock::time_point tb) const {
CHECK_GT(timestamps_size(), 0u);
- if (timestamps_size() == 1u) {
+ 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.
const chrono::nanoseconds offset =