37 #include "gtest/gtest.h" 79 EXPECT_EQ(value, 5) <<
"x*x+1 with x=2";
82 EXPECT_EQ(value, 5) <<
"x*x+1 with x=2";
88 EXPECT_NEAR(gradient(0), 4, 1e-5) <<
"2*x = 2*2 = 4";
94 EXPECT_NEAR(hessian(0, 0), 2, 1e-5);
106 values[0] = x[0] - 1;
107 values[1] = 6 - x[0];
116 Eigen::Vector2d lsq_values;
118 EXPECT_EQ(lsq_values(0), 1) <<
"(2-1)";
119 EXPECT_EQ(lsq_values(1), 4) <<
"(6-2)";
123 EXPECT_EQ(value, lsq_values.squaredNorm()) <<
"(2-1)^2 + (6-4)^2 = 1 + 16 = 17";
128 EXPECT_NEAR(lsq_jacob(0, 0), 1, 1e-5);
129 EXPECT_NEAR(lsq_jacob(1, 0), -1, 1e-5);
135 EXPECT_NEAR(gradient(0), -6, 1e-5);
141 EXPECT_NEAR(hessian(0, 0), 4, 1e-4);
146 Eigen::VectorXd
x(1);
153 values[0] = x[0] - 1;
154 values[1] = 6 - x[0];
162 Eigen::VectorXi irow(nnz);
163 Eigen::VectorXi icol(nnz);
164 Eigen::VectorXd values(nnz);
170 std::vector<Eigen::Triplet<double>> triplets;
172 if (!triplets.empty()) jacob_sparse.setFromTriplets(triplets.begin(), triplets.end());
174 Eigen::MatrixXd jacob_sol(2, 1);
187 Eigen::VectorXi hess_irow(hess_nnz);
188 Eigen::VectorXi hess_icol(hess_nnz);
189 Eigen::VectorXd hess_values(hess_nnz);
197 if (!triplets.empty()) sparse_hessian.setFromTriplets(triplets.begin(), triplets.end());
199 Eigen::MatrixXd hessian_sol(1, 1);
212 Eigen::VectorXd
x(3);
221 values[0] = x[0] * x[0] + 1;
222 values[1] = x[1] + x[2] - 10;
230 EXPECT_EQ(values[0], 5) <<
"x*x+1 with x=2";
231 EXPECT_EQ(values[1], -3) <<
"3+4-10";
237 Eigen::MatrixXd jacob_sol(2, 3);
238 jacob_sol << 4, 0, 0, 0, 1, 1;
246 Eigen::MatrixXd hessian_sol(3, 3);
247 hessian_sol << 2, 0, 0, 0, 0, 0, 0, 0, 0;
254 Eigen::VectorXd
x(3);
263 values[0] = x[0] * x[0] + 1;
264 values[1] = x[1] + x[2] - 10;
273 Eigen::VectorXi irow(nnz);
274 Eigen::VectorXi icol(nnz);
275 Eigen::VectorXd values(nnz);
280 std::vector<Eigen::Triplet<double>> triplet_list;
281 for (
int i = 0; i < nnz; ++i) triplet_list.emplace_back(irow[i], icol[i], values[i]);
283 sparse_jacobian.
setFromTriplets(triplet_list.begin(), triplet_list.end());
285 Eigen::MatrixXd jacob_sol(2, 3);
286 jacob_sol << 4, 0, 0, 0, 1, 1;
298 Eigen::VectorXi hess_irow(hess_nnz);
299 Eigen::VectorXi hess_icol(hess_nnz);
300 Eigen::VectorXd hess_values(hess_nnz);
305 triplet_list.clear();
306 for (
int i = 0; i < hess_nnz; ++i) triplet_list.emplace_back(hess_irow[i], hess_icol[i], hess_values[i]);
308 sparse_hessian.
setFromTriplets(triplet_list.begin(), triplet_list.end());
310 Eigen::MatrixXd hessian_sol(3, 3);
311 hessian_sol << 2, 0, 0, 0, 0, 0, 0, 0, 0;
323 Eigen::VectorXd
x(3);
332 values[0] = x[0] * x[0] + 1;
333 values[1] = x[1] + x[2] - 9;
341 EXPECT_EQ(values[0], 5) <<
"x*x+1 with x=2";
342 EXPECT_EQ(values[1], -10) <<
"3-4-9";
346 EXPECT_EQ(values[0], 5);
347 EXPECT_EQ(values[1], 0);
353 Eigen::MatrixXd jacob_sol(2, 3);
354 jacob_sol << 4, 0, 0, 0, 1, 1;
360 Eigen::MatrixXd active_ineq_jacob_sol(2, 3);
361 active_ineq_jacob_sol << 4, 0, 0, 0, 0, 0;
368 Eigen::MatrixXd hessian_sol(3, 3);
369 hessian_sol << 2, 0, 0, 0, 0, 0, 0, 0, 0;
376 Eigen::VectorXd
x(3);
385 values[0] = x[0] * x[0] + 1;
386 values[1] = x[1] + x[2] - 9;
395 Eigen::VectorXi irow(nnz);
396 Eigen::VectorXi icol(nnz);
397 Eigen::VectorXd values(nnz);
402 std::vector<Eigen::Triplet<double>> triplet_list;
403 for (
int i = 0; i < nnz; ++i) triplet_list.emplace_back(irow[i], icol[i], values[i]);
405 sparse_jacobian.
setFromTriplets(triplet_list.begin(), triplet_list.end());
407 Eigen::MatrixXd jacob_sol(2, 3);
408 jacob_sol << 4, 0, 0, 0, 1, 1;
419 triplet_list.clear();
420 for (
int i = 0; i < nnz; ++i) triplet_list.emplace_back(irow[i], icol[i], values[i]);
421 sparse_jacobian.setFromTriplets(triplet_list.begin(), triplet_list.end());
423 Eigen::MatrixXd active_ineq_jacob_sol(2, 3);
424 active_ineq_jacob_sol << 4, 0, 0, 0, 0, 0;
434 Eigen::VectorXi hess_irow(hess_nnz);
435 Eigen::VectorXi hess_icol(hess_nnz);
436 Eigen::VectorXd hess_values(hess_nnz);
441 triplet_list.clear();
442 for (
int i = 0; i < hess_nnz; ++i) triplet_list.emplace_back(hess_irow[i], hess_icol[i], hess_values[i]);
444 sparse_hessian.
setFromTriplets(triplet_list.begin(), triplet_list.end());
446 Eigen::MatrixXd hessian_sol(3, 3);
447 hessian_sol << 2, 0, 0, 0, 0, 0, 0, 0, 0;
461 Eigen::VectorXd
x(3);
468 Eigen::VectorXd lb(3);
485 EXPECT_EQ(ub_return[1], 5);
486 EXPECT_EQ(ub_return[2], 6);
503 Eigen::MatrixXd active_bound_jacobian_sol(2, 3);
504 active_bound_jacobian_sol << 0, -1, 0, 0, 0, 1;
514 Eigen::VectorXd
x(3);
521 Eigen::VectorXd lb(3);
535 Eigen::VectorXi irow(nnz);
536 Eigen::VectorXi icol(nnz);
537 Eigen::VectorXd values(nnz);
542 std::vector<Eigen::Triplet<double>> triplet_list;
543 for (
int i = 0; i < nnz; ++i) triplet_list.emplace_back(irow[i], icol[i], values[i]);
545 sparse_jacobian.
setFromTriplets(triplet_list.begin(), triplet_list.end());
559 triplet_list.clear();
560 for (
int i = 0; i < nnz; ++i) triplet_list.emplace_back(irow[i], icol[i], values[i]);
561 sparse_jacobian.setFromTriplets(triplet_list.begin(), triplet_list.end());
563 Eigen::MatrixXd active_bound_jacobian_sol(2, 3);
564 active_bound_jacobian_sol << 0, -1, 0, 0, 0, 1;
577 Eigen::VectorXd
x(3);
584 Eigen::VectorXd lb(3);
602 values[0] = x[0] * x[0] + 1;
603 values[1] = x[1] + x[2] - 10;
610 values[0] = x[0] * x[0] + 1;
611 values[1] = x[1] + x[2] - 9;
620 int dim_total = dim_obj_lsq + dim_eq + dim_ineq + dim_bounds;
625 Eigen::MatrixXd sol1(7, 3);
626 sol1 << 4, 0, 0, 4, 0, 0, 0, 1, 1, 4, 0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 1;
628 sol1.middleRows(dim_obj_lsq, dim_eq) *= 2;
629 sol1.middleRows(dim_obj_lsq + dim_eq, dim_ineq) *= 3;
630 sol1.middleRows(dim_obj_lsq + dim_eq + dim_ineq, dim_bounds) *= 4;
635 Eigen::MatrixXd sol2 = sol1;
641 Eigen::MatrixXd sol3(5, 3);
642 sol3 << 4, 0, 0, 4, 0, 0, 0, 1, 1, 4, 0, 0, 0, 1, 1;
646 Eigen::VectorXi jac_irow(jac_nnz);
647 Eigen::VectorXi jac_icol(jac_nnz);
648 Eigen::VectorXd jac_values(jac_nnz);
653 std::vector<Eigen::Triplet<double>> triplet_list;
656 sparse_hessian.
setFromTriplets(triplet_list.begin(), triplet_list.end());
660 double multiplier_obj_lsq = 5;
661 Eigen::Vector2d multiplier_eq(2, 3);
662 Eigen::Vector2d multiplier_ineq(3, 4);
664 sol3.row(0) *= multiplier_obj_lsq;
665 sol3.row(1) *= multiplier_eq(0);
666 sol3.row(2) *= multiplier_eq(1);
667 sol3.row(3) *= multiplier_ineq(0);
668 sol3.row(4) *= multiplier_ineq(1);
670 jac_values.setZero();
673 sparse_hessian.setZero();
674 sparse_hessian.setFromTriplets(triplet_list.begin(), triplet_list.end());
virtual double computeValueObjective()
void setX(const Eigen::Ref< const Eigen::VectorXd > &x)
void setParameterVector(const Eigen::Ref< const Eigen::VectorXd > &x) override
Same as setX() (overrides interface method)
#define EXPECT_ZERO_MATRIX(A, tol)
int getEqualityDimension() override
Total dimension of equality constraints.
virtual int finiteCombinedBoundsDimension()
Dimension of the set of finite bounds (combined such that each ub and lb component define a single di...
void setParameterValue(int idx, double x) override
Set specific value of the parameter vector.
virtual void computeSparseJacobianInequalities(Eigen::SparseMatrix< double > &jacobian, const double *multipliers=nullptr)
void setInequalityConstraint(std::function< void(const Eigen::VectorXd &, Eigen::Ref< Eigen::VectorXd >)> ineq_fun, int ineq_dim)
Set inequality constraint callback.
virtual void computeSparseJacobianLsqObjectiveStructure(Eigen::Ref< Eigen::VectorXi > i_row, Eigen::Ref< Eigen::VectorXi > j_col)
void convert_triplet(const Eigen::Ref< const Eigen::VectorXi > &i_row, const Eigen::Ref< const Eigen::VectorXi > &j_col, const Eigen::Ref< const Eigen::VectorXd > &values, std::vector< Eigen::Triplet< double >> &triplets)
virtual void computeDenseJacobianEqualities(Eigen::Ref< Eigen::MatrixXd > jacobian, const double *multipliers=nullptr)
Compute the equality constraint Jacobian Jceq(x) for the current parameter set.
int getNonLsqObjectiveDimension() override
Total dimension of objective function terms.
virtual int finiteBoundsDimension()
Dimension of the set of finite bounds (individual bounds ub and lb)
virtual int computeSparseHessianObjectiveNNZ(bool lower_part_only=false)
void setObjectiveFunction(std::function< void(const Eigen::VectorXd &, Eigen::Ref< Eigen::VectorXd >)> obj_fun, int obj_dim, bool lsq_form=false)
Set objective function callback.
virtual void computeSparseJacobianFiniteCombinedBounds(Eigen::SparseMatrix< double > &jacobian, double weight=1.0)
virtual void computeDenseJacobianFiniteCombinedBounds(Eigen::Ref< Eigen::MatrixXd > jacobian, double weight=1.0)
Compute the Jacobian for finite combined bounds.
virtual void computeDenseJacobianActiveInequalities(Eigen::Ref< Eigen::MatrixXd > jacobian, double weight=1.0)
Compute the Jacobian Jc(x) with non-zeros for active constraints c(x)>= 0 and zeros for inactive ones...
virtual void computeSparseHessianEqualitiesValues(Eigen::Ref< Eigen::VectorXd > values, const double *multipliers=nullptr, bool lower_part_only=false)
virtual int computeSparseJacobianInequalitiesNNZ()
TestStandardOptimizationProblem()
Simple optimization problem formulation (callback based configuration)
virtual void computeCombinedSparseJacobiansValues(Eigen::Ref< Eigen::VectorXd > values, bool objective_lsq=true, bool equality=true, bool inequality=true, const double *multipliers_obj=nullptr, const double *multipliers_eq=nullptr, const double *multipliers_ineq=nullptr)
virtual void computeSparseJacobianEqualities(Eigen::SparseMatrix< double > &jacobian, const double *multipliers=nullptr)
virtual void computeSparseJacobianFiniteCombinedBoundsStructure(Eigen::Ref< Eigen::VectorXi > i_row, Eigen::Ref< Eigen::VectorXi > j_col)
virtual void computeSparseJacobianLsqObjectiveValues(Eigen::Ref< Eigen::VectorXd > values, const double *multipliers=nullptr)
void computeValuesLsqObjective(Eigen::Ref< Eigen::VectorXd > values) override
Compute the objective function values f(x) for the current parameter set.
void resizeParameterVector(int parameter_dim)
Resize the dimension of the parameter vector.
virtual void computeSparseJacobianEqualitiesValues(Eigen::Ref< Eigen::VectorXd > values, const double *multipliers=nullptr)
virtual void computeSparseJacobianLsqObjective(Eigen::SparseMatrix< double > &jacobian, const double *multipliers=nullptr)
SimpleOptimizationProblemWithCallbacks optim
virtual void computeSparseHessianObjectiveStructure(Eigen::Ref< Eigen::VectorXi > i_row, Eigen::Ref< Eigen::VectorXi > j_col, bool lower_part_only=false)
virtual void computeSparseHessianInequalitiesValues(Eigen::Ref< Eigen::VectorXd > values, const double *multipliers=nullptr, bool lower_part_only=false)
virtual int computeCombinedSparseJacobiansNNZ(bool objective_lsq=true, bool equality=true, bool inequality=true)
int getObjectiveDimension() override
Get dimension of the objective (should be zero or one, includes Lsq objectives if present) ...
virtual void computeSparseHessianObjective(Eigen::SparseMatrix< double > &hessian, double multiplier=1.0)
virtual int computeSparseHessianInequalitiesNNZ(bool lower_part_only=false)
int getInequalityDimension() override
Total dimension of general inequality constraints.
int getLsqObjectiveDimension() override
Total dimension of least-squares objective function terms.
virtual int computeSparseHessianEqualitiesNNZ(bool lower_part_only=false)
virtual void computeValuesActiveInequality(Eigen::Ref< Eigen::VectorXd > values, double weight=1.0)
Compute the values of the active inequality constraints (elementwise max(0, c(x))) ...
virtual void computeSparseJacobianInequalitiesStructure(Eigen::Ref< Eigen::VectorXi > i_row, Eigen::Ref< Eigen::VectorXi > j_col)
virtual void computeSparseJacobianEqualitiesStructure(Eigen::Ref< Eigen::VectorXi > i_row, Eigen::Ref< Eigen::VectorXi > j_col)
virtual void computeSparseHessianObjectiveValues(Eigen::Ref< Eigen::VectorXd > values, double multiplier=1.0, bool lower_part_only=false)
double computeValueNonLsqObjective() override
virtual void computeSparseJacobianInequalitiesValues(Eigen::Ref< Eigen::VectorXd > values, const double *multipliers=nullptr)
constexpr const double CORBO_INF_DBL
Representation for infinity (double version)
virtual int computeSparseJacobianEqualitiesNNZ()
virtual int computeSparseJacobianLsqObjectiveNNZ()
void getBounds(Eigen::Ref< Eigen::VectorXd > lb, Eigen::Ref< Eigen::VectorXd > ub) override
Get lower and upper bound vector.
virtual ~TestStandardOptimizationProblem()
A matrix or vector expression mapping an existing expression.
void computeValuesEquality(Eigen::Ref< Eigen::VectorXd > values) override
Compute the equality constraint values ceq(x) for the current parameter set.
virtual void computeSparseJacobianFiniteCombinedBoundsValues(Eigen::Ref< Eigen::VectorXd > values, double weight=1.0)
void setFromTriplets(const InputIterators &begin, const InputIterators &end)
TEST_F(TestStandardOptimizationProblem, compute_objective)
virtual void computeSparseJacobianActiveInequalities(Eigen::SparseMatrix< double > &jacobian, double weight=1.0)
virtual void computeDenseHessianInequalities(Eigen::Ref< Eigen::MatrixXd > hessian, const double *multipliers=nullptr)
void setEqualityConstraint(std::function< void(const Eigen::VectorXd &, Eigen::Ref< Eigen::VectorXd >)> eq_fun, int eq_dim)
Set equality constraint callback.
virtual int computeSparseJacobianFiniteCombinedBoundsNNZ()
virtual void computeDenseHessianEqualities(Eigen::Ref< Eigen::MatrixXd > hessian, const double *multipliers=nullptr)
virtual void computeSparseHessianEqualitiesStructure(Eigen::Ref< Eigen::VectorXi > i_row, Eigen::Ref< Eigen::VectorXi > j_col, bool lower_part_only=false)
virtual void computeCombinedSparseJacobiansStructure(Eigen::Ref< Eigen::VectorXi > i_row, Eigen::Ref< Eigen::VectorXi > j_col, bool objective_lsq=true, bool equality=true, bool inequality=true)
virtual void computeDenseJacobianInequalities(Eigen::Ref< Eigen::MatrixXd > jacobian, const double *multipliers=nullptr)
Compute the inequality constraint Jacobian Jc(x) for the current parameter set.
virtual void computeSparseHessianInequalitiesStructure(Eigen::Ref< Eigen::VectorXi > i_row, Eigen::Ref< Eigen::VectorXi > j_col, bool lower_part_only=false)
void setLowerBounds(const Eigen::Ref< const Eigen::VectorXd > &lb)
#define EXPECT_EQ_MATRIX(A, B, tol)
virtual void computeDenseHessianObjective(const Eigen::Ref< const Eigen::MatrixXd > &jacobian, Eigen::Ref< Eigen::MatrixXd > hessian, const double *multipliers=nullptr, bool jacob_scaled=true)
Compute the objective Hessian Hf(x) for the current parameter set.
void setUpperBound(int idx, double ub) override
Set specific upper bound of a parameter.
void computeValuesInequality(Eigen::Ref< Eigen::VectorXd > values) override
Compute the inequality constraint values c(x) for the current parameter set.
virtual void computeDenseJacobianLsqObjective(Eigen::Ref< Eigen::MatrixXd > jacobian, const double *multipliers=nullptr)
Compute the objective Jacobian Jf(x) for the current parameter set.
virtual void computeSparseHessianEqualities(Eigen::SparseMatrix< double > &hessian, const double *multipliers=nullptr)
virtual void computeGradientObjective(Eigen::Ref< Eigen::VectorXd > gradient)
int getParameterDimension() override
Effictive dimension of the optimization parameter set (changeable, non-fixed part) ...
virtual void computeSparseHessianInequalities(Eigen::SparseMatrix< double > &hessian, const double *multipliers=nullptr)
virtual void computeSparseJacobianActiveInequalitiesValues(Eigen::Ref< Eigen::VectorXd > values, double weight=1.0)
virtual void computeCombinedSparseJacobian(Eigen::SparseMatrix< double > &jacobian, bool objective_lsq, bool equality, bool inequality, bool finite_combined_bounds, bool active_ineq=false, double weight_eq=1.0, double weight_ineq=1.0, double weight_bounds=1.0, const Eigen::VectorXd *values=nullptr, const Eigen::VectorXi *col_nnz=nullptr)