@@ -31,19 +31,24 @@ namespace mini_opt {
3131 * with diagonal inequality constraints on the decision variables.
3232 */
3333struct Problem {
34- using unique_ptr = std::unique_ptr<Problem>;
35-
3634 // Problem dimension. (ie. max variable index + 1)
3735 int dimension;
3836
3937 // The errors that form the sum of squares part of the cost function.
40- std::vector<ResidualBase::unique_ptr > costs;
38+ std::vector<Residual > costs;
4139
4240 // Linear inequality constraints.
4341 std::vector<LinearInequalityConstraint> inequality_constraints;
4442
4543 // Nonlinear inequality constraints.
46- std::vector<ResidualBase::unique_ptr> equality_constraints;
44+ std::vector<Residual> equality_constraints;
45+
46+ // Clear all factors.
47+ void clear () {
48+ costs.clear ();
49+ inequality_constraints.clear ();
50+ equality_constraints.clear ();
51+ }
4752};
4853
4954/*
@@ -85,8 +90,8 @@ struct ConstrainedNonlinearLeastSquares {
8590 // Value by which to decrease alpha for the backtracking line search.
8691 double armijo_search_tau{0.8 };
8792
88- // Initial value of the equality constraint penalty.
89- double equality_penalty_initial{0.01 };
93+ // Initial value of the equality constraint penalty, µ .
94+ double equality_penalty_initial{1.0 };
9095
9196 // Scale factor when increasing the equality penalty.
9297 // If µ_new > µ_old, we increase the penalty to µ_new * equality_penalty_scale_factor.
@@ -102,20 +107,24 @@ struct ConstrainedNonlinearLeastSquares {
102107 // Initial lambda value on entering the ATTEMPTING_RESTORE_LM state.
103108 double lambda_failure_init{1.0e-2 };
104109
110+ // Multiplicative amount to decrease lambda on a successful step.
111+ double lambda_decrease_on_success{0.1 };
112+
113+ // Multiplicative amount to decrease lambda when exiting the `ATTEMPTING_RESTORE_LM` state.
114+ double lambda_decrease_on_restore{0.8 };
115+
105116 // Maximum lambda value.
106117 double max_lambda{1 .};
107118
108119 // Minimum lambda value.
109120 double min_lambda{0 .};
121+
122+ // If true, compute and log summary stats of the eigenvalues from the QP hessian `G`.
123+ bool log_qp_eigenvalues{false };
110124 };
111125
112126 // Signature of custom retraction operator.
113- using Retraction =
114- std::function<void (Eigen::VectorXd* const x, const ConstVectorBlock& dx, const double alpha)>;
115-
116- // Signature of custom logger.
117- using LoggingCallback =
118- std::function<bool (const ConstrainedNonlinearLeastSquares& self, const NLSLogInfo& info)>;
127+ using Retraction = std::function<void (Eigen::VectorXd& x, ConstVectorBlock dx, double alpha)>;
119128
120129 // Construct w/ const pointer to a problem definition.
121130 explicit ConstrainedNonlinearLeastSquares (const Problem* problem,
@@ -135,28 +144,18 @@ struct ConstrainedNonlinearLeastSquares {
135144 */
136145 NLSSolverOutputs Solve (const Params& params, const Eigen::VectorXd& variables);
137146
138- // Set the callback which will be used for the QP solver.
139- template <typename T>
140- void SetQPLoggingCallback (T&& cb) {
141- // TODO: Get rid of the logging callback altogether. For now I maintain this path for existing
142- // unit tests.
143- if (QPInteriorPointSolver* ip_solver = std::get_if<QPInteriorPointSolver>(&solver_);
144- ip_solver != nullptr )
145- ip_solver->SetLoggerCallback (std::forward<T>(cb));
146- }
147-
148- // Set the callback that will be used for the nonlinear solver.
149- template <typename T>
150- void SetLoggingCallback (T&& cb) {
151- logging_callback_ = std::forward<T>(cb);
152- }
153-
154147 // Get the current linearization point.
155148 constexpr const Eigen::VectorXd& variables () const noexcept { return variables_; }
156149
157150 // Evaluate the non-linear error.
158151 Errors EvaluateNonlinearErrors (const Eigen::VectorXd& vars);
159152
153+ // The user exit callback may return `false` to cause the optimization to terminate early.
154+ template <typename T>
155+ void SetUserExitCallback (T&& cb) {
156+ user_exit_callback_ = std::forward<T>(cb);
157+ }
158+
160159 private:
161160 // Update candidate_vars w/ a step size of alpha.
162161 void RetractCandidateVars (double alpha);
@@ -166,15 +165,22 @@ struct ConstrainedNonlinearLeastSquares {
166165 const Problem& problem, QP* qp);
167166
168167 // Solve the QP, and update the step direction `dx_`.
169- std::tuple<QPSolverOutputs, std::optional<QPLagrangeMultipliers> > ComputeStepDirection (
168+ std::variant<QPNullSpaceTerminationState, QPInteriorPointSolverOutputs > ComputeStepDirection (
170169 const Params& params);
171170
171+ // Extract lagrange multipliers from the QP solver outputs, if they were used.
172+ static std::optional<QPLagrangeMultipliers> MaybeGetLagrangeMultipliers (
173+ const std::variant<QPNullSpaceTerminationState, QPInteriorPointSolverOutputs>& output);
174+
175+ // True if the QP was indefinite (in which case we will terminate optimization).
176+ static bool QPWasIndefinite (
177+ const std::variant<QPNullSpaceTerminationState, QPInteriorPointSolverOutputs>& output);
178+
172179 // Based on the outcome of the step selection, update lambda and check if
173180 // we should exit. Returns NONE if no exit is required.
174- NLSTerminationState UpdateLambdaAndCheckExitConditions (const Params& params,
175- StepSizeSelectionResult step_result,
176- const Errors& initial_errors,
177- double penalty, double & lambda);
181+ std::optional<NLSTerminationState> UpdateLambdaAndCheckExitConditions (
182+ const Params& params, StepSizeSelectionResult step_result, const Errors& initial_errors,
183+ double penalty, double & lambda);
178184
179185 // Execute a back-tracking search until the cost decreases, or we hit
180186 // a max number of iterations
@@ -185,9 +191,9 @@ struct ConstrainedNonlinearLeastSquares {
185191 double backtrack_search_tau);
186192
187193 // Repeatedly approximate the cost function as a polynomial, and find the minimum.
188- double ComputeAlphaPolynomialApproximation (int iteration, double alpha, const Errors& errors_pre,
189- const DirectionalDerivatives& derivatives,
190- double penalty) const ;
194+ std::optional< double > ComputeAlphaPolynomialApproximation (
195+ int iteration, const Errors& errors_pre, const DirectionalDerivatives& derivatives,
196+ double penalty) const ;
191197
192198 // Compute first derivative of the QP cost function.
193199 static DirectionalDerivatives ComputeQPCostDerivative (const QP& qp, const Eigen::VectorXd& dx);
@@ -199,24 +205,18 @@ struct ConstrainedNonlinearLeastSquares {
199205
200206 // Solve the quadratic approximation of the cost function for best alpha.
201207 // Implements equation (3.57/3.58)
202- static double QuadraticApproxMinimum (double phi_0, double phi_prime_0, double alpha_0 ,
203- double phi_alpha_0);
208+ static std::optional< double > QuadraticApproxMinimum (double phi_0, double phi_prime_0,
209+ double alpha_0, double phi_alpha_0);
204210
205211 // Get the polynomial coefficients c0, c1 from the cubic approximation of the cost.
206212 // Implements equation after 3.58, returns [a, b]
207213 static Eigen::Vector2d CubicApproxCoeffs (double phi_0, double phi_prime_0, double alpha_0,
208214 double phi_alpha_0, double alpha_1, double phi_alpha_1);
209215
210216 // Get the solution of the cubic approximation.
211- static double CubicApproxMinimum (double phi_prime_0, const Eigen::Vector2d& ab);
212-
213- // Compute the second order correction to the equality constraints.
214- static void ComputeSecondOrderCorrection (
215- const Eigen::VectorXd& updated_x,
216- const std::vector<ResidualBase::unique_ptr>& equality_constraints, QP* qp,
217- Eigen::CompleteOrthogonalDecomposition<Eigen::MatrixXd>* solver, Eigen::VectorXd* dx_out);
217+ static std::optional<double > CubicApproxMinimum (double phi_prime_0, const Eigen::Vector2d& ab);
218218
219- const Problem* const p_;
219+ const Problem* p_;
220220
221221 // Custom retraction operator, optional.
222222 Retraction custom_retraction_;
@@ -241,8 +241,8 @@ struct ConstrainedNonlinearLeastSquares {
241241 // Current state of the optimization
242242 OptimizerState state_{OptimizerState::NOMINAL};
243243
244- // Logging callback.
245- LoggingCallback logging_callback_ {};
244+ // A user-specified callback that can opt to terminate the optimization early .
245+ std::function< bool ( const NLSIteration&)> user_exit_callback_ {};
246246
247247 friend class ConstrainedNLSTest ;
248248};
0 commit comments