15#include "tinyopt/math.h"
16#include "tinyopt/stop_reasons.h"
18#include <tinyopt/log.h>
19#include <tinyopt/summary.h>
20#include <tinyopt/time.h>
21#include <tinyopt/traits.h>
23#include <tinyopt/optimizers/options.h>
25#ifndef TINYOPT_DISABLE_AUTODIFF
26#include <tinyopt/diff/optimize_autodiff.h>
28#ifndef TINYOPT_DISABLE_NUMDIFF
29#include <tinyopt/diff/num_diff.h>
36#if defined(TINYOPT_ENFORCE_NO_DYNAMIC_ALLOCATIONS)
37class EigenMallocGuard {
39 explicit EigenMallocGuard(
bool active)
40 : active_(active), previous_(Eigen::internal::is_malloc_allowed()) {
41 if (active_) Eigen::internal::set_is_malloc_allowed(
false);
45 if (active_) Eigen::internal::set_is_malloc_allowed(previous_);
54template <
typename Func,
typename... Params>
58 template <
typename Pack>
59 decltype(
auto)
operator()(
const Pack &pack)
const {
60 return std::apply(func, pack.values);
64template <
typename Func,
typename... Params>
68 template <
typename Pack,
typename Gradient,
typename Hessian>
69 requires std::is_invocable_v<
const Func &,
const Params &..., Gradient &, Hessian &>
70 decltype(
auto)
operator()(
const Pack &pack, Gradient &gradient, Hessian &hessian)
const {
72 [&](
const auto &...params) {
return std::invoke(func, params..., gradient, hessian); },
76 template <
typename Pack,
typename Gradient>
77 requires std::is_invocable_v<
const Func &,
const Params &..., Gradient &>
78 decltype(
auto)
operator()(
const Pack &pack, Gradient &gradient)
const {
79 return std::apply([&](
const auto &...params) {
return std::invoke(func, params..., gradient); },
84template <
typename Func,
typename... Params>
85auto make_packed_adapter(
const Func &func) {
86 if constexpr (std::is_invocable_v<
const Func &,
const Params &...>)
89 return PackedAccumulationAdapter<Func, Params...>{func};
97template <
typename SolverType>
100 using Scalar =
typename SolverType::Scalar;
101 static constexpr Index Dims = SolverType::Dims;
105 Optimizer_(
const Options &_options = {})
109 template <
int FO = SolverType::FirstOrder, std::enable_if_t<!FO,
int> = 0>
115 template <
int FO = SolverType::FirstOrder, std::enable_if_t<FO,
int> = 0>
123 template <
typename X_t>
124 std::variant<StopReason, bool> ResizeIfNeeded(X_t &x) {
125 const Index dims = traits::DynDims(x);
126 if (Dims == Dynamic && dims == 0) {
128 "❌ Error: Parameters dimensions cannot be 0 or Dynamic at "
130 return StopReason::kSkipped;
131 }
else if (dims < 0) {
132 TINYOPT_LOG(
"❌ Error: Parameters dimensions is negative: {} ", dims);
133 return StopReason::kSkipped;
137 bool resized =
false;
139 resized =
solver_.resize(dims);
140 }
catch (
const std::bad_alloc &) {
142 int num_hessians = 1;
145 "❌ Failed to allocate {} Hessian(s) of size {}x{}, "
146 "mem:{}GB, maybe use a SparseMatrix?",
147 num_hessians, dims, dims, 1e-9f *
static_cast<float>(dims * dims *
sizeof(Scalar)));
149 return StopReason::kOutOfMemory;
150 }
catch (
const std::invalid_argument &e) {
151 TINYOPT_LOG(
"❌ Error: Failed to resize the linear solver. {}", e.what());
152 return StopReason::kSkipped;
208 template <
typename T,
typename U,
typename... Rest,
typename Func>
209 requires(!std::is_same_v<std::remove_cvref_t<Func>, Options>)
212 auto wrapped = detail::make_packed_adapter<Func, T, U, Rest...>(cost_or_acc);
213 return OptimizeSingle(pack, wrapped);
217 template <
typename X_t,
typename CostOrAccFunc>
218 Summary OptimizeSingle(X_t &x,
const CostOrAccFunc &cost_or_acc,
int max_iters = -1) {
219#if defined(TINYOPT_ENFORCE_NO_DYNAMIC_ALLOCATIONS)
220 detail::EigenMallocGuard eigen_malloc_guard(Dims != Dynamic);
223 if constexpr (std::is_invocable_v<CostOrAccFunc, const X_t &>) {
225#ifndef TINYOPT_DISABLE_AUTODIFF
226 using Jet = diff::Jet<Scalar, Dims>;
228 std::conditional_t<std::is_floating_point_v<X_t>, Jet,
230 if constexpr (std::is_invocable_v<CostOrAccFunc, const XJetType &>) {
231 using ResType = std::invoke_result_t<CostOrAccFunc, const XJetType &>;
232 if constexpr (traits::is_tuple_v<ResType>) {
233 const auto optimize = [&](
auto &x,
const auto &func,
const auto &) {
236 constexpr bool kIsNLLS = SolverType::IsNLLS;
237 return tinyopt::OptimizeWithAutoDiff<kIsNLLS>(x, cost_or_acc, optimize,
options_);
238 }
else if constexpr (traits::is_jet_type_v<ResType>) {
239 const auto optimize = [&](
auto &x,
const auto &func,
const auto &) {
242 constexpr bool kIsNLLS = SolverType::IsNLLS;
243 return tinyopt::OptimizeWithAutoDiff<kIsNLLS>(x, cost_or_acc, optimize,
options_);
244 }
else if constexpr (traits::is_matrix_or_array_v<ResType>) {
245 static_assert(traits::is_jet_type_v<typename ResType::Scalar>,
246 "Matrix residuals in autodiff must be Jet-valued");
247 const auto optimize = [&](
auto &x,
const auto &func,
const auto &) {
250 constexpr bool kIsNLLS = SolverType::IsNLLS;
251 return tinyopt::OptimizeWithAutoDiff<kIsNLLS>(x, cost_or_acc, optimize,
options_);
254 sum.num_diff_used =
true;
255 if constexpr (SolverType::FirstOrder) {
256 auto loss = diff::CreateNumDiffFunc1(x, cost_or_acc);
259 auto loss = diff::CreateNumDiffFunc2(x, cost_or_acc);
266 sum.num_diff_used =
true;
267 if constexpr (SolverType::FirstOrder) {
268 auto loss = diff::CreateNumDiffFunc1(x, cost_or_acc);
271 auto loss = diff::CreateNumDiffFunc2(x, cost_or_acc);
277#ifndef TINYOPT_DISABLE_NUMDIFF
279 sum.num_diff_used =
true;
280 if constexpr (SolverType::FirstOrder) {
281 auto loss = diff::CreateNumDiffFunc1(x, cost_or_acc);
284 auto loss = diff::CreateNumDiffFunc2(x, cost_or_acc);
289 throw std::invalid_argument(
290 "Automatic and numerical differentiation are disabled for cost-only functions");
299 template <
typename X_t,
typename CostOrAccFunc>
300 Summary
Optimize(X_t &x,
const CostOrAccFunc &cost_or_acc,
int max_iters = -1) {
301 return OptimizeSingle(x, cost_or_acc, max_iters);
309 template <
typename T,
typename U,
typename... Rest,
typename Func>
310 requires(!std::is_same_v<std::remove_cvref_t<Func>, tinyopt::Options>)
311 Summary operator()(T &x, U &y, Rest &...rest,
const Func &cost_or_acc,
int max_iters = -1) {
313 auto wrapped = detail::make_packed_adapter<Func, T, U, Rest...>(cost_or_acc);
314 return OptimizeSingle(pack, wrapped, max_iters);
317 template <
typename X_t,
typename CostOrAccFunc>
318 Summary operator()(X_t &x,
const CostOrAccFunc &cost_or_acc,
int max_iters = -1) {
319 return OptimizeSingle(x, cost_or_acc, max_iters);
361 template <
typename X_t,
typename AccFunc>
363#if defined(TINYOPT_ENFORCE_NO_DYNAMIC_ALLOCATIONS)
364 detail::EigenMallocGuard eigen_malloc_guard(Dims != Dynamic);
374#if !defined(TINYOPT_ENFORCE_NO_DYNAMIC_ALLOCATIONS)
375 sum.hist.
errs.reserve(max_iters + 1);
376 sum.hist.
deltas2.reserve(max_iters + 1);
377 sum.hist.
successes.reserve(max_iters + 1);
381 constexpr bool kNoCopyX =
true;
382 using BestXType = std::conditional<kNoCopyX, std::nullptr_t, X_t>;
383 BestXType *best_x =
nullptr;
384 if constexpr (!kNoCopyX) best_x =
new X_t(x);
386 std::optional<Vector<Scalar, Dims>> last_dx;
387 bool last_was_success =
true;
390 const auto notify_step = [&](
const Vector<Scalar, Dims> &step,
bool is_rollback) {
398 for (
int iter = 0; iter < max_iters; ++iter) {
399 const auto t = tic();
400 const auto &[success, maybe_dx] =
Step(x, acc, sum);
401 bool eval_only =
false;
405 ptrait::PlusEq(x, maybe_dx.value());
406 notify_step(maybe_dx.value(),
false);
407 last_dx = maybe_dx.value();
408 last_was_success =
true;
417 if constexpr (kNoCopyX)
418 ptrait::PlusEq(x, -last_dx.value());
421 notify_step(-last_dx.value(),
true);
423 }
else if (maybe_dx) {
425 ptrait::PlusEq(x, maybe_dx.value());
426 notify_step(maybe_dx.value(),
false);
427 last_dx = maybe_dx.value();
430 eval_only = last_was_success ==
false;
431 last_was_success =
false;
448 if constexpr (SolverType::FirstOrder == 0) {
449#if defined(TINYOPT_ENFORCE_NO_DYNAMIC_ALLOCATIONS)
450 if constexpr (Dims == Dynamic) {
460 if constexpr (!kNoCopyX)
delete best_x;
466 TINYOPT_LOG(
"{}, cost: [{}]", StopReasonDescription(sum,
options_),
473 template <
typename X_t,
typename AccFunc>
474 std::pair<bool, std::optional<Vector<Scalar, Dims>>>
Step(X_t &x,
const AccFunc &acc,
477 std::pair<bool, std::optional<Vector<Scalar, Dims>>> status{
false, std::nullopt};
480 const auto t = tic();
484 const auto resize_status = ResizeIfNeeded(x);
485 if (
auto fail_reason = std::get_if<StopReason>(&resize_status)) {
490 const bool resize_and_clear_solver =
true;
493 Vector<Scalar, Dims> dx;
496 bool solver_failed =
true;
503 if (
solver_.Build(x, acc, resize_and_clear_solver)) {
505 if (
const auto &maybe_dx =
solver_.Solve()) {
506 dx = maybe_dx.value();
507 solver_failed =
false;
518 if (
options_.log.
enable) TINYOPT_LOG(
"❌ #{}: No residuals, stopping", iter);
521 }
else if (std::isnan(cost.
cost) || std::isinf(cost.
cost)) {
522 if (
options_.log.
enable) TINYOPT_LOG(
"❌ #{}: NaN/Inf in error", iter);
527 if (sum.
final_cost < std::numeric_limits<Scalar>::max())
531 TINYOPT_LOG(
"❌ #{}:Failed to solve the linear system", iter);
544 const auto &err = cost.
cost;
548 if (std::isnan(err) || std::isinf(err)) {
549 if (
options_.log.
enable) TINYOPT_LOG(
"❌ #{}: NaN/Inf in error: ε:{}", iter, err);
555 const double dx_norm2 = solver_failed ? 0 : dx.squaredNorm();
558 const double grad_norm2 = has_grad_norm2 ?
solver_.GradientSquaredNorm() : 0.0;
559 if (std::isnan(dx_norm2) || std::isinf(dx_norm2)) {
561 TINYOPT_LOG(
"❌ Failure, dX = \n{}", dx.template cast<float>());
562 TINYOPT_LOG(
"Solver: {}",
solver_.stateAsString());
563 TINYOPT_LOG(
"grad = \n{}",
solver_.Gradient());
564 if constexpr (!SolverType::FirstOrder) TINYOPT_LOG(
"H = \n{}",
solver_.H());
572 const bool is_good_step = derr < Scalar(0.0);
574 const double rel_derr = sum.
final_cost > FloatEpsilon<Scalar>() &&
575 sum.
final_cost < std::numeric_limits<Scalar>::max()
579 sum.hist.Add(err, dx_norm2, is_good_step);
582 if (is_good_step || iter == 0) {
587 sum.final_rerr_dec = rel_derr;
605 std::ostringstream oss;
607 oss <<
"#" << iter <<
" ";
609 if constexpr (traits::is_scalar_v<X_t>) {
610 oss << TINYOPT_FORMAT_NS::format(
"x:{:.5f} ", x);
611 }
else if constexpr (traits::is_matrix_or_array_v<X_t>) {
613#ifdef TINYOPT_NO_FORMATTERS
614 << x.reshaped().transpose()
616 << TINYOPT_FORMAT_NS::format(
"{}", x.reshaped().transpose())
619 }
else if constexpr (traits::is_streamable_v<X_t>) {
621 oss <<
"{" << x <<
"} ";
623 }
else if constexpr (Dims == Dynamic) {
624 const auto dims = traits::DynDims(x);
625 oss <<
"x:ℝ^" << dims <<
" ";
626 if (
solver_.dims() != dims) oss <<
"∇:ℝ^" << dims <<
" ";
630 oss << TINYOPT_FORMAT_NS::format(
"{}:{:.4e} n:{} d{}:{:+.2e} r{}:{:+.1e} ",
options_.log.
e,
631 err, nerr,
options_.log.
e, iter == 0 ? 0.0f : derr,
635 oss << TINYOPT_FORMAT_NS::format(
"|δx|:{:.2e} ", sqrt(dx_norm2));
636 if (
options_.log.
print_dx) oss << TINYOPT_FORMAT_NS::format(
"δx:[{}] ", dx);
638 if constexpr (!SolverType::FirstOrder) {
640 oss << TINYOPT_FORMAT_NS::format(
"⎡σ⎤:{:.2f} ",
solver_.MaxStdDev());
643 if (has_grad_norm2) oss << TINYOPT_FORMAT_NS::format(
"|∇|:{:.2e} ", sqrt(grad_norm2));
645 oss <<
solver_.stateAsString();
648 oss << TINYOPT_FORMAT_NS::format(
"in:{:.2f}% ({}) ", cost.
inlier_ratio * 100.0,
656 TINYOPT_LOG(
"{}", oss.str());
672 else if (
options_.stop.stop_callback &&
673 options_.stop.stop_callback(err, dx_norm2, grad_norm2))
675 else if (
options_.stop.stop_callback2 &&
676 options_.stop.stop_callback2(
float(err), dx.template cast<float>(),
677 solver_.Gradient().template cast<float>()))
681 status.first = is_good_step;
686 SolverType &solver() {
return solver_; }
687 const SolverType &solver()
const {
return solver_; }
690 static Options PrepareOptions(
const Options &options) {
691 Options prepared = options;
692#if defined(TINYOPT_ENFORCE_NO_DYNAMIC_ALLOCATIONS)
693 if constexpr (Dims != Dynamic) {
694 prepared.log.enable =
false;
695 prepared.hessian.save_last =
false;
void InitWith(const auto &g)
Initialize solver with specific gradient.
Definition optimizer.h:116
SolverType solver_
Linear solver.
Definition optimizer.h:704
Summary operator()(T &x, U &y, Rest &...rest, const Func &cost_or_acc, int max_iters=-1)
Performs optimization on the given parameters x using the provided cost or accumulation function cost...
Definition optimizer.h:311
std::pair< bool, std::optional< Vector< Scalar, Dims > > > Step(X_t &x, const AccFunc &acc, Summary &sum)
Definition optimizer.h:474
Summary OptimizeAcc(X_t &x, const AccFunc &acc, int max_iters=-1)
Performs optimization on the given parameters x using the provided accumulation function acc.
Definition optimizer.h:362
const Options options_
Optimization options.
Definition optimizer.h:702
Summary Optimize(T &x, U &y, Rest &...rest, const Func &cost_or_acc)
Performs optimization on the given parameters x using the provided cost or accumulation function cost...
Definition optimizer.h:210
void reset()
Reset the optimization and solver.
Definition optimizer.h:121
void InitWith(const auto &g, const auto &h)
Initialize solver with specific gradient and hessian.
Definition optimizer.h:110
float inlier_ratio
The ratio of inlier residuals (when robust norms are used).
Definition cost.h:95
int num_resisuals
The number of residuals (for e.g. NLLS).
Definition cost.h:94
std::string log_str
Extra information that will be printed as part of the opt. iterations.
Definition cost.h:96
Scalar cost
The function cost.
Definition cost.h:93
bool save_last
Saves the last Hessian H as part of the output results.
Definition options.h:70
bool print_dx
Log the value of step 'dx'.
Definition options.h:120
std::string e
Symbol used when logging the error, e.g ε, ε² or √ε etc.
Definition options.h:117
bool enable
Whether to enable the logging.
Definition options.h:116
bool print_t
Log the duration (in ms).
Definition options.h:122
bool print_emoji
Whether to show the emoji or not.
Definition options.h:118
bool print_max_stdev
Definition options.h:124
bool print_x
Log the value of 'x'.
Definition options.h:119
bool print_inliers
Log the inliers ratio (in %).
Definition options.h:121
bool use_step_quality_approx
Use relative error decrease as step quality; otherwise use 0.0.
Definition options.h:52
bool check_final_cost
Recompute the final cost as a rollback safety check.
Definition options.h:49
float min_grad_norm2
Minimum squared gradient norm.
Definition options.h:97
uint16_t max_iters
Maximum number of outer iterations.
Definition options.h:93
std::function< bool(const VecXf &dx, bool is_rollback)> step_callback
Definition options.h:107
uint8_t max_consec_failures
Maximum consecutive failures to decrease error.
Definition options.h:99
double max_duration_ms
Maximum optimization duration in milliseconds.
Definition options.h:100
float min_error
Minimum error/cost.
Definition options.h:94
float min_rerr_dec
Minimum relative error decrease.
Definition options.h:95
uint8_t max_total_failures
Overall max failures to decrease error.
Definition options.h:98
float min_step_norm2
Minimum squared step norm.
Definition options.h:96
Definition params_pack.h:12
std::vector< Scalar > deltas2
Squared step sizes of all iterations.
Definition summary.h:156
std::vector< Scalar > errs
Mean squared accumulated errors of all iterations.
Definition summary.h:155
std::vector< bool > successes
Step acceptance status.
Definition summary.h:157
StopReason stop_reason
Stop reason.
Definition summary.h:107
Cost final_cost
Last valid error.
Definition summary.h:103
uint8_t num_failures
Final number of failures to decrease the error.
Definition summary.h:115
uint8_t num_consec_failures
Final number of the last consecutive failures to decrease the error.
Definition summary.h:116
std::variant< std::monostate, MatX, SparseMat > final_hessian
Final H, excluding any damping (only saved if options.hessian.save = true).
Definition summary.h:125
uint16_t num_residuals
Final number of residuals.
Definition summary.h:113
TimePoint start_time
Definition summary.h:119
float duration_ms
Cumulated optimization duration.
Definition summary.h:122
uint16_t num_iters
Final number of iterations.
Definition summary.h:114
Definition optimizer.h:65
Definition optimizer.h:55
Definition parameter_traits.h:15