gradient_problem_solver.cc 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298
  1. // Ceres Solver - A fast non-linear least squares minimizer
  2. // Copyright 2023 Google Inc. All rights reserved.
  3. // http://ceres-solver.org/
  4. //
  5. // Redistribution and use in source and binary forms, with or without
  6. // modification, are permitted provided that the following conditions are met:
  7. //
  8. // * Redistributions of source code must retain the above copyright notice,
  9. // this list of conditions and the following disclaimer.
  10. // * Redistributions in binary form must reproduce the above copyright notice,
  11. // this list of conditions and the following disclaimer in the documentation
  12. // and/or other materials provided with the distribution.
  13. // * Neither the name of Google Inc. nor the names of its contributors may be
  14. // used to endorse or promote products derived from this software without
  15. // specific prior written permission.
  16. //
  17. // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  18. // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19. // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20. // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  21. // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  22. // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  23. // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  24. // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  25. // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  26. // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  27. // POSSIBILITY OF SUCH DAMAGE.
  28. //
  29. // Author: sameeragarwal@google.com (Sameer Agarwal)
  30. #include "ceres/gradient_problem_solver.h"
  31. #include <map>
  32. #include <memory>
  33. #include <string>
  34. #include "ceres/callbacks.h"
  35. #include "ceres/gradient_problem.h"
  36. #include "ceres/gradient_problem_evaluator.h"
  37. #include "ceres/internal/eigen.h"
  38. #include "ceres/internal/export.h"
  39. #include "ceres/map_util.h"
  40. #include "ceres/minimizer.h"
  41. #include "ceres/solver.h"
  42. #include "ceres/solver_utils.h"
  43. #include "ceres/stringprintf.h"
  44. #include "ceres/types.h"
  45. #include "ceres/wall_time.h"
  46. namespace ceres {
  47. using internal::StringAppendF;
  48. using internal::StringPrintf;
  49. namespace {
  50. Solver::Options GradientProblemSolverOptionsToSolverOptions(
  51. const GradientProblemSolver::Options& options) {
  52. #define COPY_OPTION(x) solver_options.x = options.x
  53. Solver::Options solver_options;
  54. solver_options.minimizer_type = LINE_SEARCH;
  55. COPY_OPTION(line_search_direction_type);
  56. COPY_OPTION(line_search_type);
  57. COPY_OPTION(nonlinear_conjugate_gradient_type);
  58. COPY_OPTION(max_lbfgs_rank);
  59. COPY_OPTION(use_approximate_eigenvalue_bfgs_scaling);
  60. COPY_OPTION(line_search_interpolation_type);
  61. COPY_OPTION(min_line_search_step_size);
  62. COPY_OPTION(line_search_sufficient_function_decrease);
  63. COPY_OPTION(max_line_search_step_contraction);
  64. COPY_OPTION(min_line_search_step_contraction);
  65. COPY_OPTION(max_num_line_search_step_size_iterations);
  66. COPY_OPTION(max_num_line_search_direction_restarts);
  67. COPY_OPTION(line_search_sufficient_curvature_decrease);
  68. COPY_OPTION(max_line_search_step_expansion);
  69. COPY_OPTION(max_num_iterations);
  70. COPY_OPTION(max_solver_time_in_seconds);
  71. COPY_OPTION(parameter_tolerance);
  72. COPY_OPTION(function_tolerance);
  73. COPY_OPTION(gradient_tolerance);
  74. COPY_OPTION(logging_type);
  75. COPY_OPTION(minimizer_progress_to_stdout);
  76. COPY_OPTION(callbacks);
  77. return solver_options;
  78. #undef COPY_OPTION
  79. }
  80. } // namespace
  81. bool GradientProblemSolver::Options::IsValid(std::string* error) const {
  82. const Solver::Options solver_options =
  83. GradientProblemSolverOptionsToSolverOptions(*this);
  84. return solver_options.IsValid(error);
  85. }
  86. GradientProblemSolver::~GradientProblemSolver() = default;
  87. void GradientProblemSolver::Solve(const GradientProblemSolver::Options& options,
  88. const GradientProblem& problem,
  89. double* parameters_ptr,
  90. GradientProblemSolver::Summary* summary) {
  91. using internal::CallStatistics;
  92. using internal::GradientProblemEvaluator;
  93. using internal::GradientProblemSolverStateUpdatingCallback;
  94. using internal::LoggingCallback;
  95. using internal::Minimizer;
  96. using internal::SetSummaryFinalCost;
  97. using internal::WallTimeInSeconds;
  98. double start_time = WallTimeInSeconds();
  99. CHECK(summary != nullptr);
  100. *summary = Summary();
  101. // clang-format off
  102. summary->num_parameters = problem.NumParameters();
  103. summary->num_tangent_parameters = problem.NumTangentParameters();
  104. summary->line_search_direction_type = options.line_search_direction_type; // NOLINT
  105. summary->line_search_interpolation_type = options.line_search_interpolation_type; // NOLINT
  106. summary->line_search_type = options.line_search_type;
  107. summary->max_lbfgs_rank = options.max_lbfgs_rank;
  108. summary->nonlinear_conjugate_gradient_type = options.nonlinear_conjugate_gradient_type; // NOLINT
  109. // clang-format on
  110. // Check validity
  111. if (!options.IsValid(&summary->message)) {
  112. LOG(ERROR) << "Terminating: " << summary->message;
  113. return;
  114. }
  115. VectorRef parameters(parameters_ptr, problem.NumParameters());
  116. Vector solution(problem.NumParameters());
  117. solution = parameters;
  118. // TODO(sameeragarwal): This is a bit convoluted, we should be able
  119. // to convert to minimizer options directly, but this will do for
  120. // now.
  121. Minimizer::Options minimizer_options =
  122. Minimizer::Options(GradientProblemSolverOptionsToSolverOptions(options));
  123. minimizer_options.evaluator =
  124. std::make_unique<GradientProblemEvaluator>(problem);
  125. std::unique_ptr<IterationCallback> logging_callback;
  126. if (options.logging_type != SILENT) {
  127. logging_callback = std::make_unique<LoggingCallback>(
  128. LINE_SEARCH, options.minimizer_progress_to_stdout);
  129. minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(),
  130. logging_callback.get());
  131. }
  132. std::unique_ptr<IterationCallback> state_updating_callback;
  133. if (options.update_state_every_iteration) {
  134. state_updating_callback =
  135. std::make_unique<GradientProblemSolverStateUpdatingCallback>(
  136. problem.NumParameters(), solution.data(), parameters_ptr);
  137. minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(),
  138. state_updating_callback.get());
  139. }
  140. std::unique_ptr<Minimizer> minimizer(Minimizer::Create(LINE_SEARCH));
  141. Solver::Summary solver_summary;
  142. solver_summary.fixed_cost = 0.0;
  143. solver_summary.preprocessor_time_in_seconds = 0.0;
  144. solver_summary.postprocessor_time_in_seconds = 0.0;
  145. solver_summary.line_search_polynomial_minimization_time_in_seconds = 0.0;
  146. minimizer->Minimize(minimizer_options, solution.data(), &solver_summary);
  147. // clang-format off
  148. summary->termination_type = solver_summary.termination_type;
  149. summary->message = solver_summary.message;
  150. summary->initial_cost = solver_summary.initial_cost;
  151. summary->final_cost = solver_summary.final_cost;
  152. summary->iterations = solver_summary.iterations;
  153. // clang-format on
  154. summary->line_search_polynomial_minimization_time_in_seconds =
  155. solver_summary.line_search_polynomial_minimization_time_in_seconds;
  156. if (summary->IsSolutionUsable()) {
  157. parameters = solution;
  158. SetSummaryFinalCost(summary);
  159. }
  160. const std::map<std::string, CallStatistics>& evaluator_statistics =
  161. minimizer_options.evaluator->Statistics();
  162. {
  163. const CallStatistics& call_stats = FindWithDefault(
  164. evaluator_statistics, "Evaluator::Residual", CallStatistics());
  165. summary->cost_evaluation_time_in_seconds = call_stats.time;
  166. summary->num_cost_evaluations = call_stats.calls;
  167. }
  168. {
  169. const CallStatistics& call_stats = FindWithDefault(
  170. evaluator_statistics, "Evaluator::Jacobian", CallStatistics());
  171. summary->gradient_evaluation_time_in_seconds = call_stats.time;
  172. summary->num_gradient_evaluations = call_stats.calls;
  173. }
  174. summary->total_time_in_seconds = WallTimeInSeconds() - start_time;
  175. }
  176. bool GradientProblemSolver::Summary::IsSolutionUsable() const {
  177. return internal::IsSolutionUsable(*this);
  178. }
  179. std::string GradientProblemSolver::Summary::BriefReport() const {
  180. return StringPrintf(
  181. "Ceres GradientProblemSolver Report: "
  182. "Iterations: %d, "
  183. "Initial cost: %e, "
  184. "Final cost: %e, "
  185. "Termination: %s",
  186. static_cast<int>(iterations.size()),
  187. initial_cost,
  188. final_cost,
  189. TerminationTypeToString(termination_type));
  190. }
  191. std::string GradientProblemSolver::Summary::FullReport() const {
  192. using internal::VersionString;
  193. // NOTE operator+ is not usable for concatenating a string and a string_view.
  194. std::string report =
  195. std::string{"\nSolver Summary (v "}.append(VersionString()) + ")\n\n";
  196. StringAppendF(&report, "Parameters % 25d\n", num_parameters);
  197. if (num_tangent_parameters != num_parameters) {
  198. StringAppendF(
  199. &report, "Tangent parameters % 25d\n", num_tangent_parameters);
  200. }
  201. std::string line_search_direction_string;
  202. if (line_search_direction_type == LBFGS) {
  203. line_search_direction_string = StringPrintf("LBFGS (%d)", max_lbfgs_rank);
  204. } else if (line_search_direction_type == NONLINEAR_CONJUGATE_GRADIENT) {
  205. line_search_direction_string = NonlinearConjugateGradientTypeToString(
  206. nonlinear_conjugate_gradient_type);
  207. } else {
  208. line_search_direction_string =
  209. LineSearchDirectionTypeToString(line_search_direction_type);
  210. }
  211. StringAppendF(&report,
  212. "Line search direction %19s\n",
  213. line_search_direction_string.c_str());
  214. const std::string line_search_type_string = StringPrintf(
  215. "%s %s",
  216. LineSearchInterpolationTypeToString(line_search_interpolation_type),
  217. LineSearchTypeToString(line_search_type));
  218. StringAppendF(&report,
  219. "Line search type %19s\n",
  220. line_search_type_string.c_str());
  221. StringAppendF(&report, "\n");
  222. StringAppendF(&report, "\nCost:\n");
  223. StringAppendF(&report, "Initial % 30e\n", initial_cost);
  224. if (termination_type != FAILURE && termination_type != USER_FAILURE) {
  225. StringAppendF(&report, "Final % 30e\n", final_cost);
  226. StringAppendF(&report, "Change % 30e\n", initial_cost - final_cost);
  227. }
  228. StringAppendF(&report,
  229. "\nMinimizer iterations % 16d\n",
  230. static_cast<int>(iterations.size()));
  231. StringAppendF(&report, "\nTime (in seconds):\n");
  232. StringAppendF(&report,
  233. "\n Cost evaluation %23.6f (%d)\n",
  234. cost_evaluation_time_in_seconds,
  235. num_cost_evaluations);
  236. StringAppendF(&report,
  237. " Gradient & cost evaluation %16.6f (%d)\n",
  238. gradient_evaluation_time_in_seconds,
  239. num_gradient_evaluations);
  240. StringAppendF(&report,
  241. " Polynomial minimization %17.6f\n",
  242. line_search_polynomial_minimization_time_in_seconds);
  243. StringAppendF(
  244. &report, "Total %25.6f\n\n", total_time_in_seconds);
  245. StringAppendF(&report,
  246. "Termination: %25s (%s)\n",
  247. TerminationTypeToString(termination_type),
  248. message.c_str());
  249. return report;
  250. }
  251. void Solve(const GradientProblemSolver::Options& options,
  252. const GradientProblem& problem,
  253. double* parameters,
  254. GradientProblemSolver::Summary* summary) {
  255. GradientProblemSolver solver;
  256. solver.Solve(options, problem, parameters, summary);
  257. }
  258. } // namespace ceres