test_simple_optimization_problem.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement
4  *
5  * Copyright (c) 2020,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * This program is free software: you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation, either version 3 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program. If not, see <https://www.gnu.org/licenses/>.
21  *
22  * Authors: Christoph Rösmann
23  *********************************************************************/
24 
26 
27 #include <corbo-core/console.h>
28 #include <corbo-core/macros.h>
29 #include <corbo-core/types.h>
30 #include <corbo-core/utilities.h>
33 
34 #include <array>
35 #include <functional>
36 
37 #include "gtest/gtest.h"
38 
41 
42 class TestStandardOptimizationProblem : public testing::Test
43 {
44  protected:
45  // You can do set-up work for each test here.
47  // You can do clean-up work that doesn't throw exceptions here.
49  // If the constructor and destructor are not enough for setting up
50  // and cleaning up each test, you can define the following methods:
51 
52  // Code here will be called immediately after the constructor (right
53  // before each test).
54  void SetUp() override {}
55  // Code here will be called immediately after each test (right
56  // before the destructor).
57  // virtual void TearDown()
58 
60 };
61 
63 {
64  Eigen::VectorXd x(1);
65  x[0] = 2;
66  optim.setX(x);
67 
68  // define objective function
69  // f = x^2 +1
70  auto objective = [](const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> values) { values[0] = x[0] * x[0] + 1; };
71  optim.setObjectiveFunction(objective, 1, false);
72 
73  EXPECT_EQ(optim.getObjectiveDimension(), 1);
74  EXPECT_EQ(optim.getNonLsqObjectiveDimension(), 1);
75  EXPECT_EQ(optim.getLsqObjectiveDimension(), 0);
76 
77  // compute values
78  double value = optim.computeValueObjective();
79  EXPECT_EQ(value, 5) << "x*x+1 with x=2";
80 
82  EXPECT_EQ(value, 5) << "x*x+1 with x=2";
83 
84  // compute gradient
85  Eigen::VectorXd gradient(optim.getParameterDimension());
87 
88  EXPECT_NEAR(gradient(0), 4, 1e-5) << "2*x = 2*2 = 4";
89 
90  // compute hessian
91  Eigen::MatrixXd hessian(optim.getParameterDimension(), optim.getParameterDimension());
93 
94  EXPECT_NEAR(hessian(0, 0), 2, 1e-5);
95 }
96 
97 TEST_F(TestStandardOptimizationProblem, compute_objective_lsq)
98 {
99  Eigen::VectorXd x(1);
100  x[0] = 2;
101  optim.setX(x);
102 
103  // define lsq objective function
104  // f = (x-1)^2 + (6-x)^2
105  auto objective = [](const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> values) {
106  values[0] = x[0] - 1;
107  values[1] = 6 - x[0];
108  };
109  optim.setObjectiveFunction(objective, 2, true);
110 
111  EXPECT_EQ(optim.getObjectiveDimension(), 1);
112  EXPECT_EQ(optim.getNonLsqObjectiveDimension(), 0);
113  EXPECT_EQ(optim.getLsqObjectiveDimension(), 2);
114 
115  // compute values
116  Eigen::Vector2d lsq_values;
117  optim.computeValuesLsqObjective(lsq_values);
118  EXPECT_EQ(lsq_values(0), 1) << "(2-1)";
119  EXPECT_EQ(lsq_values(1), 4) << "(6-2)";
120 
121  double value = optim.computeValueObjective();
122 
123  EXPECT_EQ(value, lsq_values.squaredNorm()) << "(2-1)^2 + (6-4)^2 = 1 + 16 = 17";
124 
125  // compute lsq jacobian
126  Eigen::MatrixXd lsq_jacob(optim.getLsqObjectiveDimension(), optim.getParameterDimension());
128  EXPECT_NEAR(lsq_jacob(0, 0), 1, 1e-5);
129  EXPECT_NEAR(lsq_jacob(1, 0), -1, 1e-5);
130 
131  // compute overall objective gradient
132  // df/dx = 2*(x-1) - 2*(6-x) = 2*1 - 2*4 = -6;
133  Eigen::VectorXd gradient(optim.getParameterDimension());
135  EXPECT_NEAR(gradient(0), -6, 1e-5);
136 
137  // compute overall objective hessian
138  // df/dx^2 = 2 + 2 = 4
139  Eigen::MatrixXd hessian(optim.getParameterDimension(), optim.getParameterDimension());
141  EXPECT_NEAR(hessian(0, 0), 4, 1e-4);
142 }
143 
144 TEST_F(TestStandardOptimizationProblem, compute_objective_sparse_lsq)
145 {
146  Eigen::VectorXd x(1);
147  x[0] = 2;
148  optim.setX(x);
149 
150  // define lsq objective function
151  // f = (x-1)^2 + (6-x)^2
152  auto objective = [](const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> values) {
153  values[0] = x[0] - 1;
154  values[1] = 6 - x[0];
155  };
156  optim.setObjectiveFunction(objective, 2, true);
157 
158  // compute sparse jacobian
160  EXPECT_GE(nnz, 2);
161 
162  Eigen::VectorXi irow(nnz);
163  Eigen::VectorXi icol(nnz);
164  Eigen::VectorXd values(nnz);
167 
168  // convert to Eigen type for convenient comparison
170  std::vector<Eigen::Triplet<double>> triplets;
171  corbo::convert_triplet(irow, icol, values, triplets);
172  if (!triplets.empty()) jacob_sparse.setFromTriplets(triplets.begin(), triplets.end());
173 
174  Eigen::MatrixXd jacob_sol(2, 1);
175  jacob_sol << 1, -1;
176 
177  EXPECT_EQ_MATRIX(jacob_sparse, jacob_sol, 1e-5);
178 
179  // compute sparse eigen jacobian directly
181  optim.computeSparseJacobianLsqObjective(jacob_sparse_eigen);
182  EXPECT_EQ_MATRIX(jacob_sparse_eigen, jacob_sol, 1e-5);
183 
184  // compute sparse hessian
185  int hess_nnz = optim.computeSparseHessianObjectiveNNZ();
186 
187  Eigen::VectorXi hess_irow(hess_nnz);
188  Eigen::VectorXi hess_icol(hess_nnz);
189  Eigen::VectorXd hess_values(hess_nnz);
190  optim.computeSparseHessianObjectiveStructure(hess_irow, hess_icol);
192 
193  // convert to Eigen type for convenient comparison
194  triplets.clear();
196  corbo::convert_triplet(hess_irow, hess_icol, hess_values, triplets);
197  if (!triplets.empty()) sparse_hessian.setFromTriplets(triplets.begin(), triplets.end());
198 
199  Eigen::MatrixXd hessian_sol(1, 1);
200  hessian_sol << 4;
201 
202  EXPECT_EQ_MATRIX(sparse_hessian, hessian_sol, 1e-4);
203 
204  // compute sparse eigen hessian directly
206  optim.computeSparseHessianObjective(sparse_hessian_eigen);
207  EXPECT_EQ_MATRIX(sparse_hessian_eigen, hessian_sol, 1e-4);
208 }
209 
210 TEST_F(TestStandardOptimizationProblem, compute_equality_constr)
211 {
212  Eigen::VectorXd x(3);
213  x[0] = 2;
214  x[1] = 3;
215  x[2] = 4;
216  optim.setX(x);
217 
218  // define equality constraint function
219  // ceq = [x1^2+1; x2+x3-10]
220  auto ceq = [](const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> values) {
221  values[0] = x[0] * x[0] + 1;
222  values[1] = x[1] + x[2] - 10;
223  };
225 
226  // compute values
227  Eigen::VectorXd values(optim.getEqualityDimension());
229 
230  EXPECT_EQ(values[0], 5) << "x*x+1 with x=2";
231  EXPECT_EQ(values[1], -3) << "3+4-10";
232 
233  // compute jacobian
234  Eigen::MatrixXd jacobian(optim.getEqualityDimension(), optim.getParameterDimension());
235  optim.computeDenseJacobianEqualities(jacobian, nullptr);
236 
237  Eigen::MatrixXd jacob_sol(2, 3);
238  jacob_sol << 4, 0, 0, 0, 1, 1;
239 
240  EXPECT_EQ_MATRIX(jacobian, jacob_sol, 1e-5);
241 
242  // compute hessian
243  Eigen::MatrixXd hessian(optim.getParameterDimension(), optim.getParameterDimension());
244  optim.computeDenseHessianEqualities(hessian, nullptr);
245 
246  Eigen::MatrixXd hessian_sol(3, 3);
247  hessian_sol << 2, 0, 0, 0, 0, 0, 0, 0, 0;
248 
249  EXPECT_EQ_MATRIX(hessian, hessian_sol, 1e-5);
250 }
251 
252 TEST_F(TestStandardOptimizationProblem, compute_equality_constr_sparse)
253 {
254  Eigen::VectorXd x(3);
255  x[0] = 2;
256  x[1] = 3;
257  x[2] = 4;
258  optim.setX(x);
259 
260  // define equality constraint
261  // ceq = [x1^2+1; x2+x3-10]
262  auto ceq = [](const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> values) {
263  values[0] = x[0] * x[0] + 1;
264  values[1] = x[1] + x[2] - 10;
265  };
267 
268  // compute sparse jacobian
270  // This formulation does not support sparse matrices actually, so we check if default conversions are working
272 
273  Eigen::VectorXi irow(nnz);
274  Eigen::VectorXi icol(nnz);
275  Eigen::VectorXd values(nnz);
278 
279  // convert to Eigen type for convenient comparison
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());
284 
285  Eigen::MatrixXd jacob_sol(2, 3);
286  jacob_sol << 4, 0, 0, 0, 1, 1;
287 
288  EXPECT_EQ_MATRIX(sparse_jacobian, jacob_sol, 1e-5);
289 
290  // compute sparse eigen jacobian directly
292  optim.computeSparseJacobianEqualities(sparse_jacob_eigen);
293  EXPECT_EQ_MATRIX(sparse_jacob_eigen, jacob_sol, 1e-5);
294 
295  // compute sparse hessian
296  int hess_nnz = optim.computeSparseHessianEqualitiesNNZ();
297 
298  Eigen::VectorXi hess_irow(hess_nnz);
299  Eigen::VectorXi hess_icol(hess_nnz);
300  Eigen::VectorXd hess_values(hess_nnz);
301  optim.computeSparseHessianEqualitiesStructure(hess_irow, hess_icol);
303 
304  // convert to Eigen type for convenient comparison
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());
309 
310  Eigen::MatrixXd hessian_sol(3, 3);
311  hessian_sol << 2, 0, 0, 0, 0, 0, 0, 0, 0;
312 
313  EXPECT_EQ_MATRIX(sparse_hessian, hessian_sol, 1e-5);
314 
315  // compute sparse eigen hessian directly
317  optim.computeSparseHessianEqualities(sparse_hessian_eigen);
318  EXPECT_EQ_MATRIX(sparse_hessian_eigen, hessian_sol, 1e-5);
319 }
320 
321 TEST_F(TestStandardOptimizationProblem, compute_inequality_constr)
322 {
323  Eigen::VectorXd x(3);
324  x[0] = 2;
325  x[1] = 3;
326  x[2] = -4;
327  optim.setX(x);
328 
329  // define inequality constraint function
330  // c = [x1^2+1; x2+x3-9]
331  auto c = [](const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> values) {
332  values[0] = x[0] * x[0] + 1;
333  values[1] = x[1] + x[2] - 9;
334  };
336 
337  // compute values
338  Eigen::VectorXd values(optim.getInequalityDimension());
340 
341  EXPECT_EQ(values[0], 5) << "x*x+1 with x=2";
342  EXPECT_EQ(values[1], -10) << "3-4-9";
343 
344  // compute only active constraints max(0,c(x))
346  EXPECT_EQ(values[0], 5);
347  EXPECT_EQ(values[1], 0);
348 
349  // compute jacobian
350  Eigen::MatrixXd jacobian(optim.getInequalityDimension(), optim.getParameterDimension());
351  optim.computeDenseJacobianInequalities(jacobian, nullptr);
352 
353  Eigen::MatrixXd jacob_sol(2, 3);
354  jacob_sol << 4, 0, 0, 0, 1, 1;
355  EXPECT_EQ_MATRIX(jacobian, jacob_sol, 1e-5);
356 
357  // compute jacobian with zero-rows for inactive constraints
359 
360  Eigen::MatrixXd active_ineq_jacob_sol(2, 3);
361  active_ineq_jacob_sol << 4, 0, 0, 0, 0, 0;
362  EXPECT_EQ_MATRIX(jacobian, active_ineq_jacob_sol, 1e-5);
363 
364  // compute hessian
365  Eigen::MatrixXd hessian(optim.getParameterDimension(), optim.getParameterDimension());
366  optim.computeDenseHessianInequalities(hessian, nullptr);
367 
368  Eigen::MatrixXd hessian_sol(3, 3);
369  hessian_sol << 2, 0, 0, 0, 0, 0, 0, 0, 0;
370 
371  EXPECT_EQ_MATRIX(hessian, hessian_sol, 1e-5);
372 }
373 
374 TEST_F(TestStandardOptimizationProblem, compute_inequality_constr_sparse)
375 {
376  Eigen::VectorXd x(3);
377  x[0] = 2;
378  x[1] = 3;
379  x[2] = -4;
380  optim.setX(x);
381 
382  // define inequality constraint function
383  // c = [x1^2+1; x2+x3-9]
384  auto c = [](const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> values) {
385  values[0] = x[0] * x[0] + 1;
386  values[1] = x[1] + x[2] - 9;
387  };
389 
390  // compute sparse jacobian
392  // This formulation does not support sparse matrices actually, so we check if default conversions are working
394 
395  Eigen::VectorXi irow(nnz);
396  Eigen::VectorXi icol(nnz);
397  Eigen::VectorXd values(nnz);
400 
401  // convert to Eigen type for convenient comparison
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());
406 
407  Eigen::MatrixXd jacob_sol(2, 3);
408  jacob_sol << 4, 0, 0, 0, 1, 1;
409 
410  EXPECT_EQ_MATRIX(sparse_jacobian, jacob_sol, 1e-5);
411 
412  // compute sparse eigen matrix directly
414  optim.computeSparseJacobianInequalities(sparse_jacob_eigen);
415  EXPECT_EQ_MATRIX(sparse_jacob_eigen, jacob_sol, 1e-5);
416 
417  // compute jacobian with zero-rows for inactive constraints
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());
422 
423  Eigen::MatrixXd active_ineq_jacob_sol(2, 3);
424  active_ineq_jacob_sol << 4, 0, 0, 0, 0, 0;
425  EXPECT_EQ_MATRIX(sparse_jacobian, active_ineq_jacob_sol, 1e-5);
426 
427  // compute sparse eigen jacobian directly
429  EXPECT_EQ_MATRIX(sparse_jacob_eigen, active_ineq_jacob_sol, 1e-5);
430 
431  // compute sparse hessian
433 
434  Eigen::VectorXi hess_irow(hess_nnz);
435  Eigen::VectorXi hess_icol(hess_nnz);
436  Eigen::VectorXd hess_values(hess_nnz);
437  optim.computeSparseHessianInequalitiesStructure(hess_irow, hess_icol);
439 
440  // convert to Eigen type for convenient comparison
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());
445 
446  Eigen::MatrixXd hessian_sol(3, 3);
447  hessian_sol << 2, 0, 0, 0, 0, 0, 0, 0, 0;
448 
449  EXPECT_EQ_MATRIX(sparse_hessian, hessian_sol, 1e-5);
450 
451  // compute sparse eigen hessian directly
453  optim.computeSparseHessianInequalities(sparse_hessian_eigen);
454  EXPECT_EQ_MATRIX(sparse_hessian_eigen, hessian_sol, 1e-5);
455 }
456 
458 {
460 
461  Eigen::VectorXd x(3);
462  x[0] = 2;
463  x[1] = 3;
464  x[2] = 4;
466 
467  // set bounds
468  Eigen::VectorXd lb(3);
469  lb[0] = -CORBO_INF_DBL;
470  lb[1] = -2;
471  lb[2] = -CORBO_INF_DBL;
472  optim.setLowerBounds(lb);
473 
474  // optim.setUpperBound(0, CORBO_INF_DBL); //<- Should be initialized with CORBO_INF_DBL
475  optim.setUpperBound(1, 5);
476  optim.setUpperBound(2, 6);
477 
478  // check bounds
479  Eigen::VectorXd lb_return(optim.getParameterDimension());
480  Eigen::VectorXd ub_return(optim.getParameterDimension());
481  optim.getBounds(lb_return, ub_return);
482 
483  EXPECT_EQ_MATRIX(lb_return, lb, 0.01);
484  EXPECT_GE(ub_return[0], CORBO_INF_DBL);
485  EXPECT_EQ(ub_return[1], 5);
486  EXPECT_EQ(ub_return[2], 6);
487 
488  // check dimension of active bounds
489  EXPECT_EQ(optim.finiteBoundsDimension(), 3);
490  EXPECT_EQ(optim.finiteCombinedBoundsDimension(), 2);
491 
492  // Jacobian for bounds
493 
494  // Reduced Jacobian for active bounds only
495  Eigen::MatrixXd active_bound_jacobian(optim.finiteCombinedBoundsDimension(), optim.getParameterDimension());
496  optim.computeDenseJacobianFiniteCombinedBounds(active_bound_jacobian);
497 
498  EXPECT_ZERO_MATRIX(active_bound_jacobian, 1e-5);
499 
500  // now change parameters such that bounds are violated
501  optim.setParameterValue(1, -3); // violating bounds now
502  optim.setParameterValue(2, 6.01); // here as well
503  Eigen::MatrixXd active_bound_jacobian_sol(2, 3);
504  active_bound_jacobian_sol << 0, -1, 0, 0, 0, 1;
505 
506  optim.computeDenseJacobianFiniteCombinedBounds(active_bound_jacobian);
507  EXPECT_EQ_MATRIX(active_bound_jacobian, active_bound_jacobian_sol, 1e-5);
508 }
509 
511 {
513 
514  Eigen::VectorXd x(3);
515  x[0] = 2;
516  x[1] = 3;
517  x[2] = 4;
519 
520  // set bounds
521  Eigen::VectorXd lb(3);
522  lb[0] = -CORBO_INF_DBL;
523  lb[1] = -2;
524  lb[2] = -CORBO_INF_DBL;
525  optim.setLowerBounds(lb);
526 
527  // optim.setUpperBound(0, CORBO_INF_DBL); //<- Should be initialized with CORBO_INF_DBL
528  optim.setUpperBound(1, 5);
529  optim.setUpperBound(2, 6);
530 
531  // Reduced Jacobian for active bounds only
532  // compute sparse jacobian
534 
535  Eigen::VectorXi irow(nnz);
536  Eigen::VectorXi icol(nnz);
537  Eigen::VectorXd values(nnz);
540 
541  // convert to Eigen type for convenient comparison
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());
546  EXPECT_ZERO_MATRIX(sparse_jacobian.toDense(), 1e-5);
547 
548  // compute sparse eigen matrix directly
551  EXPECT_ZERO_MATRIX(sparse_jacobian.toDense(), 1e-5);
552 
553  // now change parameters such that bounds are violated
554  optim.setParameterValue(1, -3); // violating bounds now
555  optim.setParameterValue(2, 6.01); // here as well
556 
558 
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());
562 
563  Eigen::MatrixXd active_bound_jacobian_sol(2, 3);
564  active_bound_jacobian_sol << 0, -1, 0, 0, 0, 1;
565 
566  EXPECT_EQ_MATRIX(sparse_jacobian, active_bound_jacobian_sol, 1e-5);
567 
568  // compute sparse eigen matrix directly
570  EXPECT_EQ_MATRIX(sparse_jacob_eigen, active_bound_jacobian_sol, 1e-5);
571 }
572 
573 TEST_F(TestStandardOptimizationProblem, test_combined_jacobian_sparse)
574 {
576 
577  Eigen::VectorXd x(3);
578  x[0] = 2;
579  x[1] = -3;
580  x[2] = 7;
582 
583  // set bounds
584  Eigen::VectorXd lb(3);
585  lb[0] = -CORBO_INF_DBL;
586  lb[1] = -2;
587  lb[2] = -CORBO_INF_DBL;
588  optim.setLowerBounds(lb);
589 
590  // optim.setUpperBound(0, CORBO_INF_DBL); //<- Should be initialized with CORBO_INF_DBL
591  optim.setUpperBound(1, 5);
592  optim.setUpperBound(2, 6);
593 
594  // define objective function
595  // f = (x^2 +1)^2
596  auto objective = [](const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> values) { values[0] = x[0] * x[0] + 1; };
597  optim.setObjectiveFunction(objective, 1, true);
598 
599  // define equality constraint function
600  // ceq = [x1^2+1; x2+x3-10]
601  auto ceq = [](const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> values) {
602  values[0] = x[0] * x[0] + 1;
603  values[1] = x[1] + x[2] - 10;
604  };
606 
607  // define inequality constraint function
608  // c = [x1^2+1; x2+x3-9]
609  auto c = [](const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> values) {
610  values[0] = x[0] * x[0] + 1;
611  values[1] = x[1] + x[2] - 9;
612  };
614 
615  int dim_obj_lsq = optim.getLsqObjectiveDimension();
616  int dim_eq = optim.getEqualityDimension();
617  int dim_ineq = optim.getInequalityDimension();
618  int dim_bounds = optim.finiteCombinedBoundsDimension();
619 
620  int dim_total = dim_obj_lsq + dim_eq + dim_ineq + dim_bounds;
621 
623  optim.computeCombinedSparseJacobian(jacobian, true, true, true, true, true, 2, 3, 4);
624 
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;
627 
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;
631  EXPECT_EQ_MATRIX(jacobian, sol1, 1e-5);
632 
633  optim.computeCombinedSparseJacobian(jacobian, true, true, true, true, false, 2, 3, 4);
634 
635  Eigen::MatrixXd sol2 = sol1;
636  sol2(4, 1) = 1 * 3;
637  sol2(4, 2) = 1 * 3;
638  EXPECT_EQ_MATRIX(jacobian, sol2, 1e-5);
639 
640  // test triplet version
641  Eigen::MatrixXd sol3(5, 3);
642  sol3 << 4, 0, 0, 4, 0, 0, 0, 1, 1, 4, 0, 0, 0, 1, 1;
643 
645 
646  Eigen::VectorXi jac_irow(jac_nnz);
647  Eigen::VectorXi jac_icol(jac_nnz);
648  Eigen::VectorXd jac_values(jac_nnz);
651 
652  // convert to Eigen type for convenient comparison
653  std::vector<Eigen::Triplet<double>> triplet_list;
654  corbo::convert_triplet(jac_irow, jac_icol, jac_values, triplet_list);
655  Eigen::SparseMatrix<double> sparse_hessian(dim_obj_lsq + dim_eq + dim_ineq, optim.getParameterDimension());
656  sparse_hessian.setFromTriplets(triplet_list.begin(), triplet_list.end());
657  EXPECT_EQ_MATRIX(sparse_hessian, sol3, 1e-5);
658 
659  // now with multipliers
660  double multiplier_obj_lsq = 5;
661  Eigen::Vector2d multiplier_eq(2, 3);
662  Eigen::Vector2d multiplier_ineq(3, 4);
663 
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);
669 
670  jac_values.setZero();
671  optim.computeCombinedSparseJacobiansValues(jac_values, true, true, true, &multiplier_obj_lsq, multiplier_eq.data(), multiplier_ineq.data());
672  corbo::convert_triplet(jac_irow, jac_icol, jac_values, triplet_list);
673  sparse_hessian.setZero();
674  sparse_hessian.setFromTriplets(triplet_list.begin(), triplet_list.end());
675  EXPECT_EQ_MATRIX(sparse_hessian, sol3, 1e-5);
676 }
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)
Definition: macros.h:53
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.
Scalar * x
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)
Definition: misc.cpp:29
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)
RealScalar c
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)
virtual void computeSparseJacobianInequalitiesValues(Eigen::Ref< Eigen::VectorXd > values, const double *multipliers=nullptr)
constexpr const double CORBO_INF_DBL
Representation for infinity (double version)
void getBounds(Eigen::Ref< Eigen::VectorXd > lb, Eigen::Ref< Eigen::VectorXd > ub) override
Get lower and upper bound vector.
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:192
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)
Definition: SparseMatrix.h:993
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 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)
Definition: macros.h:39
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)


control_box_rst
Author(s): Christoph Rösmann
autogenerated on Mon Feb 28 2022 22:07:52