Add Debug method to print the time optimization problem
This helps a lot in seeing what is being solved and understanding why
something is hard to solve.
Change-Id: I8d1f0b09bfc699811518d744673d431c61c3d148
diff --git a/aos/network/multinode_timestamp_filter.cc b/aos/network/multinode_timestamp_filter.cc
index 92d729c..5a617da 100644
--- a/aos/network/multinode_timestamp_filter.cc
+++ b/aos/network/multinode_timestamp_filter.cc
@@ -149,6 +149,50 @@
return cost;
}
+void TimestampProblem::Debug() {
+ 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_) {
+ gradients[NodeToSolutionIndex(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_) {
+ gradients[NodeToSolutionIndex(filter.b_index)].emplace_back(
+ filter.filter->DebugDCostDtb(base_clock_[i], 0.0,
+ base_clock_[filter.b_index], 0.0, i,
+ filter.b_index));
+ }
+ }
+ }
+
+ for (size_t i = 0u; i < filters_.size(); ++i) {
+ LOG(INFO) << "Grad[" << i << "] = "
+ << (gradients[i].empty() ? std::string("0.0")
+ : absl::StrJoin(gradients[i], " + "));
+ }
+
+ for (size_t i = 0u; i < filters_.size(); ++i) {
+ LOG(INFO) << "base_clock[" << i << "] = " << base_clock_[i];
+ }
+}
+
void MultiNodeNoncausalOffsetEstimator::Start(
SimulatedEventLoopFactory *factory) {
for (std::pair<const std::tuple<const Node *, const Node *>,
diff --git a/aos/network/multinode_timestamp_filter.h b/aos/network/multinode_timestamp_filter.h
index 64e0125..3e00d87 100644
--- a/aos/network/multinode_timestamp_filter.h
+++ b/aos/network/multinode_timestamp_filter.h
@@ -72,6 +72,9 @@
: x[NodeToSolutionIndex(time_index)];
}
+ // LOGs a representation of the problem.
+ void Debug();
+
private:
// Static trampoline for nlopt. n is the number of constraints, x is input
// solution to solve for, grad is the gradient to fill out (if not nullptr),
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index da1125c..69aee34 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -5,6 +5,7 @@
#include <tuple>
#include "absl/strings/str_cat.h"
+#include "absl/strings/str_format.h"
#include "aos/configuration.h"
#include "aos/time/time.h"
#include "third_party/gmp/gmpxx.h"
@@ -805,6 +806,49 @@
(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 {
+ if (timestamps_size() == 1u) {
+ return absl::StrFormat("-2. * (t%d - t%d - %d)", node_b, node_a,
+ std::get<1>(timestamp(0)).count());
+ }
+
+ NormalizeTimestamps(&ta_base, &ta);
+ NormalizeTimestamps(&tb_base, &tb);
+
+ std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
+ points = FindTimestamps(ta_base, ta);
+
+ // 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 {
@@ -821,6 +865,88 @@
return 2.0 * OffsetError(ta_base, ta, tb_base, tb);
}
+std::string NoncausalTimestampFilter::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 {
+ if (timestamps_size() == 1u) {
+ return absl::StrFormat("2. * (t%d - t%d - %d)", node_b, node_a,
+ std::get<1>(timestamp(0)).count());
+ }
+
+ NormalizeTimestamps(&ta_base, &ta);
+ NormalizeTimestamps(&tb_base, &tb);
+
+ std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
+ points = FindTimestamps(ta_base, ta);
+
+ // 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("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()));
+ }
+
+ return absl::StrFormat("2. * (t%d - t%d - (t%d - %d.) * %d. / %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());
+}
+
+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 {
+ if (timestamps_size() == 1u) {
+ return absl::StrFormat("(t%d - t%d - %d) ** 2.", node_b, node_a,
+ std::get<1>(timestamp(0)).count());
+ }
+
+ NormalizeTimestamps(&ta_base, &ta);
+ NormalizeTimestamps(&tb_base, &tb);
+
+ std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
+ points = FindTimestamps(ta_base, ta);
+
+ // 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());
+ }
+}
+
bool NoncausalTimestampFilter::Sample(
aos::monotonic_clock::time_point monotonic_now,
chrono::nanoseconds sample_ns) {
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index ccbf334..3771315 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -366,13 +366,22 @@
// 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;
double Convert(double ta) const {
return ta +