Add a pointer for caching to NoncausalTimestampFilter
This makes repeated lookups a lot cheaper since we can save which
timestamps we used last time, and can reuse them if the solution is
similar. This gives us a spot in the future to do a more clever binary
search. This all assumes that subsequent lookups are likely to be for
similar times.
Change-Id: Ic137a16eb98fa5b0a63fc140bd20aae855eaef8b
Signed-off-by: Austin Schuh <austin.schuh@bluerivertech.com>
diff --git a/aos/network/multinode_timestamp_filter.cc b/aos/network/multinode_timestamp_filter.cc
index 5c35412..87b9a81 100644
--- a/aos/network/multinode_timestamp_filter.cc
+++ b/aos/network/multinode_timestamp_filter.cc
@@ -73,8 +73,8 @@
clock_offset_filter_for_node_[node_a]) {
// There's something in this direction, so we don't need to check the
// opposite direction to confirm we have observations.
- if (!filter.filter->timestamps_empty(
- base_clock_[node_a].boot, base_clock_[filter.b_index].boot)) {
+ if (!filter.filter->timestamps_empty(base_clock_[node_a].boot,
+ base_clock_[filter.b_index].boot)) {
continue;
}
@@ -97,8 +97,8 @@
for (const struct FilterPair &filter : clock_offset_filter_for_node_[i]) {
// There's nothing in this direction, so there will be nothing to
// validate.
- if (filter.filter->timestamps_empty(
- base_clock_[i].boot, base_clock_[filter.b_index].boot)) {
+ if (filter.filter->timestamps_empty(base_clock_[i].boot,
+ base_clock_[filter.b_index].boot)) {
// For a boot to exist, we need to have some observations between it and
// another boot. We wouldn't bother to build a problem to solve for
// this node otherwise. Confirm that is true so we at least get
@@ -113,9 +113,9 @@
continue;
}
const bool iteration = filter.filter->ValidateSolution(
- solution[i], solution[filter.b_index]);
+ filter.pointer, solution[i], solution[filter.b_index]);
if (!iteration) {
- filter.filter->ValidateSolution(solution[i], 0.0,
+ filter.filter->ValidateSolution(filter.pointer, solution[i], 0.0,
solution[filter.b_index], 0.0);
}
@@ -126,10 +126,10 @@
}
Eigen::VectorXd TimestampProblem::Gradient(
- const Eigen::Ref<Eigen::VectorXd> time_offsets) const {
+ const Eigen::Ref<Eigen::VectorXd> time_offsets) {
Eigen::VectorXd grad = Eigen::VectorXd::Zero(live_nodes_);
for (size_t i = 0; i < clock_offset_filter_for_node_.size(); ++i) {
- for (const struct FilterPair &filter : clock_offset_filter_for_node_[i]) {
+ for (struct FilterPair &filter : clock_offset_filter_for_node_[i]) {
// Especially when reboots are involved, it isn't guarenteed that there
// will be timestamps going both ways. In this case, we want to avoid the
// cost.
@@ -162,12 +162,12 @@
// extra factor, the solution will be the same (or close enough).
constexpr double kMinNetworkDelay = 2.0;
- const double error =
- 2.0 * (filter.filter->OffsetError(base_clock_[i],
- time_offsets(a_solution_index),
- base_clock_[filter.b_index],
- time_offsets(b_solution_index)) -
- kMinNetworkDelay);
+ const std::pair<NoncausalTimestampFilter::Pointer, double> offset_error =
+ filter.filter->OffsetError(
+ filter.pointer, base_clock_[i], time_offsets(a_solution_index),
+ base_clock_[filter.b_index], time_offsets(b_solution_index));
+ const double error = 2.0 * (offset_error.second - kMinNetworkDelay);
+ filter.pointer = offset_error.first;
grad(a_solution_index) += -error;
grad(b_solution_index) += error;
@@ -202,7 +202,7 @@
std::tuple<Eigen::VectorXd, size_t> TimestampProblem::Newton(
const Eigen::Ref<Eigen::VectorXd> time_offsets,
- const std::vector<logger::BootTimestamp> &points) const {
+ const std::vector<logger::BootTimestamp> &points) {
CHECK_GT(live_nodes_, 0u) << ": No live nodes to solve for.";
// TODO(austin): Each of the DCost functions does a binary search of the
// timestamps list. By the time we have computed the gradient and Hessian,
@@ -454,12 +454,12 @@
// report.
gradients[i].emplace_back(
std::string("- ") +
- filter.filter->DebugOffsetError(base_clock_[i], 0.0,
+ filter.filter->DebugOffsetError(filter.pointer, base_clock_[i], 0.0,
base_clock_[filter.b_index], 0.0, i,
filter.b_index));
gradients[filter.b_index].emplace_back(filter.filter->DebugOffsetError(
- base_clock_[i], 0.0, base_clock_[filter.b_index], 0.0, i,
- filter.b_index));
+ filter.pointer, base_clock_[i], 0.0, base_clock_[filter.b_index],
+ 0.0, i, filter.b_index));
}
}
}
diff --git a/aos/network/multinode_timestamp_filter.h b/aos/network/multinode_timestamp_filter.h
index 77d5226..ea52b23 100644
--- a/aos/network/multinode_timestamp_filter.h
+++ b/aos/network/multinode_timestamp_filter.h
@@ -104,8 +104,7 @@
// Returns the Hessian of the cost function at time_offsets.
Eigen::MatrixXd Hessian(const Eigen::Ref<Eigen::VectorXd> time_offsets) const;
// Returns the gradient of the cost function at time_offsets.
- Eigen::VectorXd Gradient(
- const Eigen::Ref<Eigen::VectorXd> time_offsets) const;
+ Eigen::VectorXd Gradient(const Eigen::Ref<Eigen::VectorXd> time_offsets);
// Returns the newton step of the timestamp problem, and the node which was
// used for the equality constraint. The last term is the scalar on the
@@ -113,7 +112,7 @@
// actual newton step.
std::tuple<Eigen::VectorXd, size_t> Newton(
const Eigen::Ref<Eigen::VectorXd> time_offsets,
- const std::vector<logger::BootTimestamp> &points) const;
+ const std::vector<logger::BootTimestamp> &points);
void MaybeUpdateNodeMapping();
@@ -147,6 +146,8 @@
const size_t b_index;
const NoncausalTimestampFilter *const b_filter;
+
+ NoncausalTimestampFilter::Pointer pointer;
};
// List of filters indexed by node.
diff --git a/aos/network/multinode_timestamp_filter_test.cc b/aos/network/multinode_timestamp_filter_test.cc
index d82135b..49f20da 100644
--- a/aos/network/multinode_timestamp_filter_test.cc
+++ b/aos/network/multinode_timestamp_filter_test.cc
@@ -390,11 +390,15 @@
// Confirm that the error is almost equal for both directions. The solution
// is an integer solution, so there will be a little bit of error left over.
- EXPECT_NEAR(a.OffsetError(std::get<0>(result1)[0], 0.0,
- std::get<0>(result1)[1], 0.0) -
- b.OffsetError(std::get<0>(result1)[1], 0.0,
- std::get<0>(result1)[0], 0.0),
- 0.0, 0.5);
+ EXPECT_NEAR(
+ a.OffsetError(NoncausalTimestampFilter::Pointer(),
+ std::get<0>(result1)[0], 0.0, std::get<0>(result1)[1], 0.0)
+ .second -
+ b.OffsetError(NoncausalTimestampFilter::Pointer(),
+ std::get<0>(result1)[1], 0.0, std::get<0>(result1)[0],
+ 0.0)
+ .second,
+ 0.0, 0.5);
}
} // namespace testing
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index b217fd8..1a7bc6d 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -16,6 +16,7 @@
namespace chrono = std::chrono;
using logger::BootDuration;
using logger::BootTimestamp;
+using Pointer = NoncausalTimestampFilter::Pointer;
std::string TimeString(const aos::monotonic_clock::time_point t,
std::chrono::nanoseconds o) {
@@ -487,37 +488,90 @@
return std::make_tuple(std::get<0>(t), std::get<1>(t));
}
-std::pair<std::tuple<BootTimestamp, BootDuration>,
- std::tuple<BootTimestamp, BootDuration>>
-NoncausalTimestampFilter::FindTimestamps(BootTimestamp ta_base, double ta,
- size_t sample_boot) const {
+std::pair<Pointer, std::pair<std::tuple<BootTimestamp, BootDuration>,
+ std::tuple<BootTimestamp, BootDuration>>>
+NoncausalTimestampFilter::FindTimestamps(Pointer pointer, BootTimestamp ta_base,
+ double ta, size_t sample_boot) 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, sample_boot);
+ return FindTimestamps(pointer, ta_base, sample_boot);
}
-std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
- std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
+std::pair<
+ Pointer,
+ std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>>
NoncausalTimestampFilter::SingleFilter::FindTimestamps(
- monotonic_clock::time_point ta_base, double ta) const {
+ Pointer pointer, 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);
+ return FindTimestamps(pointer, ta_base);
}
-std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
- std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
+std::pair<
+ Pointer,
+ std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>>
NoncausalTimestampFilter::SingleFilter::FindTimestamps(
- monotonic_clock::time_point ta) const {
+ Pointer pointer, monotonic_clock::time_point ta) const {
CHECK_GT(timestamps_size(), 1u);
+
+ // boot_filter_ is non-null when the rest of the contents are valid. Make
+ // sure it's pointing to this filter, and the pointer is in bounds.
+ if (pointer.boot_filter_ != nullptr &&
+ &pointer.boot_filter_->filter == this &&
+ pointer.index_ + 1 != timestamps_size()) {
+ CHECK_LT(pointer.index_ + 1, timestamps_size()) << " " << this;
+ // Confirm that the cached timestamps haven't changed so we can trust the
+ // results.
+ //
+ // TODO(austin): Should this be a DCHECK when we are happier? This is a
+ // constraint on the user's behavior we are enforcing.
+ CHECK(timestamp(pointer.index_) == pointer.t0_)
+ << ": " << this << " boot_filter " << pointer.boot_filter_ << " got "
+ << std::get<0>(timestamp(pointer.index_)) << ", "
+ << std::get<1>(timestamp(pointer.index_)).count()
+ << "ns, expected index " << pointer.index_ << ", "
+ << std::get<0>(pointer.t0_) << ", " << std::get<1>(pointer.t0_).count()
+ << "ns";
+ // Last point has been filtered out.
+ if (pointer.t0_ != pointer.t1_) {
+ // If t0 and t1 match, this was a "before the start" point. We can still
+ // check it against the first segment, but we know it won't match so don't
+ // enforce the CHECK that the cache matches.
+ CHECK(timestamp(pointer.index_ + 1) == pointer.t1_)
+ << ": " << this << " boot_filter " << pointer.boot_filter_
+ << " index " << pointer.index_ << ", size " << timestamps_size()
+ << ", got " << std::get<0>(timestamp(pointer.index_ + 1)) << ", "
+ << std::get<1>(timestamp(pointer.index_ + 1)).count()
+ << "ns, expected index " << pointer.index_ << ", "
+ << std::get<0>(pointer.t1_) << ", "
+ << std::get<1>(pointer.t1_).count() << "ns";
+ }
+
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds> t0 =
+ timestamp(pointer.index_);
+ if (ta >= std::get<0>(t0)) {
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds> t1 =
+ timestamp(pointer.index_ + 1);
+ if (ta < std::get<0>(t1)) {
+ return std::make_pair(pointer, std::make_pair(t0, t1));
+ }
+ }
+ }
+
+ // Otherwise, do a log(n) search from the starting point. We should be close.
+ // TODO(austin): We should be able to do a better search here if we've got a
+ // previous pointer. Searches tend to be close to the previous search.
+
auto it = std::upper_bound(
timestamps_.begin() + 1, timestamps_.end() - 1, ta,
[](monotonic_clock::time_point ta,
@@ -526,7 +580,58 @@
const size_t index = std::distance(timestamps_.begin(), it);
- return std::make_pair(timestamp(index - 1), timestamp(index));
+ pointer.index_ = index - 1;
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> t0 =
+ pointer.t0_ = timestamp(index - 1);
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> t1 =
+ pointer.t1_ = timestamp(index);
+
+ return std::make_pair(pointer, std::make_pair(t0, t1));
+}
+
+std::pair<Pointer, std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
+NoncausalTimestampFilter::SingleFilter::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);
+
+ // TODO(austin): Return more here. Whether we are the first or last, and the
+ // corresponding index.
+ if (ta_base >= std::get<0>(timestamp(timestamps_size() - 1u))) {
+ reference_timestamp = timestamp(timestamps_size() - 1u);
+ return std::make_pair(Pointer(nullptr, timestamps_size() - 1u,
+ reference_timestamp, reference_timestamp),
+ reference_timestamp);
+ } else {
+ return std::make_pair(
+ Pointer(nullptr, 0, reference_timestamp, reference_timestamp),
+ reference_timestamp);
+ }
+}
+
+bool NoncausalTimestampFilter::SingleFilter::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::SingleFilter::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;
}
chrono::nanoseconds NoncausalTimestampFilter::ExtrapolateOffset(
@@ -659,117 +764,98 @@
}
}
-bool NoncausalTimestampFilter::SingleFilter::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::SingleFilter::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::SingleFilter::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::SingleFilter::Offset(
- monotonic_clock::time_point ta) const {
+std::pair<Pointer, chrono::nanoseconds>
+NoncausalTimestampFilter::SingleFilter::Offset(
+ Pointer pointer, monotonic_clock::time_point ta) const {
CHECK_GT(timestamps_size(), 0u);
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);
+ return std::make_pair(reference_timestamp.first,
+ NoncausalTimestampFilter::ExtrapolateOffset(
+ reference_timestamp.second, 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<
+ Pointer,
+ std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>>
+ points = FindTimestamps(pointer, ta);
+ return std::make_pair(points.first,
+ NoncausalTimestampFilter::InterpolateOffset(
+ points.second.first, points.second.second, ta));
}
-std::pair<chrono::nanoseconds, double>
+std::pair<Pointer, std::pair<chrono::nanoseconds, double>>
NoncausalTimestampFilter::SingleFilter::Offset(
- monotonic_clock::time_point ta_base, double ta) const {
+ Pointer pointer, monotonic_clock::time_point ta_base, double ta) const {
CHECK_GT(timestamps_size(), 0u) << node_names_;
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<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)));
}
- std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
- std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
- points = FindTimestamps(ta_base, ta);
+ std::pair<
+ Pointer,
+ std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>>
+ points = FindTimestamps(pointer, ta_base, ta);
+ CHECK_LT(std::get<0>(points.second.first), std::get<0>(points.second.second));
// 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));
+ 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)));
}
-double NoncausalTimestampFilter::SingleFilter::OffsetError(
- aos::monotonic_clock::time_point ta_base, double ta,
+std::pair<Pointer, double> NoncausalTimestampFilter::SingleFilter::OffsetError(
+ Pointer pointer, 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);
+ const std::pair<Pointer, std::pair<std::chrono::nanoseconds, double>> offset =
+ Offset(pointer, 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);
+ return std::make_pair(
+ offset.first,
+ static_cast<double>(((tb_base - ta_base) - offset.second.first).count()) +
+ ((tb - ta) - offset.second.second));
}
-std::string NoncausalTimestampFilter::DebugOffsetError(BootTimestamp ta_base,
- double ta,
- BootTimestamp tb_base,
- double tb, size_t node_a,
- size_t node_b) const {
+std::string NoncausalTimestampFilter::DebugOffsetError(
+ Pointer pointer, BootTimestamp ta_base, double ta, BootTimestamp tb_base,
+ double tb, size_t node_a, size_t node_b) const {
NormalizeTimestamps(&ta_base, &ta);
NormalizeTimestamps(&tb_base, &tb);
- const SingleFilter *f = maybe_filter(ta_base.boot, tb_base.boot);
- if (f == nullptr || f->timestamps_size() == 0u) {
+ const BootFilter *f = maybe_filter(pointer, ta_base.boot, tb_base.boot);
+ if (f == nullptr || f->filter.timestamps_size() == 0u) {
return "0";
}
- if (f->IsOutsideSamples(ta_base.time, ta)) {
- auto reference_timestamp = f->GetReferenceTimestamp(ta_base.time, ta);
+ if (f->filter.IsOutsideSamples(ta_base.time, ta)) {
+ auto reference_timestamp =
+ f->filter.GetReferenceTimestamp(ta_base.time, ta);
double slope = kMaxVelocity();
std::string note = "_";
- if (f->IsAfterSamples(ta_base.time, ta)) {
+ if (f->filter.IsAfterSamples(ta_base.time, ta)) {
slope = -kMaxVelocity();
note = "^";
}
@@ -777,13 +863,13 @@
// 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::get<0>(reference_timestamp.second).time_since_epoch().count(),
+ slope, std::get<1>(reference_timestamp.second).count(), note);
}
std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
- points = f->FindTimestamps(ta_base.time, ta);
+ points = f->filter.FindTimestamps(pointer, ta_base.time, ta).second;
// As a reminder, our cost function is essentially:
// ((tb - ta - (ma ta + ba))^2
@@ -816,7 +902,7 @@
}
bool NoncausalTimestampFilter::SingleFilter::ValidateSolution(
- aos::monotonic_clock::time_point ta_base, double ta,
+ Pointer pointer, 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);
@@ -833,11 +919,11 @@
// Special case size = 1 or ta before first timestamp, so we extrapolate
const chrono::nanoseconds offset_base =
- NoncausalTimestampFilter::ExtrapolateOffset(reference_timestamp,
+ NoncausalTimestampFilter::ExtrapolateOffset(reference_timestamp.second,
ta_base, ta);
const double offset_remainder =
NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
- reference_timestamp, ta_base, ta);
+ reference_timestamp.second, ta_base, ta);
// We want to do offset + ta > tb, but we need to do it with minimal
// numerical precision problems.
@@ -855,28 +941,30 @@
return true;
}
- std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
- std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
- points = FindTimestamps(ta_base, ta);
+ std::pair<
+ Pointer,
+ std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>>
+ points = FindTimestamps(pointer, ta_base, ta);
const chrono::nanoseconds offset_base =
- NoncausalTimestampFilter::InterpolateOffset(points.first, points.second,
- ta_base, ta);
+ NoncausalTimestampFilter::InterpolateOffset(
+ points.second.first, points.second.second, ta_base, ta);
const double offset = NoncausalTimestampFilter::InterpolateOffsetRemainder(
- points.first, points.second, ta_base, ta);
+ points.second.first, points.second.second, ta_base, ta);
if (static_cast<double>((offset_base + ta_base - tb_base).count()) >=
tb - offset - ta) {
LOG(ERROR) << node_names_ << " "
<< TimeString(ta_base, ta, offset_base, offset)
<< " > solution time " << tb_base << ", " << tb;
- LOG(ERROR) << "Bracketing times are " << TimeString(points.first) << " and "
- << TimeString(points.second);
+ LOG(ERROR) << "Bracketing times are " << TimeString(points.second.first)
+ << " and " << TimeString(points.second.second);
return false;
}
return true;
}
bool NoncausalTimestampFilter::SingleFilter::ValidateSolution(
- aos::monotonic_clock::time_point ta,
+ Pointer pointer, aos::monotonic_clock::time_point ta,
aos::monotonic_clock::time_point tb) const {
CHECK_GT(timestamps_size(), 0u);
if (ta < std::get<0>(timestamp(0)) && has_popped_) {
@@ -891,7 +979,8 @@
// Special case size = 1 or ta before first timestamp, so we extrapolate
const chrono::nanoseconds offset =
- NoncausalTimestampFilter::ExtrapolateOffset(reference_timestamp, ta);
+ NoncausalTimestampFilter::ExtrapolateOffset(reference_timestamp.second,
+ ta);
if (offset + ta >= tb) {
LOG(ERROR) << node_names_ << " " << TimeString(ta, offset)
<< " > solution time " << tb;
@@ -900,17 +989,19 @@
return true;
}
- std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
- std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
- points = FindTimestamps(ta);
+ std::pair<
+ Pointer,
+ std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>>
+ points = FindTimestamps(pointer, ta);
const chrono::nanoseconds offset =
- NoncausalTimestampFilter::InterpolateOffset(points.first, points.second,
- ta);
+ NoncausalTimestampFilter::InterpolateOffset(points.second.first,
+ points.second.second, ta);
if (offset + ta >= tb) {
LOG(ERROR) << node_names_ << " " << TimeString(ta, offset)
<< " > solution time " << tb;
- LOG(ERROR) << "Bracketing times are " << TimeString(points.first) << " and "
- << TimeString(points.second);
+ LOG(ERROR) << "Bracketing times are " << TimeString(points.second.first)
+ << " and " << TimeString(points.second.second);
return false;
}
return true;
@@ -919,7 +1010,7 @@
void NoncausalTimestampFilter::Sample(BootTimestamp monotonic_now_all,
BootDuration sample_ns) {
filter(monotonic_now_all.boot, sample_ns.boot)
- ->Sample(monotonic_now_all.time, sample_ns.duration);
+ ->filter.Sample(monotonic_now_all.time, sample_ns.duration);
}
void NoncausalTimestampFilter::SingleFilter::Sample(
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index 3b966c2..d85154e 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -243,6 +243,9 @@
// double portion. All the functions are subtracting the large times in integer
// precision and handling the remainder with double precision.
class NoncausalTimestampFilter {
+ private:
+ struct BootFilter;
+
public:
NoncausalTimestampFilter(const Node *node_a, const Node *node_b)
: node_a_(node_a), node_b_(node_b) {}
@@ -268,27 +271,71 @@
delete;
~NoncausalTimestampFilter();
+ // A class used to hold all the state information required for identifying a
+ // point and caching decisions.
+ class Pointer {
+ public:
+ Pointer() = default;
+
+ Pointer(
+ const BootFilter *boot_filter, size_t index,
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> t0,
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> t1)
+ : boot_filter_(boot_filter), index_(index), t0_(t0), t1_(t1) {}
+
+ // For testing.
+ bool operator==(const Pointer &other) const {
+ return other.boot_filter_ == boot_filter_ && other.index_ == index_ &&
+ other.t0_ == t0_ && other.t1_ == t1_;
+ }
+
+ private:
+ friend class NoncausalTimestampFilter;
+
+ // The filter which this timestamp came from.
+ const BootFilter *boot_filter_ = nullptr;
+ // The index for t0 into the timestamps list.
+ size_t index_ = 0;
+ // The two bounding timestamps. They will be equal if time last querried
+ // time was outside the extents of our timestamps. These are mostly used to
+ // make sure the underlying filter didn't change between when the pointer is
+ // created, and re-looked-up.
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> t0_ =
+ std::make_tuple(monotonic_clock::min_time, std::chrono::nanoseconds(0));
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> t1_ =
+ std::make_tuple(monotonic_clock::min_time, std::chrono::nanoseconds(0));
+ };
+
// Returns the error between the offset in the provided timestamps, and the
- // offset at ta.
- double OffsetError(logger::BootTimestamp ta_base, double ta,
- logger::BootTimestamp tb_base, double tb) const {
- return filter(ta_base.boot, tb_base.boot)
- ->OffsetError(ta_base.time, ta, tb_base.time, tb);
+ // offset at ta. Also returns a pointer to the timestamps used for the
+ // lookup to be passed back in again for a more efficient second lookup.
+ std::pair<Pointer, double> OffsetError(Pointer pointer,
+ logger::BootTimestamp ta_base,
+ double ta,
+ logger::BootTimestamp tb_base,
+ double tb) const {
+ const BootFilter *boot_filter = filter(pointer, ta_base.boot, tb_base.boot);
+ std::pair<Pointer, double> result = boot_filter->filter.OffsetError(
+ pointer, ta_base.time, ta, tb_base.time, tb);
+ result.first.boot_filter_ = boot_filter;
+ return result;
}
// Returns the string representation of 2 * OffsetError(ta, tb)
- std::string DebugOffsetError(logger::BootTimestamp ta_base, double ta,
- logger::BootTimestamp tb_base, double tb,
- size_t node_a, size_t node_b) const;
+ std::string DebugOffsetError(Pointer pointer, logger::BootTimestamp ta_base,
+ double ta, logger::BootTimestamp tb_base,
+ double tb, size_t node_a, size_t node_b) const;
// Confirms that the solution meets the constraints. Returns true on success.
- bool ValidateSolution(logger::BootTimestamp ta,
+ bool ValidateSolution(Pointer pointer, logger::BootTimestamp ta,
logger::BootTimestamp tb) const {
- return filter(ta.boot, tb.boot)->ValidateSolution(ta.time, tb.time);
+ return filter(pointer, ta.boot, tb.boot)
+ ->filter.ValidateSolution(pointer, ta.time, tb.time);
}
- bool ValidateSolution(logger::BootTimestamp ta_base, double ta,
- logger::BootTimestamp tb_base, double tb) const {
- return filter(ta_base.boot, tb_base.boot)
- ->ValidateSolution(ta_base.time, ta, tb_base.time, tb);
+ bool ValidateSolution(Pointer pointer, logger::BootTimestamp ta_base,
+ double ta, logger::BootTimestamp tb_base,
+ double tb) const {
+ return filter(pointer, ta_base.boot, tb_base.boot)
+ ->filter.ValidateSolution(pointer, ta_base.time, ta, tb_base.time, tb);
}
// Adds a new sample to our filtered timestamp list.
@@ -310,21 +357,21 @@
// Returns the number of timestamps for a specific boot. This is useful to
// determine if there are observations in this direction or not.
size_t timestamps_size(const size_t boota, const size_t bootb) const {
- const SingleFilter *f = maybe_filter(boota, bootb);
+ const BootFilter *f = maybe_filter(boota, bootb);
if (f == nullptr) {
return 0u;
} else {
- return f->timestamps_size();
+ return f->filter.timestamps_size();
}
}
// Returns true if there are no timestamps available for the provide boots.
bool timestamps_empty(const size_t boota, const size_t bootb) const {
- const SingleFilter *f = maybe_filter(boota, bootb);
+ const BootFilter *f = maybe_filter(boota, bootb);
if (f == nullptr) {
return true;
} else {
- return f->timestamps_empty();
+ return f->filter.timestamps_empty();
}
}
@@ -343,9 +390,9 @@
logger::BootTimestamp remote_monotonic_now) {
// TODO(austin): CHECK that all older boots are fully frozen.
filter(node_monotonic_now.boot, remote_monotonic_now.boot)
- ->FreezeUntil(node_monotonic_now.time);
+ ->filter.FreezeUntil(node_monotonic_now.time);
filter(node_monotonic_now.boot, remote_monotonic_now.boot)
- ->FreezeUntilRemote(remote_monotonic_now.time);
+ ->filter.FreezeUntilRemote(remote_monotonic_now.time);
}
// Returns true if there is a full line which hasn't been observed.
@@ -431,39 +478,58 @@
// Public for testing.
// Returns the offset for the point in time, using the timestamps in the deque
// to form a polyline used to interpolate.
- logger::BootDuration Offset(logger::BootTimestamp ta,
+ logger::BootDuration Offset(Pointer pointer, logger::BootTimestamp ta,
size_t sample_boot) const {
- return {sample_boot, filter(ta.boot, sample_boot)->Offset(ta.time)};
+ return {
+ sample_boot,
+ filter(ta.boot, sample_boot)->filter.Offset(pointer, ta.time).second};
}
- std::pair<logger::BootDuration, double> Offset(logger::BootTimestamp ta_base,
+ std::pair<logger::BootDuration, double> Offset(Pointer pointer,
+ logger::BootTimestamp ta_base,
double ta,
size_t sample_boot) const {
- std::pair<std::chrono::nanoseconds, double> result =
- filter(ta_base.boot, sample_boot)->Offset(ta_base.time, ta);
- return std::make_pair(logger::BootDuration{sample_boot, result.first},
- result.second);
+ std::pair<Pointer, std::pair<std::chrono::nanoseconds, double>> result =
+ filter(ta_base.boot, sample_boot)
+ ->filter.Offset(pointer, ta_base.time, ta);
+ return std::make_pair(
+ logger::BootDuration{sample_boot, result.second.first},
+ result.second.second);
}
// Assuming that there are at least 2 points in timestamps_, finds the 2
// matching points.
- std::pair<std::tuple<logger::BootTimestamp, logger::BootDuration>,
- std::tuple<logger::BootTimestamp, logger::BootDuration>>
- FindTimestamps(logger::BootTimestamp ta, size_t sample_boot) const {
- std::pair<std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>,
- std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>>
- result = filter(ta.boot, sample_boot)->FindTimestamps(ta.time);
+ std::pair<Pointer,
+ std::pair<std::tuple<logger::BootTimestamp, logger::BootDuration>,
+ std::tuple<logger::BootTimestamp, logger::BootDuration>>>
+ FindTimestamps(Pointer pointer, logger::BootTimestamp ta,
+ size_t sample_boot) const {
+ const BootFilter *boot_filter = filter(ta.boot, sample_boot);
+ std::pair<
+ Pointer,
+ std::pair<
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>>>
+ result = boot_filter->filter.FindTimestamps(pointer, ta.time);
+ result.first.boot_filter_ = boot_filter;
return std::make_pair(
- std::make_tuple(
- logger::BootTimestamp{ta.boot, std::get<0>(result.first)},
- logger::BootDuration{sample_boot, std::get<1>(result.first)}),
- std::make_tuple(
- logger::BootTimestamp{ta.boot, std::get<0>(result.second)},
- logger::BootDuration{sample_boot, std::get<1>(result.second)}));
+ result.first,
+ std::make_pair(
+ std::make_tuple(
+ logger::BootTimestamp{ta.boot,
+ std::get<0>(result.second.first)},
+ logger::BootDuration{sample_boot,
+ std::get<1>(result.second.first)}),
+ std::make_tuple(
+ logger::BootTimestamp{ta.boot,
+ std::get<0>(result.second.second)},
+ logger::BootDuration{sample_boot,
+ std::get<1>(result.second.second)})));
}
- std::pair<std::tuple<logger::BootTimestamp, logger::BootDuration>,
- std::tuple<logger::BootTimestamp, logger::BootDuration>>
- FindTimestamps(logger::BootTimestamp ta_base, double ta,
+ std::pair<Pointer,
+ std::pair<std::tuple<logger::BootTimestamp, logger::BootDuration>,
+ std::tuple<logger::BootTimestamp, logger::BootDuration>>>
+ FindTimestamps(Pointer pointer, logger::BootTimestamp ta_base, double ta,
size_t sample_boot) const;
static std::chrono::nanoseconds InterpolateOffset(
@@ -514,26 +580,36 @@
SingleFilter operator=(const SingleFilter &) = delete;
~SingleFilter();
- 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;
+ std::pair<
+ Pointer,
+ std::pair<
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>>>
+ FindTimestamps(Pointer pointer, monotonic_clock::time_point ta) const;
+ std::pair<
+ Pointer,
+ std::pair<
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>>>
+ FindTimestamps(Pointer pointer, monotonic_clock::time_point ta_base,
+ double ta) const;
// 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>
+ std::pair<Pointer,
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>>
GetReferenceTimestamp(monotonic_clock::time_point ta_base, double ta) const;
- 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;
- double OffsetError(aos::monotonic_clock::time_point ta_base, double ta,
- aos::monotonic_clock::time_point tb_base,
- double tb) const;
+ std::pair<Pointer, std::chrono::nanoseconds> Offset(
+ Pointer pointer, monotonic_clock::time_point ta) const;
+ std::pair<Pointer, std::pair<std::chrono::nanoseconds, double>> Offset(
+ Pointer pointer, monotonic_clock::time_point ta_base, double ta) const;
+ std::pair<Pointer, double> OffsetError(
+ Pointer pointer, aos::monotonic_clock::time_point ta_base, double ta,
+ aos::monotonic_clock::time_point tb_base, double tb) const;
+
bool has_unobserved_line() const;
monotonic_clock::time_point unobserved_line_end() const;
monotonic_clock::time_point unobserved_line_remote_end() const;
@@ -599,9 +675,10 @@
}
// Confirms that the solution meets the constraints. Returns true on
// success.
- bool ValidateSolution(aos::monotonic_clock::time_point ta,
+ bool ValidateSolution(Pointer pointer, aos::monotonic_clock::time_point ta,
aos::monotonic_clock::time_point tb) const;
- bool ValidateSolution(aos::monotonic_clock::time_point ta_base, double ta,
+ bool ValidateSolution(Pointer pointer,
+ aos::monotonic_clock::time_point ta_base, double ta,
aos::monotonic_clock::time_point tb_base,
double tb) const;
@@ -663,12 +740,12 @@
}
protected:
- SingleFilter *filter(int boota, int bootb) {
+ BootFilter *filter(int boota, int bootb) {
auto it =
std::lower_bound(filters_.begin(), filters_.end(),
std::make_pair(boota, bootb), FilterLessThanLower);
if (it != filters_.end() && (*it)->boot == std::make_pair(boota, bootb)) {
- return &(*it)->filter;
+ return it->get();
}
if (!filters_.empty() && current_filter_ >= 0) {
@@ -676,15 +753,14 @@
CHECK_GE(boota, filters_[current_filter_]->boot.first);
CHECK_GE(bootb, filters_[current_filter_]->boot.second) << NodeNames();
}
- SingleFilter *result =
- &filters_
- .emplace(std::upper_bound(filters_.begin(), filters_.end(),
- std::make_pair(boota, bootb),
- FilterLessThanUpper),
- std::make_unique<BootFilter>(std::make_pair(boota, bootb),
- NodeNames()))
- ->get()
- ->filter;
+ BootFilter *result =
+ filters_
+ .emplace(std::upper_bound(filters_.begin(), filters_.end(),
+ std::make_pair(boota, bootb),
+ FilterLessThanUpper),
+ std::make_unique<BootFilter>(std::make_pair(boota, bootb),
+ NodeNames()))
+ ->get();
{
// Confirm we don't have boots go backwards.
@@ -705,7 +781,7 @@
return result;
}
- const SingleFilter *maybe_filter(int boota, int bootb) const {
+ const BootFilter *maybe_filter(int boota, int bootb) const {
auto it =
std::lower_bound(filters_.begin(), filters_.end(),
std::make_pair(boota, bootb), FilterLessThanLower);
@@ -713,14 +789,31 @@
return nullptr;
}
if (it->get()->boot == std::make_pair(boota, bootb)) {
- return &it->get()->filter;
+ return it->get();
} else {
return nullptr;
}
}
- const SingleFilter *filter(int boota, int bootb) const {
- const SingleFilter *result = maybe_filter(boota, bootb);
+ const BootFilter *maybe_filter(Pointer pointer, int boota, int bootb) const {
+ if (pointer.boot_filter_ != nullptr &&
+ pointer.boot_filter_->boot.first == static_cast<int>(boota) &&
+ pointer.boot_filter_->boot.second == static_cast<int>(bootb)) {
+ return pointer.boot_filter_;
+ }
+
+ return maybe_filter(boota, bootb);
+ }
+
+ const BootFilter *filter(int boota, int bootb) const {
+ const BootFilter *result = maybe_filter(boota, bootb);
+ CHECK(result != nullptr)
+ << NodeNames() << " Failed to find " << boota << ", " << bootb;
+ return result;
+ }
+
+ const BootFilter *filter(Pointer pointer, int boota, int bootb) const {
+ const BootFilter *result = maybe_filter(pointer, boota, bootb);
CHECK(result != nullptr)
<< NodeNames() << " Failed to find " << boota << ", " << bootb;
return result;
diff --git a/aos/network/timestamp_filter_test.cc b/aos/network/timestamp_filter_test.cc
index 5e2a68b..bbfd437 100644
--- a/aos/network/timestamp_filter_test.cc
+++ b/aos/network/timestamp_filter_test.cc
@@ -16,20 +16,21 @@
using aos::monotonic_clock;
using logger::BootDuration;
using logger::BootTimestamp;
+using Pointer = NoncausalTimestampFilter::Pointer;
class TestingNoncausalTimestampFilter : public NoncausalTimestampFilter {
public:
TestingNoncausalTimestampFilter(const Node *node_a, const Node *node_b)
: NoncausalTimestampFilter(node_a, node_b) {}
- bool frozen(size_t index) const { return filter(0, 0)->frozen(index); }
+ bool frozen(size_t index) const { return filter(0, 0)->filter.frozen(index); }
bool frozen(logger::BootTimestamp t) const {
- return filter(t.boot, 0)->frozen(t.time);
+ return filter(t.boot, 0)->filter.frozen(t.time);
}
std::tuple<BootTimestamp, BootDuration> timestamp(size_t i) const {
std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>
- result = filter(0, 0)->timestamp(i);
+ result = filter(0, 0)->filter.timestamp(i);
return std::make_tuple(BootTimestamp{0, std::get<0>(result)},
BootDuration{0, std::get<1>(result)});
}
@@ -946,59 +947,119 @@
TestingNoncausalTimestampFilter filter(node_a, node_b);
+ std::pair<Pointer,
+ std::pair<std::tuple<logger::BootTimestamp, logger::BootDuration>,
+ std::tuple<logger::BootTimestamp, logger::BootDuration>>>
+ result;
+
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), 0),
+ result = filter.FindTimestamps(Pointer(), e - chrono::microseconds(10), 0);
+ EXPECT_THAT(result.second,
::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, 0),
+ EXPECT_EQ(result, filter.FindTimestamps(result.first,
+ e - chrono::microseconds(10), 0));
+
+ result =
+ filter.FindTimestamps(Pointer(), e - chrono::microseconds(10), 0.9, 0);
+ EXPECT_THAT(result.second,
::testing::Pair(::testing::Eq(std::make_tuple(t1, o1)),
::testing::Eq(std::make_tuple(t2, o2))));
+ EXPECT_EQ(result, filter.FindTimestamps(
+ result.first, e - chrono::microseconds(10), 0.9, 0));
- EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(0), 0),
+ result = filter.FindTimestamps(Pointer(), e + chrono::microseconds(0), 0);
+ EXPECT_THAT(result.second,
::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, 0),
+ EXPECT_EQ(result, filter.FindTimestamps(result.first,
+ e + chrono::microseconds(0), 0));
+
+ result =
+ filter.FindTimestamps(Pointer(), e + chrono::microseconds(0), 0.8, 0);
+ EXPECT_THAT(result.second,
::testing::Pair(::testing::Eq(std::make_tuple(t1, o1)),
::testing::Eq(std::make_tuple(t2, o2))));
+ EXPECT_EQ(result, filter.FindTimestamps(result.first,
+ e + chrono::microseconds(0), 0.8, 0));
- EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(100), 0),
+ result = filter.FindTimestamps(Pointer(), e + chrono::microseconds(100), 0);
+ EXPECT_THAT(result.second,
::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, 0),
+ EXPECT_EQ(result, filter.FindTimestamps(result.first,
+ e + chrono::microseconds(100), 0));
+
+ result =
+ filter.FindTimestamps(Pointer(), e + chrono::microseconds(100), 0.7, 0);
+ EXPECT_THAT(result.second,
::testing::Pair(::testing::Eq(std::make_tuple(t1, o1)),
::testing::Eq(std::make_tuple(t2, o2))));
+ EXPECT_EQ(result, filter.FindTimestamps(
+ result.first, e + chrono::microseconds(100), 0.7, 0));
- EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(1000), 0),
+ result = filter.FindTimestamps(Pointer(), e + chrono::microseconds(1000), 0);
+ EXPECT_THAT(result.second,
::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, 0),
- ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
- ::testing::Eq(std::make_tuple(t3, o3))));
+ EXPECT_EQ(result, filter.FindTimestamps(result.first,
- EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(1500), 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), 0.0, 0),
- ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
- ::testing::Eq(std::make_tuple(t3, o3))));
+ e + chrono::microseconds(1000), 0));
- EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(2000), 0),
+ result =
+ filter.FindTimestamps(Pointer(), e + chrono::microseconds(1000), 0.0, 0);
+ EXPECT_THAT(result.second,
::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, 0),
- ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
- ::testing::Eq(std::make_tuple(t3, o3))));
+ EXPECT_EQ(result, filter.FindTimestamps(
+ result.first, e + chrono::microseconds(1000), 0.0, 0));
- EXPECT_THAT(filter.FindTimestamps(e + chrono::microseconds(2500), 0),
+ result = filter.FindTimestamps(Pointer(), e + chrono::microseconds(1500), 0);
+ EXPECT_THAT(result.second,
::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, 0),
+ EXPECT_EQ(result, filter.FindTimestamps(result.first,
+ e + chrono::microseconds(1500), 0));
+
+ result =
+ filter.FindTimestamps(Pointer(), e + chrono::microseconds(1500), 0.0, 0);
+ EXPECT_THAT(result.second,
::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
::testing::Eq(std::make_tuple(t3, o3))));
+ EXPECT_EQ(result, filter.FindTimestamps(
+ result.first, e + chrono::microseconds(1500), 0.0, 0));
+
+ result = filter.FindTimestamps(Pointer(), e + chrono::microseconds(2000), 0);
+ EXPECT_THAT(result.second,
+ ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
+ ::testing::Eq(std::make_tuple(t3, o3))));
+ EXPECT_EQ(result, filter.FindTimestamps(result.first,
+ e + chrono::microseconds(2000), 0));
+ result =
+ filter.FindTimestamps(Pointer(), e + chrono::microseconds(2000), 0.1, 0);
+ EXPECT_THAT(result.second,
+ ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
+ ::testing::Eq(std::make_tuple(t3, o3))));
+ EXPECT_EQ(result, filter.FindTimestamps(
+ result.first, e + chrono::microseconds(2000), 0.1, 0));
+
+ result = filter.FindTimestamps(Pointer(), e + chrono::microseconds(2500), 0);
+ EXPECT_THAT(result.second,
+ ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
+ ::testing::Eq(std::make_tuple(t3, o3))));
+ EXPECT_EQ(result, filter.FindTimestamps(result.first,
+ e + chrono::microseconds(2500), 0));
+
+ result =
+ filter.FindTimestamps(Pointer(), e + chrono::microseconds(2500), 0.0, 0);
+ EXPECT_THAT(result.second,
+ ::testing::Pair(::testing::Eq(std::make_tuple(t2, o2)),
+ ::testing::Eq(std::make_tuple(t3, o3))));
+ EXPECT_EQ(result, filter.FindTimestamps(
+ result.first, e + chrono::microseconds(2500), 0.0, 0));
}
// Tests that Offset returns results indicative of it calling InterpolateOffset
@@ -1026,52 +1087,58 @@
filter.Sample(t1, o1);
// 1 point is handled properly.
- EXPECT_EQ(filter.Offset(t1, 0), o1);
- EXPECT_EQ(filter.Offset(t1, 0.0, 0), std::make_pair(o1, 0.0));
+ EXPECT_EQ(filter.Offset(Pointer(), t1, 0), o1);
+ EXPECT_EQ(filter.Offset(Pointer(), t1, 0.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.time - e.time).count() * kMaxVelocity();
- EXPECT_EQ(filter.Offset(e, 0),
+ EXPECT_EQ(filter.Offset(Pointer(), e, 0),
o1 + chrono::nanoseconds(static_cast<int64_t>(offset_pre)));
- EXPECT_EQ(filter.Offset(e, 0.0, 0), std::make_pair(o1, offset_pre));
+ EXPECT_EQ(filter.Offset(Pointer(), e, 0.0, 0),
+ std::make_pair(o1, offset_pre));
double offset_post = -(t2.time - t1.time).count() * kMaxVelocity();
- EXPECT_EQ(filter.Offset(t2, 0),
+ EXPECT_EQ(filter.Offset(Pointer(), t2, 0),
o1 + chrono::nanoseconds(static_cast<int64_t>(offset_post)));
- EXPECT_EQ(filter.Offset(t2, 0.0, 0), std::make_pair(o1, offset_post));
+ EXPECT_EQ(filter.Offset(Pointer(), t2, 0.0, 0),
+ std::make_pair(o1, offset_post));
filter.Sample(t2, o2);
filter.Sample(t3, o3);
- EXPECT_EQ(filter.Offset(t1, 0), o1);
- EXPECT_EQ(filter.Offset(t2, 0), o2);
- EXPECT_EQ(filter.Offset(t3, 0), o3);
+ EXPECT_EQ(filter.Offset(Pointer(), t1, 0), o1);
+ EXPECT_EQ(filter.Offset(Pointer(), t2, 0), o2);
+ EXPECT_EQ(filter.Offset(Pointer(), t3, 0), o3);
- EXPECT_EQ(filter.Offset(t1, 0.0, 0), std::make_pair(o1, 0.0));
+ EXPECT_EQ(filter.Offset(Pointer(), t1, 0.0, 0), std::make_pair(o1, 0.0));
EXPECT_EQ(
- filter.Offset(e + (t2.time_since_epoch() + t1.time_since_epoch()) / 2,
+ filter.Offset(Pointer(),
+ e + (t2.time_since_epoch() + t1.time_since_epoch()) / 2,
0.0, 0),
std::make_pair(o1, (o2d - o1d) / 2.));
- EXPECT_EQ(filter.Offset(t2, 0.0, 0), std::make_pair(o2, 0.0));
+ EXPECT_EQ(filter.Offset(Pointer(), t2, 0.0, 0), std::make_pair(o2, 0.0));
EXPECT_EQ(
- filter.Offset(e + (t2.time_since_epoch() + t3.time_since_epoch()) / 2,
+ filter.Offset(Pointer(),
+ e + (t2.time_since_epoch() + t3.time_since_epoch()) / 2,
0.0, 0),
std::make_pair(o2, (o2d + o3d) / 2. - o2d));
- EXPECT_EQ(filter.Offset(t3, 0.0, 0), std::make_pair(o3, 0.0));
+ EXPECT_EQ(filter.Offset(Pointer(), t3, 0.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, 0),
+ EXPECT_EQ(filter.Offset(Pointer(), e, 0),
o1 + chrono::nanoseconds(static_cast<int64_t>(offset_pre)));
- EXPECT_EQ(filter.Offset(e, 0.0, 0), std::make_pair(o1, offset_pre));
+ EXPECT_EQ(filter.Offset(Pointer(), e, 0.0, 0),
+ std::make_pair(o1, offset_pre));
// ... and after
offset_post = -(t4.time - t3.time).count() * kMaxVelocity();
- EXPECT_EQ(filter.Offset(t4, 0),
+ EXPECT_EQ(filter.Offset(Pointer(), t4, 0),
(o3 + chrono::nanoseconds(static_cast<int64_t>(offset_post))));
- EXPECT_EQ(filter.Offset(t4, 0.0, 0), std::make_pair(o3, offset_post));
+ EXPECT_EQ(filter.Offset(Pointer(), t4, 0.0, 0),
+ std::make_pair(o3, offset_post));
}
// Tests that adding duplicates gets correctly deduplicated.