Clear the point cache in NoncausalTimestampFilter's pointer
When we change the cached t0, t1, there was a code path which would end
up leaving the old, bad points from before. We would then interpolate
very poorly, failing line search and making the solver not converge.
Add a check to catch the bad code path, and then fix it.
Change-Id: Ie544a47c704ee009f0dde2d4c22907f4ffd7189f
Signed-off-by: James Kuszmaul <james.kuszmaul@bluerivertech.com>
diff --git a/aos/network/multinode_timestamp_filter.cc b/aos/network/multinode_timestamp_filter.cc
index 85ee12c..04a3631 100644
--- a/aos/network/multinode_timestamp_filter.cc
+++ b/aos/network/multinode_timestamp_filter.cc
@@ -770,25 +770,32 @@
const Eigen::Ref<const Eigen::VectorXd> v =
y.block(x.rows() + lambda.rows(), 0, derivatives.A.rows(), 1);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix << "x: " << x.transpose().format(heavy);
+ << std::setprecision(12) << " " << prefix
+ << "x: " << x.transpose().format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix << "lambda: " << lambda.transpose().format(heavy);
+ << std::setprecision(12) << " " << prefix
+ << "lambda: " << lambda.transpose().format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix << "v: " << v.transpose().format(heavy);
+ << std::setprecision(12) << " " << prefix
+ << "v: " << v.transpose().format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix
+ << std::setprecision(12) << " " << prefix
<< "hessian: " << derivatives.hessian.format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix
+ << std::setprecision(12) << " " << prefix
<< "gradient: " << derivatives.gradient.format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix << "A: " << derivatives.A.format(heavy);
+ << std::setprecision(12) << " " << prefix
+ << "A: " << derivatives.A.format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix << "Ax-b: " << derivatives.Axmb.format(heavy);
+ << std::setprecision(12) << " " << prefix
+ << "Ax-b: " << derivatives.Axmb.format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix << "f: " << derivatives.f.format(heavy);
+ << std::setprecision(12) << " " << prefix
+ << "f: " << derivatives.f.format(heavy);
SOLVE_VLOG(my_solve_number_, verbosity)
- << " " << prefix << "df: " << derivatives.df.format(heavy);
+ << std::setprecision(12) << " " << prefix
+ << "df: " << derivatives.df.format(heavy);
}
}
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index 4a29dfc..9736679 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -531,6 +531,13 @@
if (!use_other) {
return std::make_pair(pointer, std::make_pair(t0, t1));
}
+
+ // The invariant of pointer is that other_points is bounded by t0, t1. Confirm
+ // it before we return things depending on it since it is easy.
+ CHECK_GT(std::get<0>(pointer.other_points_[0].second), std::get<0>(t0));
+ CHECK_LT(std::get<0>(
+ pointer.other_points_[pointer.other_points_.size() - 1].second),
+ std::get<0>(t1));
// We have 2 timestamps bookending everything, and a list of points in the
// middle.
//
@@ -660,11 +667,14 @@
const size_t index = std::distance(timestamps_.begin(), it);
+ // Now, update the pointer cache.
pointer.index_ = index - 1;
t0 = timestamp(index - 1);
pointer.t0_ = t0;
t1 = timestamp(index);
pointer.t1_ = t1;
+ // And clear out the point cache since the points changed.
+ pointer.other_points_.clear();
if (other != nullptr && !other->timestamps_empty()) {
// Ok, we now need to find all points within our range in the matched
@@ -688,8 +698,6 @@
if (std::get<0>(*other_t0_it) + std::get<1>(*other_t0_it) <
std::get<0>(pointer.t1_)) {
- pointer.other_points_.clear();
-
// Now, we've got a range. [other_t0_it, other_t1_it).
for (auto other_it = other_t0_it; other_it != other_t1_it; ++other_it) {
const std::tuple<monotonic_clock::time_point,
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index 8b597bd..b1aff86 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -306,7 +306,12 @@
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> t1_ =
std::make_tuple(monotonic_clock::min_time, std::chrono::nanoseconds(0));
- // List of points and their associated times going the other way.
+ // List of points and their associated times going the other way. This lets
+ // us handle when there is a large gap in timestamps, but time drifts in
+ // such a way to create an impossible situation when the points are
+ // connected by straight lines.
+ //
+ // These need to be between t0 and t1.
std::vector<std::pair<size_t, std::tuple<monotonic_clock::time_point,
std::chrono::nanoseconds>>>
other_points_;
@@ -869,6 +874,14 @@
return result;
}
+ // Interpolates between two points for time ta using the provided pointer, t0,
+ // and t1. If use_other is true, then we use other_points_ inside pointer to
+ // also interpolate with. This lets us handle cases where we have
+ // observations only going one way, but the filter lines cross because time
+ // drifted.
+ //
+ // Returns the 2 points which define the line we should interpolate along, and
+ // an updated pointer caching what points were actually used.
static std::pair<
Pointer,
std::pair<