Added a time-limited optimization option to the Graph class (#234)

fuse_core/include/fuse_core/graph.h

@@ -364,6 +364,22 @@ class Graph
    */
   virtual ceres::Solver::Summary optimize(const ceres::Solver::Options& options = ceres::Solver::Options()) = 0;
 
+  /**
+   * @brief Optimize the values of the current set of variables, given the current set of constraints for a maximum
+   * amount of time.
+   *
+   * The \p max_optimization_time should be viewed as a "best effort" limit, and the actual optimization time may
+   * exceed this limit by a small amount. After the call, the values in the graph will be updated to the latest values.
+   *
+   * @param[in] max_optimization_time The maximum allowed duration of the optimization call
+   * @param[in] options An optional Ceres Solver::Options object that controls various aspects of the optimizer.
+   *                    See https://ceres-solver.googlesource.com/ceres-solver/+/master/include/ceres/solver.h#59
+   * @return            A Ceres Solver Summary structure containing information about the optimization process
+   */
+  virtual ceres::Solver::Summary optimizeFor(
+    const ros::Duration& max_optimization_time,
+    const ceres::Solver::Options& options = ceres::Solver::Options()) = 0;
+
   /**
    * @brief Evalute the values of the current set of variables, given the current set of constraints.
    *

fuse_graphs/include/fuse_graphs/hash_graph.h

@@ -316,6 +316,22 @@ class HashGraph : public fuse_core::Graph
    */
   ceres::Solver::Summary optimize(const ceres::Solver::Options& options = ceres::Solver::Options()) override;
 
+  /**
+   * @brief Optimize the values of the current set of variables, given the current set of constraints for a maximum
+   * amount of time.
+   *
+   * The \p max_optimization_time should be viewed as a "best effort" limit, and the actual optimization time may
+   * exceed this limit by a small amount. After the call, the values in the graph will be updated to the latest values.
+   *
+   * @param[in] max_optimization_time The maximum allowed duration of the optimization call
+   * @param[in] options An optional Ceres Solver::Options object that controls various aspects of the optimizer.
+   *                    See https://ceres-solver.googlesource.com/ceres-solver/+/master/include/ceres/solver.h#59
+   * @return            A Ceres Solver Summary structure containing information about the optimization process
+   */
+  ceres::Solver::Summary optimizeFor(
+    const ros::Duration& max_optimization_time,
+    const ceres::Solver::Options& options = ceres::Solver::Options()) override;
+
   /**
    * @brief Evalute the values of the current set of variables, given the current set of constraints.
    *

fuse_graphs/src/hash_graph.cpp

@@ -420,6 +420,26 @@ ceres::Solver::Summary HashGraph::optimize(const ceres::Solver::Options& options
   return summary;
 }
 
+ceres::Solver::Summary HashGraph::optimizeFor(
+  const ros::Duration& max_optimization_time,
+  const ceres::Solver::Options& options)
+{
+  auto start = ros::Time::now();
+  // Construct the ceres::Problem object from scratch
+  ceres::Problem problem(problem_options_);
+  createProblem(problem);
+  auto created_problem = ros::Time::now();
+  // Modify the options to enforce the maximum time
+  auto remaining = max_optimization_time - (created_problem - start);
+  auto time_constrained_options = options;
+  time_constrained_options.max_solver_time_in_seconds = std::max(0.0, remaining.toSec());
+  // Run the solver. This will update the variables in place.
+  ceres::Solver::Summary summary;
+  ceres::Solve(time_constrained_options, &problem, &summary);
+  // Return the optimization summary
+  return summary;
+}
+
 bool HashGraph::evaluate(double* cost, std::vector<double>* residuals, std::vector<double>* gradient,
                          const ceres::Problem::EvaluateOptions& options) const
 {