Added a time-limited optimization option to the Graph class

[svwilliams]

Added a time-limited optimization option to the Graph class

[efernandez]

@svwilliams I know I'm a bit late here, but I'd like to remember the max_solver_time_in_seconds can be set with a ROS parameter. This optimizeFor method would override that value, and also include the time spent creating the problem. That sounds like a good idea.

I believe this could be used in place of optimize and in that case pass the max_optimization_time from a ROS parameter. I wonder if that'd be a new parameter, or it'd be based on the max_solver_time_in_seconds one used for the solver options. 🤔

[svwilliams]

I'm not sure I completely follow the request/suggestion. Are you saying that optimize() and optimizeFor() get collapsed into a single call. Something like:

ceres::Solver::Summary optimize(const ceres::Solver::Options& options = ceres::Solver::Options(), const ros::Duration& max_optimization_time = ros::DURATION_MAX)
{
  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();

  auto time_constrained_options = options;
  auto max_solver_time = std::min(options.max_solver_time_in_seconds, max_optimization_time.toSec());
  if (max_solver_time < std::numeric_limit<double>::max())
  {
    auto remaining = max_solver_time - (created_problem - start).toSec();
    time_constrained_options.max_solver_time_in_seconds = std::max(0.0, remaining);
  }

  // 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;
}

[efernandez]

Sorry I wasn't very clear.

I wonder if now that we have optimize and optimizeFor, which one should be used, or which one is the preferred one?

For example, in

fuse/fuse_optimizers/src/fixed_lag_smoother.cpp

Line 224 in 82c284e

summary_ = graph_->optimize(params_.solver_options);

optimize is still being used. This means that the max_solver_time_in_seconds ROS parameter is still used to set the corresponding Ceres option.

If we switched it to use optimizeFor, we could either have another parameter, but then it'd somehow be related with the one for the Ceres option, mentioned above.

A possible implementation would be the one in your comment, but I still wonder what would be done in the caller side. 🤔