Remove nlopt based timestamp solving code
The new code appears to be working nicely with fewer lines of code and
fewer dependencies.
Change-Id: Ib9e91e1968ec67ceddfbbbeddbd44f2786ee1253
diff --git a/aos/network/BUILD b/aos/network/BUILD
index 42e115b..96e9f90 100644
--- a/aos/network/BUILD
+++ b/aos/network/BUILD
@@ -502,7 +502,6 @@
"//aos/events:simulated_event_loop",
"//aos/events/logging:logfile_utils",
"//aos/time",
- "@com_github_stevengj_nlopt//:nlopt",
"@org_tuxfamily_eigen//:eigen",
],
)
@@ -543,7 +542,5 @@
":testing_time_converter",
":timestamp_filter",
"//aos/testing:googletest",
- "//third_party/gmp",
- "@com_github_stevengj_nlopt//:nlopt",
],
)
diff --git a/aos/network/multinode_timestamp_filter.cc b/aos/network/multinode_timestamp_filter.cc
index 292d24d..d7bbff0 100644
--- a/aos/network/multinode_timestamp_filter.cc
+++ b/aos/network/multinode_timestamp_filter.cc
@@ -10,7 +10,6 @@
#include "aos/network/timestamp_filter.h"
#include "aos/time/time.h"
#include "glog/logging.h"
-#include "nlopt.h"
DEFINE_bool(timestamps_to_csv, false,
"If true, write all the time synchronization information to a set "
@@ -43,101 +42,6 @@
// TODO(austin): Add a rate of change constraint from the last sample. 1
// ms/s. Figure out how to define it. Do this last. This lets us handle
// constraints going away, and constraints close in time.
-//
-// TODO(austin): When the new newton's method solver prooves it's worth, kill
-// the old SLSQP solver. It will be unreachable for a little bit.
-
-std::vector<double> TimestampProblem::SolveDouble() {
- MaybeUpdateNodeMapping();
- // TODO(austin): Add constraints for relevant segments.
- const size_t n = filters_.size() - 1u;
- // NLOPT_LD_MMA and NLOPT_LD_LBFGS are alternative solvers, but SLSQP is a
- // better fit for the quadratic nature of this problem.
- nlopt_opt opt = nlopt_create(NLOPT_LD_SLSQP, n);
- nlopt_set_min_objective(opt, TimestampProblem::DoCost, this);
-
- // Ask for really good. This is very quadratic, so it should be pretty
- // precise.
- nlopt_set_xtol_rel(opt, 1e-9);
-
- cost_call_count_ = 0;
-
- std::vector<double> result(n, 0.0);
- double minf = 0.0;
- nlopt_result status = nlopt_optimize(opt, result.data(), &minf);
- if (status < 0) {
- if (status == NLOPT_ROUNDOFF_LIMITED) {
- constexpr double kTolerance = 1e-9;
- std::vector<double> gradient(n, 0.0);
- Cost(result.data(), gradient.data());
- for (double g : gradient) {
- if (std::abs(g) > kTolerance) {
- // If we failed, update base_clock_ to the current time so it gets
- // printed inside Debug and explode with a CHECK saying the same
- // thing.
- std::vector<monotonic_clock::time_point> new_base =
- DoubleToMonotonic(result.data());
- // Put the result into base_clock_ so Debug prints out something
- // useful.
- base_clock_ = std::move(new_base);
- Debug();
- CHECK_LE(std::abs(g), kTolerance)
- << ": Optimization problem failed with a large gradient. "
- << nlopt_result_to_string(status);
- }
- }
- } else {
- LOG(FATAL) << "Failed to solve optimization problem "
- << nlopt_result_to_string(status);
- }
- }
-
- if (VLOG_IS_ON(1)) {
- PrintSolution(result);
- }
- nlopt_destroy(opt);
- return result;
-}
-
-void TimestampProblem::PrintSolution(const std::vector<double> &solution) {
- const size_t n = filters_.size() - 1u;
- std::vector<double> gradient(n, 0.0);
- const double minf = Cost(solution.data(), gradient.data());
-
- // High precision formatter for the gradient.
- struct MyFormatter {
- void operator()(std::string *out, double i) const {
- std::stringstream ss;
- ss << std::setprecision(12) << std::fixed << i;
- out->append(ss.str());
- }
- };
-
- LOG(INFO) << std::setprecision(12) << std::fixed << "Found minimum at f("
- << absl::StrJoin(solution, ", ") << ") -> " << minf << " grad ["
- << absl::StrJoin(gradient, ", ", MyFormatter()) << "] after "
- << cost_call_count_ << " cycles for node " << solution_node_ << ".";
-}
-
-std::vector<monotonic_clock::time_point> TimestampProblem::DoubleToMonotonic(
- const double *r) const {
- std::vector<monotonic_clock::time_point> result(filters_.size());
- for (size_t i = 0; i < result.size(); ++i) {
- if (live(i)) {
- result[i] = base_clock(i) + std::chrono::nanoseconds(static_cast<int64_t>(
- std::round(get_t(r, i))));
- } else {
- result[i] = monotonic_clock::min_time;
- }
- }
-
- return result;
-}
-
-std::vector<monotonic_clock::time_point> TimestampProblem::Solve() {
- std::vector<double> solution = SolveDouble();
- return DoubleToMonotonic(solution.data());
-}
bool TimestampProblem::ValidateSolution(
std::vector<monotonic_clock::time_point> solution) {
@@ -384,99 +288,26 @@
return result;
}
-double TimestampProblem::Cost(const double *time_offsets, double *grad) {
- ++cost_call_count_;
-
- if (grad != nullptr) {
- for (size_t i = 0; i < filters_.size() - 1u; ++i) {
- grad[i] = 0;
- }
-
- for (size_t i = 0u; i < filters_.size(); ++i) {
- for (const struct FilterPair &filter : filters_[i]) {
- if (i != solution_node_) {
- grad[NodeToSolutionIndex(i)] += filter.filter->DCostDta(
- base_clock_[i], get_t(time_offsets, i),
- base_clock_[filter.b_index], get_t(time_offsets, filter.b_index));
- }
- if (filter.b_index != solution_node_) {
- grad[NodeToSolutionIndex(filter.b_index)] += filter.filter->DCostDtb(
- base_clock_[i], get_t(time_offsets, i),
- base_clock_[filter.b_index], get_t(time_offsets, filter.b_index));
- }
- }
- }
- }
-
- double cost = 0;
- for (size_t i = 0u; i < filters_.size(); ++i) {
- for (const struct FilterPair &filter : filters_[i]) {
- cost += filter.filter->Cost(base_clock_[i], get_t(time_offsets, i),
- base_clock_[filter.b_index],
- get_t(time_offsets, filter.b_index));
- }
- }
-
- if (VLOG_IS_ON(2)) {
- struct MyFormatter {
- void operator()(std::string *out, monotonic_clock::time_point t) const {
- std::stringstream ss;
- ss << t;
- out->append(ss.str());
- }
- void operator()(std::string *out, double i) const {
- std::stringstream ss;
- ss << std::setprecision(12) << std::fixed << i;
- out->append(ss.str());
- }
- };
-
- std::string gradient;
- if (grad) {
- std::stringstream ss;
- ss << " grad ["
- << absl::StrJoin(absl::Span<const double>(grad, LiveNodesCount() - 1u),
- ", ", MyFormatter())
- << "]";
- gradient = ss.str();
- }
-
- LOG(INFO) << std::setprecision(12) << std::fixed
- << "Evaluated minimum at f("
- << absl::StrJoin(DoubleToMonotonic(time_offsets), ", ",
- MyFormatter())
- << ") -> " << cost << gradient << " after " << cost_call_count_
- << " cycles.";
- }
- return cost;
-}
-
void TimestampProblem::Debug() {
MaybeUpdateNodeMapping();
LOG(INFO) << "Solving for node " << solution_node_ << " at "
<< base_clock_[solution_node_];
- std::vector<std::string> cost;
- for (size_t i = 0u; i < filters_.size(); ++i) {
- for (const struct FilterPair &filter : filters_[i]) {
- cost.emplace_back(filter.filter->DebugCost(base_clock_[i], 0.0,
- base_clock_[filter.b_index],
- 0.0, i, filter.b_index));
- }
- }
- LOG(INFO) << "Cost: " << absl::StrJoin(cost, " + ");
-
std::vector<std::vector<std::string>> gradients(filters_.size());
for (size_t i = 0u; i < filters_.size(); ++i) {
- std::string gradient = "0.0";
for (const struct FilterPair &filter : filters_[i]) {
- if (i != solution_node_ && live(i)) {
- gradients[i].emplace_back(filter.filter->DebugDCostDta(
- base_clock_[i], 0.0, base_clock_[filter.b_index], 0.0, i,
- filter.b_index));
- }
- if (filter.b_index != solution_node_ && live(filter.b_index)) {
- gradients[filter.b_index].emplace_back(filter.filter->DebugDCostDtb(
+ if (live(i) && live(filter.b_index)) {
+ // TODO(austin): This should be right, but I haven't gone and spent a
+ // bunch of time making sure it all matches perfectly. We aren't
+ // hitting this anymore. I'm also likely the one who will be debugging
+ // it next and would rather spend the time debugging it when I get a bug
+ // report.
+ gradients[i].emplace_back(
+ std::string("- ") +
+ filter.filter->DebugOffsetError(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));
}
diff --git a/aos/network/multinode_timestamp_filter.h b/aos/network/multinode_timestamp_filter.h
index 338a515..929c837 100644
--- a/aos/network/multinode_timestamp_filter.h
+++ b/aos/network/multinode_timestamp_filter.h
@@ -57,40 +57,10 @@
filters_[a_index].emplace_back(filter, b_index);
}
- // Solves the optimization problem phrased and returns the offsets from the
- // base clock for each node, excluding the solution node.
- std::vector<double> SolveDouble();
- // Solves the optimization problem phrased and returns the optimal time on
- // each node.
- std::vector<monotonic_clock::time_point> Solve();
-
// Solves the optimization problem phrased using the symmetric Netwon's method
// solver and returns the optimal time on each node.
std::vector<monotonic_clock::time_point> SolveNewton();
- // Returns the squared error for all of the offsets.
- // time_offsets is the offsets from the base_clock for every node (in order)
- // except the solution node. It should be one element shorter than the number
- // of nodes this problem was constructed with. grad (if non-nullptr) is the
- // place to put the current gradient and needs to be the same length as
- // time_offsets.
- double Cost(const double *time_offsets, double *grad);
-
- // Returns the time offset from base for a node.
- double get_t(const double *time_offsets, size_t node_index) const {
- if (node_index == solution_node_) return 0.0;
- size_t mapped_index = NodeToSolutionIndex(node_index);
- if (mapped_index == std::numeric_limits<size_t>::max()) {
- return 0.0;
- }
- return time_offsets[mapped_index];
- }
-
- // Converts a list of solutions to the corresponding monotonic times for all
- // nodes, not just the nodes being solved.
- std::vector<monotonic_clock::time_point> DoubleToMonotonic(
- const double *r) const;
-
// Validates the solution, returning true if it meets all the constraints, and
// false otherwise.
bool ValidateSolution(std::vector<monotonic_clock::time_point> solution);
@@ -115,21 +85,7 @@
return count;
}
- // LOG(INFO)'s the provided solution, showing the arguments, the minimum
- // value, and the gradient.
- void PrintSolution(const std::vector<double> &solution);
-
private:
- // Static trampoline for nlopt. n is the number of constraints, time_offsets
- // is input solution to solve for, grad is the gradient to fill out (if not
- // nullptr), and data is an untyped pointer to a TimestampProblem.
- static double DoCost(unsigned n, const double *time_offsets, double *grad,
- void *data) {
- CHECK_EQ(n + 1u,
- reinterpret_cast<TimestampProblem *>(data)->filters_.size());
- return reinterpret_cast<TimestampProblem *>(data)->Cost(time_offsets, grad);
- }
-
// 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.
@@ -158,17 +114,6 @@
node_mapping_valid_ = true;
}
- // Converts from a node index to an index in the solution.
- size_t NodeToSolutionIndex(size_t node_index) const {
- CHECK_NE(node_index, solution_node_);
- // The solver is going to provide us a matrix with solution_node_ removed.
- // The indices of all nodes before solution_node_ are in the same spot, and
- // the indices of the nodes after solution node are shifted over.
- size_t mapped_node_index = NodeToFullSolutionIndex(node_index);
- return node_index < solution_node_ ? mapped_node_index
- : (mapped_node_index - 1);
- }
-
// Converts from a node index to an index in the solution without skipping the
// solution node.
size_t NodeToFullSolutionIndex(size_t node_index) const {
@@ -176,9 +121,6 @@
return node_mapping_[node_index];
}
- // Number of times Cost has been called for tracking.
- int cost_call_count_ = 0;
-
// The node to hold fixed when solving.
size_t solution_node_ = 0;
diff --git a/aos/network/multinode_timestamp_filter_test.cc b/aos/network/multinode_timestamp_filter_test.cc
index 1d13601..f1cf0aa 100644
--- a/aos/network/multinode_timestamp_filter_test.cc
+++ b/aos/network/multinode_timestamp_filter_test.cc
@@ -8,8 +8,6 @@
#include "aos/network/multinode_timestamp_filter.h"
#include "aos/network/testing_time_converter.h"
#include "gtest/gtest.h"
-#include "nlopt.h"
-#include "third_party/gmp/gmpxx.h"
namespace aos {
namespace message_bridge {
@@ -18,140 +16,6 @@
namespace chrono = std::chrono;
using aos::monotonic_clock;
-// Converts a int64_t into a mpq_class. This only uses 32 bit precision
-// internally, so it will work on ARM. This should only be used on 64 bit
-// platforms to test out the 32 bit implementation.
-inline mpq_class FromInt64(int64_t i) {
- uint64_t absi = std::abs(i);
- mpq_class bits(static_cast<uint32_t>((absi >> 32) & 0xffffffffu));
- bits *= mpq_class(0x10000);
- bits *= mpq_class(0x10000);
- bits += mpq_class(static_cast<uint32_t>(absi & 0xffffffffu));
-
- if (i < 0) {
- return -bits;
- } else {
- return bits;
- }
-}
-
-// Class to hold an affine function for the time offset.
-// O(t) = slope * t + offset
-//
-// This is stored using mpq_class, which stores everything as full rational
-// fractions.
-class Line {
- public:
- Line() {}
-
- // Constructs a line given the offset and slope.
- Line(mpq_class offset, mpq_class slope) : offset_(offset), slope_(slope) {}
-
- // TODO(austin): Remove this one.
- Line(std::chrono::nanoseconds offset, double slope)
- : offset_(DoFromInt64(offset.count())), slope_(slope) {}
-
- // Fits a line to 2 points and returns the associated line.
- static Line Fit(
- const std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> a,
- const std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds>
- b);
-
- // Returns the full precision slopes and offsets.
- mpq_class mpq_offset() const { return offset_; }
- mpq_class mpq_slope() const { return slope_; }
- void increment_mpq_offset(mpq_class increment) { offset_ += increment; }
-
- // Returns the rounded offsets and slopes.
- std::chrono::nanoseconds offset() const {
- double o = offset_.get_d();
- return std::chrono::nanoseconds(static_cast<int64_t>(o));
- }
- double slope() const { return slope_.get_d(); }
-
- std::string DebugString() const {
- std::stringstream ss;
- ss << "Offset " << mpq_offset() << " slope " << mpq_slope();
- return ss.str();
- }
-
- void Debug() const { LOG(INFO) << DebugString(); }
-
- // Returns the offset at a given time.
- // TODO(austin): get_d() ie double -> int64 can't be accurate...
- std::chrono::nanoseconds Eval(monotonic_clock::time_point pt) const {
- mpq_class result =
- mpq_class(FromInt64(pt.time_since_epoch().count())) * slope_ + offset_;
- return std::chrono::nanoseconds(static_cast<int64_t>(result.get_d()));
- }
-
- private:
- static mpq_class DoFromInt64(int64_t i) {
-#if GMP_NUMB_BITS == 32
- return FromInt64(i);
-#else
- return i;
-#endif
- }
-
- mpq_class offset_;
- mpq_class slope_;
-};
-
-Line Line::Fit(
- const std::tuple<monotonic_clock::time_point, chrono::nanoseconds> a,
- const std::tuple<monotonic_clock::time_point, chrono::nanoseconds> b) {
- mpq_class slope = FromInt64((std::get<1>(b) - std::get<1>(a)).count()) /
- FromInt64((std::get<0>(b) - std::get<0>(a)).count());
- slope.canonicalize();
- mpq_class offset =
- FromInt64(std::get<1>(a).count()) -
- FromInt64(std::get<0>(a).time_since_epoch().count()) * slope;
- offset.canonicalize();
- Line f(offset, slope);
- return f;
-}
-
-mpq_class SolveExact(Line la, Line lb, monotonic_clock::time_point ta) {
- mpq_class ma = la.mpq_slope();
- mpq_class ba = la.mpq_offset();
- mpq_class mb = lb.mpq_slope();
- mpq_class bb = lb.mpq_offset();
- // The min of a quadratic is when the slope is 0. Solve algebraically.
- //
- // 2.0 * (tb - (1 + d.ma) * ta - d.ba) + 2.0 * ((1.0 + d.mb) * tb - ta +
- // d.bb) * (1.0 + d.mb) = 0;
- //
- // tb - (1 + d.ma) * ta - d.ba + ((1.0 + d.mb) *
- // tb - ta + d.bb) * (1.0 + d.mb) = 0;
- //
- // tb - (1 + d.ma) * ta - d.ba + (1 + d.mb) (1 + d.mb) * tb - (1 + d.mb) ta
- // + (1 + d.mb) d.bb = 0;
- //
- // (1 + (1 + d.mb) (1 + d.mb)) tb - ((1 + d.ma) + (1
- // + d.mb)) * ta - d.ba + (1 + d.mb) d.bb = 0;
- //
- // tb = (((1 + d.ma) + (1 + d.mb)) * ta + d.ba - (1 + d.mb) d.bb) / (1 + (1
- // + d.mb) (1 + d.mb))
-
- mpq_class mpq_ta(FromInt64(ta.time_since_epoch().count()));
- mpq_class one(1);
- mpq_class mpq_tb =
- (((one + ma) + (one + mb)) * mpq_ta + ba - (one + mb) * bb) /
- (one + (one + mb) * (one + mb));
- mpq_tb.canonicalize();
- return mpq_tb;
-}
-
-Line FitLine(const NoncausalTimestampFilter &filter) {
- if (filter.timestamps_size() == 1) {
- Line fit(std::get<1>(filter.timestamp(0)), 0.0);
- return fit;
- } else {
- return Line::Fit(filter.timestamp(0), filter.timestamp(1));
- }
-}
-
// Tests solution time(s) comparison and measure of invalid / inconsistent times
TEST(TimestampProblemTest, CompareTimes) {
const monotonic_clock::time_point e = monotonic_clock::epoch();
@@ -213,93 +77,6 @@
CHECK_EQ(InvalidDistance(times_b_mixed, times_a).count(), 3000);
}
-// Tests that an infinite precision solution matches our numeric solver solution
-// for a couple of simple problems.
-TEST(TimestampProblemTest, Solve) {
- const monotonic_clock::time_point e = monotonic_clock::epoch();
- const monotonic_clock::time_point ta = e + chrono::milliseconds(500);
-
- NoncausalTimestampFilter a(nullptr, nullptr);
- // Delivered at e, sent at e + offset.
- // Sent at 1.001, received at 0
- a.Sample(e, chrono::milliseconds(1001));
- a.Sample(e + chrono::milliseconds(1000), chrono::milliseconds(1001));
- a.Sample(e + chrono::milliseconds(3000), chrono::milliseconds(999));
-
- NoncausalTimestampFilter b(nullptr, nullptr);
- // Sent at 0.001s, received at 1.000s
- b.Sample(e + chrono::milliseconds(1000), -chrono::milliseconds(999));
- b.Sample(e + chrono::milliseconds(2000), -chrono::milliseconds(1000));
- b.Sample(e + chrono::milliseconds(4000), -chrono::milliseconds(1002));
-
- TimestampProblem problem(2);
- problem.set_base_clock(0, ta);
- problem.set_base_clock(1, e);
- problem.set_solution_node(0);
- problem.add_filter(0, &a, 1);
- problem.add_filter(1, &b, 0);
-
- // Solve the problem with infinite precision as a verification and compare the
- // result.
- {
- const std::vector<double> result = problem.SolveDouble();
-
- mpq_class tb_mpq =
- SolveExact(FitLine(a), FitLine(b), problem.base_clock(0));
- EXPECT_EQ(tb_mpq.get_d(), result[0])
- << std::setprecision(12) << std::fixed << " Expected " << tb_mpq.get_d()
- << " " << tb_mpq << " got " << result[0];
- }
-
- // Solve some other timestamps for grins.
- {
- problem.set_base_clock(0, e + chrono::milliseconds(500));
- std::vector<double> result = problem.SolveDouble();
-
- mpq_class tb_mpq =
- SolveExact(FitLine(a), FitLine(b), problem.base_clock(0));
-
- EXPECT_EQ(tb_mpq.get_d(), result[0])
- << std::setprecision(12) << std::fixed << " Expected " << tb_mpq.get_d()
- << " " << tb_mpq << " got " << result[0];
- }
-
- // Now do the second line segment.
- {
- NoncausalTimestampFilter a(nullptr, nullptr);
- a.Sample(e + chrono::milliseconds(1000), chrono::milliseconds(1001));
- a.Sample(e + chrono::milliseconds(3000), chrono::milliseconds(999));
-
- NoncausalTimestampFilter b(nullptr, nullptr);
- b.Sample(e + chrono::milliseconds(2000), -chrono::milliseconds(1000));
- b.Sample(e + chrono::milliseconds(4000), -chrono::milliseconds(1002));
- {
- problem.set_base_clock(0, e + chrono::milliseconds(1500));
- const std::vector<double> result = problem.SolveDouble();
-
- mpq_class tb_mpq =
- SolveExact(FitLine(a), FitLine(b), problem.base_clock(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];
- }
-
- {
- problem.set_base_clock(0, e + chrono::milliseconds(1600));
- const std::vector<double> result = problem.SolveDouble();
-
- mpq_class tb_mpq =
- SolveExact(FitLine(a), FitLine(b), problem.base_clock(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]
- << " difference of " << tb_mpq.get_d() - result[0];
- }
- }
-}
-
// Tests that a single timestamp InterpolatedTimeConverter returns equal
// results. 1 second should be 1 second everywhere.
TEST(InterpolatedTimeConverterTest, OneTime) {
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index dd77f40..1041b97 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -753,164 +753,7 @@
((tb - ta) - offset.second);
}
-double NoncausalTimestampFilter::Cost(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);
-
- // Squaring the error throws away half the digits. The optimizer uses the
- // gradient heavily to compensate, so we don't need to care much about
- // computing this carefully.
- return std::pow(OffsetError(ta_base, ta, tb_base, tb), 2.0);
-}
-
-double NoncausalTimestampFilter::DCostDta(
- aos::monotonic_clock::time_point ta_base, double ta,
- aos::monotonic_clock::time_point tb_base, double tb) const {
- // As a reminder, our cost function is:
- // (OffsetError(ta, tb))^2
- // ie
- // ((tb - ta - Offset(ta))^2
- //
- // Assuming Offset(ta) = m * ta + ba (linear): this becomes
- // ((tb - ta - (ma ta + ba))^2
- // ie
- // ((tb - (1 + ma) ta - ba)^2
- //
- // d cost/dta =>
- // 2 * (tb - (1 + ma) ta - ba) * (-(1 + ma))
- //
- // 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)
- //
- // We can compute this a lot more accurately.
-
- // 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.
- NormalizeTimestamps(&ta_base, &ta);
- NormalizeTimestamps(&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>,
- std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
- points = FindTimestamps(ta_base, ta);
-
- const double m =
- static_cast<double>(
- (std::get<1>(points.second) - std::get<1>(points.first)).count()) /
- static_cast<double>(
- (std::get<0>(points.second) - std::get<0>(points.first)).count());
- // Subtract the integer offsets first and then handle the double remainder to
- // keep precision up.
- //
- // (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(
- points.first, points.second, ta_base, ta))
- .count()) +
- (tb - ta) -
- NoncausalTimestampFilter::InterpolateOffsetRemainder(
- points.first, points.second, ta_base, ta)) *
- (1.0 + m);
-}
-
-std::string NoncausalTimestampFilter::DebugDCostDta(
- 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 {
- NormalizeTimestamps(&ta_base, &ta);
- NormalizeTimestamps(&tb_base, &tb);
-
- 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>,
- std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
- points = FindTimestamps(ta_base, ta);
-
- // As a reminder, our cost function is essentially:
- // ((tb - ta - (ma ta + ba))^2
- // ie
- // ((tb - (1 + ma) ta - ba)^2
- //
- // d cost/dta =>
- // 2 * (tb - (1 + ma) ta - ba) * (-(1 + ma))
-
- const int64_t rise =
- (std::get<1>(points.second) - std::get<1>(points.first)).count();
- const int64_t run =
- (std::get<0>(points.second) - std::get<0>(points.first)).count();
-
- if (rise == 0) {
- return absl::StrFormat("-2. * (t%d - t%d %c %d.)", node_b, node_a,
- std::get<1>(points.first).count() >= 0 ? '-' : '+',
- std::abs(std::get<1>(points.first).count()));
- } else {
- return absl::StrFormat(
- "-2. * (t%d - t%d - (t%d - %d.) * %d. / %d. - %d.) * (1 + %d. / "
- "%d.)",
- node_b, node_a, node_a,
- std::get<0>(points.first).time_since_epoch().count(), rise, run,
- std::get<1>(points.first).count(), rise, run);
- }
-}
-
-double NoncausalTimestampFilter::DCostDtb(
- aos::monotonic_clock::time_point ta_base, double ta,
- aos::monotonic_clock::time_point tb_base, double tb) const {
- // As a reminder, our cost function is:
- // (OffsetError(ta, tb))^2
- //
- // d cost/dtb =>
- // 2 * OffsetError(ta, tb) * d/dtb OffsetError(ta, tb)
- //
- // OffsetError => (tb - (1 + ma) ta - ba), so
- // 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);
-}
-
-std::string NoncausalTimestampFilter::DebugDCostDtb(
+std::string NoncausalTimestampFilter::DebugOffsetError(
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 {
@@ -962,58 +805,6 @@
rise, run, std::get<1>(points.first).count());
}
-std::string NoncausalTimestampFilter::DebugCost(
- 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 {
- NormalizeTimestamps(&ta_base, &ta);
- NormalizeTimestamps(&tb_base, &tb);
-
- 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>,
- std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
- points = FindTimestamps(ta_base, ta);
-
- // As a reminder, our cost function is essentially:
- // ((tb - ta - (ma ta + ba))^2
- // ie
- // ((tb - (1 + ma) ta - ba)^2
- //
- // d cost/dta =>
- // 2 * ((tb - (1 + ma) ta - ba)
-
- const int64_t rise =
- (std::get<1>(points.second) - std::get<1>(points.first)).count();
- const int64_t run =
- (std::get<0>(points.second) - std::get<0>(points.first)).count();
-
- if (rise == 0) {
- return absl::StrFormat("(t%d - t%d %c %d.) ** 2.", node_b, node_a,
- std::get<1>(points.first).count() < 0 ? '+' : '-',
- std::abs(std::get<1>(points.first).count()));
- } else {
- return absl::StrFormat("(t%d - t%d - (t%d - %d.) * %d. / %d. - %d.) ** 2.",
- node_b, node_a, node_a,
- std::get<0>(points.first).time_since_epoch().count(),
- rise, run, std::get<1>(points.first).count());
- }
-}
-
std::string NoncausalTimestampFilter::NodeNames() const {
return absl::StrCat(node_a_->name()->string_view(), " -> ",
node_b_->name()->string_view());
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index 6090fce..73c9006 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -261,29 +261,11 @@
// offset at ta.
double OffsetError(aos::monotonic_clock::time_point ta_base, double ta,
aos::monotonic_clock::time_point tb_base, double tb) const;
-
- // Returns the cost (OffsetError^2), ie (ob - oa - offset(oa, ob))^2,
- // calculated accurately.
- // Since this is designed to be used with a gradient based solver, it isn't
- // super important if Cost is precise.
- double Cost(aos::monotonic_clock::time_point ta_base, double ta,
- aos::monotonic_clock::time_point tb_base, double tb) const;
- std::string DebugCost(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;
-
- // Returns the partial derivitive dcost/dta
- double DCostDta(aos::monotonic_clock::time_point ta_base, double ta,
- aos::monotonic_clock::time_point tb_base, double tb) const;
- std::string DebugDCostDta(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;
- // Returns the partial derivitive dcost/dtb
- double DCostDtb(aos::monotonic_clock::time_point ta_base, double ta,
- aos::monotonic_clock::time_point tb_base, double tb) const;
- std::string DebugDCostDtb(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;
+ // Returns the string representation of 2 * OffsetError(ta, tb)
+ std::string DebugOffsetError(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;
// Confirms that the solution meets the constraints. Returns true on success.
bool ValidateSolution(aos::monotonic_clock::time_point ta,
diff --git a/aos/network/timestamp_filter_test.cc b/aos/network/timestamp_filter_test.cc
index a7e1dcf..95efe08 100644
--- a/aos/network/timestamp_filter_test.cc
+++ b/aos/network/timestamp_filter_test.cc
@@ -984,166 +984,6 @@
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
-// derivatives are consistent. Do this with a massive offset to ensure that we
-// are subtracting out nominal offsets correctly to retain numerical precision
-// in the result.
-TEST_F(NoncausalTimestampFilterTest, CostAndSlopeSinglePoint) {
- const monotonic_clock::time_point e = monotonic_clock::epoch();
- const monotonic_clock::time_point t1 =
- e + chrono::nanoseconds(0) + chrono::seconds(10000000000);
- const chrono::nanoseconds o1 =
- chrono::nanoseconds(1000) - chrono::seconds(10000000000);
-
- NoncausalTimestampFilter filter(node_a, node_b);
-
- 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, 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, 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.
- // 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 =
- filter.Cost(t1, ta_nominal + kDelta, t1 + o1, tb_nominal);
-
- const double minus_costb =
- filter.Cost(t1, ta_nominal, t1 + o1, tb_nominal - kDelta);
- const double plus_costb =
- filter.Cost(t1, ta_nominal, t1 + o1, tb_nominal + kDelta);
-
- EXPECT_NEAR((plus_costa - minus_costa) / (2.0 * kDelta),
- filter.DCostDta(t1, ta_nominal, t1 + o1, tb_nominal), 1e-9);
- EXPECT_NEAR((plus_costb - minus_costb) / (2.0 * kDelta),
- filter.DCostDtb(t1, ta_nominal, t1 + o1, tb_nominal), 1e-9);
- }
- }
- }
-}
-
-TEST_F(NoncausalTimestampFilterTest, CostAndSlope) {
- const monotonic_clock::time_point e = monotonic_clock::epoch();
- // 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) + chrono::seconds(10000000000);
- const chrono::nanoseconds o1 =
- chrono::nanoseconds(1000) - chrono::seconds(10000000000);
-
- const monotonic_clock::time_point t2 =
- e + chrono::microseconds(1000) + chrono::seconds(10000000000);
- const chrono::nanoseconds o2 =
- chrono::nanoseconds(1500) - chrono::seconds(10000000000);
-
- const monotonic_clock::time_point t3 =
- e + chrono::microseconds(2000) + chrono::seconds(10000000000);
- const chrono::nanoseconds o3 =
- chrono::nanoseconds(500) - chrono::seconds(10000000000);
-
- NoncausalTimestampFilter filter(node_a, node_b);
-
- filter.Sample(t1, o1);
- filter.Sample(t2, o2);
- filter.Sample(t3, o3);
-
- // Spot check some known points.
- EXPECT_EQ(filter.OffsetError(t1, 0.0, t1 + o1, 0.0), 0.0);
- 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);
-
- EXPECT_EQ(filter.Cost(t1, 0.0, t1 + o1, 0.0), 0.0);
- EXPECT_EQ(filter.Cost(t2, 0.0, t2 + o2, 0.0), 0.0);
- EXPECT_EQ(filter.Cost(t3, 0.0, t3 + o3, 0.0), 0.0);
-
- // Perturb ta and tbd so we make sure it works away from 0.
- constexpr double kDelta = 10.;
-
- // 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 : {-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.
- {
- const double minus_costa =
- filter.Cost(t1, ta_nominal - kDelta, t1 + o1, tb_nominal);
- const double plus_costa =
- filter.Cost(t1, ta_nominal + kDelta, t1 + o1, tb_nominal);
-
- const double minus_costb =
- filter.Cost(t1, ta_nominal, t1 + o1, tb_nominal - kDelta);
- const double plus_costb =
- filter.Cost(t1, ta_nominal, t1 + o1, tb_nominal + kDelta);
-
- EXPECT_NEAR((plus_costa - minus_costa) / (2.0 * kDelta),
- filter.DCostDta(t1, ta_nominal, t1 + o1, tb_nominal), 1e-9);
- EXPECT_NEAR((plus_costb - minus_costb) / (2.0 * kDelta),
- filter.DCostDtb(t1, ta_nominal, t1 + o1, tb_nominal), 1e-9);
- }
-
- {
- const double minus_costa =
- filter.Cost(t2, ta_nominal - kDelta, t2 + o2, tb_nominal);
- const double plus_costa =
- filter.Cost(t2, ta_nominal + kDelta, t2 + o2, tb_nominal);
-
- const double minus_costb =
- filter.Cost(t2, ta_nominal, t2 + o2, tb_nominal - kDelta);
- const double plus_costb =
- filter.Cost(t2, ta_nominal, t2 + o2, tb_nominal + kDelta);
-
- EXPECT_NEAR((plus_costa - minus_costa) / (2.0 * kDelta),
- filter.DCostDta(t2, ta_nominal, t2 + o2, tb_nominal), 1e-9);
- EXPECT_NEAR((plus_costb - minus_costb) / (2.0 * kDelta),
- filter.DCostDtb(t2, ta_nominal, t2 + o2, tb_nominal), 1e-9);
- }
-
- {
- const double minus_costa =
- filter.Cost(t3, ta_nominal - kDelta, t3 + o3, tb_nominal);
- const double plus_costa =
- filter.Cost(t3, ta_nominal + kDelta, t3 + o3, tb_nominal);
-
- const double minus_costb =
- filter.Cost(t3, ta_nominal, t3 + o3, tb_nominal - kDelta);
- const double plus_costb =
- filter.Cost(t3, ta_nominal, t3 + o3, tb_nominal + kDelta);
-
- EXPECT_NEAR((plus_costa - minus_costa) / (2.0 * kDelta),
- filter.DCostDta(t3, ta_nominal, t3 + o3, tb_nominal), 1e-9);
- EXPECT_NEAR((plus_costb - minus_costb) / (2.0 * kDelta),
- filter.DCostDtb(t3, ta_nominal, t3 + o3, tb_nominal), 1e-9);
- }
- }
- }
-}
-
// Run a couple of points through the estimator and confirm it works.
TEST(NoncausalOffsetEstimatorTest, FullEstimator) {
const aos::FlatbufferDetachedBuffer<Node> node_a_buffer =