Merge "Return monotonic times from the solver directly"
diff --git a/aos/network/multinode_timestamp_filter.cc b/aos/network/multinode_timestamp_filter.cc
index 6a63b0c..cfa74d4 100644
--- a/aos/network/multinode_timestamp_filter.cc
+++ b/aos/network/multinode_timestamp_filter.cc
@@ -76,7 +76,7 @@
// ms/s. Figure out how to define it. Do this last. This lets us handle
// constraints going away, and constraints close in time.
-std::vector<double> TimestampProblem::Solve() {
+std::vector<double> TimestampProblem::SolveDouble() {
// 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
@@ -86,13 +86,37 @@
// Ask for really good. This is very quadratic, so it should be pretty
// precise.
- nlopt_set_xtol_rel(opt, 1e-19);
+ nlopt_set_xtol_rel(opt, 1e-5);
count_ = 0;
std::vector<double> result(n, 0.0);
double minf = 0.0;
- CHECK_GE(nlopt_optimize(opt, result.data(), &minf), 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());
+ 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)) {
std::vector<double> gradient(n, 0.0);
@@ -110,12 +134,29 @@
VLOG(1) << std::setprecision(12) << std::fixed << "Found minimum at f("
<< result[0] << ") -> " << minf << " grad ["
<< absl::StrJoin(gradient, ", ", MyFormatter()) << "] after "
- << count_ << " cycles.";
+ << count_ << " cycles for node " << solution_node_ << ".";
}
nlopt_destroy(opt);
return result;
}
+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) {
+ result[i] =
+ base_clock(i) +
+ std::chrono::nanoseconds(static_cast<int64_t>(std::floor(get_t(r, i))));
+ }
+
+ return result;
+}
+
+std::vector<monotonic_clock::time_point> TimestampProblem::Solve() {
+ std::vector<double> solution = SolveDouble();
+ return DoubleToMonotonic(solution.data());
+}
+
double TimestampProblem::Cost(const double *x, double *grad) {
++count_;
@@ -148,6 +189,37 @@
get_t(x, filter.b_index));
}
}
+
+ if (VLOG_IS_ON(1)) {
+ 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, filters_.size() - 1u),
+ ", ", MyFormatter())
+ << "]";
+ gradient = ss.str();
+ }
+
+ LOG(INFO) << std::setprecision(12) << std::fixed
+ << "Evaluated minimum at f("
+ << absl::StrJoin(DoubleToMonotonic(x), ", ", MyFormatter())
+ << ") -> " << cost << gradient << " after " << count_
+ << " cycles.";
+ }
return cost;
}
diff --git a/aos/network/multinode_timestamp_filter.h b/aos/network/multinode_timestamp_filter.h
index e9c1d08..b4f20ff 100644
--- a/aos/network/multinode_timestamp_filter.h
+++ b/aos/network/multinode_timestamp_filter.h
@@ -57,7 +57,10 @@
// Solves the optimization problem phrased and returns the offsets from the
// base clock for each node, excluding the solution node.
- std::vector<double> Solve();
+ std::vector<double> SolveDouble();
+ // Solves the optimization problem phrased and returns the optimal time on
+ // each node.
+ std::vector<monotonic_clock::time_point> Solve();
// Returns the squared error for all of the offsets.
// x is the offsets from the base_clock for every node (in order) except the
@@ -68,11 +71,16 @@
double Cost(const double *x, double *grad);
// Returns the time offset from base for a node.
- double get_t(const double *x, size_t time_index) {
+ double get_t(const double *x, size_t time_index) const {
return time_index == solution_node_ ? 0.0
- : x[NodeToSolutionIndex(time_index)];
+ : x[NodeToSolutionIndex(time_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;
+
// LOGs a representation of the problem.
void Debug();
@@ -87,7 +95,7 @@
}
// Converts from a node index to an index in the solution.
- size_t NodeToSolutionIndex(size_t node_index) {
+ size_t NodeToSolutionIndex(size_t node_index) const {
// 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.
diff --git a/aos/network/multinode_timestamp_filter_test.cc b/aos/network/multinode_timestamp_filter_test.cc
index 69f76c5..c94317a 100644
--- a/aos/network/multinode_timestamp_filter_test.cc
+++ b/aos/network/multinode_timestamp_filter_test.cc
@@ -77,7 +77,7 @@
// Solve the problem with infinate precision as a verification and compare the
// result.
{
- const std::vector<double> result = problem.Solve();
+ const std::vector<double> result = problem.SolveDouble();
mpq_class tb_mpq =
SolveExact(a.FitLine(), b.FitLine(), problem.base_clock(0));
@@ -89,7 +89,7 @@
// Solve some other timestamps for grins.
{
problem.set_base_clock(0, e + chrono::milliseconds(500));
- std::vector<double> result = problem.Solve();
+ std::vector<double> result = problem.SolveDouble();
mpq_class tb_mpq =
SolveExact(a.FitLine(), b.FitLine(), problem.base_clock(0));
@@ -110,7 +110,7 @@
b.Sample(e + chrono::milliseconds(4000), -chrono::milliseconds(1002));
{
problem.set_base_clock(0, e + chrono::milliseconds(1500));
- const std::vector<double> result = problem.Solve();
+ const std::vector<double> result = problem.SolveDouble();
mpq_class tb_mpq =
SolveExact(a.FitLine(), b.FitLine(), problem.base_clock(0));
@@ -122,7 +122,7 @@
{
problem.set_base_clock(0, e + chrono::milliseconds(1600));
- const std::vector<double> result = problem.Solve();
+ const std::vector<double> result = problem.SolveDouble();
mpq_class tb_mpq =
SolveExact(a.FitLine(), b.FitLine(), problem.base_clock(0));