12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Working nonlinear inequality constraints with unit tests.

release/4.3a0
krunalchande 2014-12-22 18:20:44 -05:00 committed by thduynguyen
parent 4f92492e34
commit cc0e5cd3ca
4 changed files with 272 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam_unstable/linear/LinearInequality.h
View File

@ -51,7 +51,7 @@ public:
}
/** Conversion from JacobianFactor */
explicit LinearInequality(const JacobianFactor& jf) : Base(jf), dualKey_(dualKey), active_(true) {
explicit LinearInequality(const JacobianFactor& jf, Key dualKey) : Base(jf), dualKey_(dualKey), active_(true) {
if (!jf.isConstrained()) {
throw std::runtime_error("Cannot convert an unconstrained JacobianFactor to LinearEquality");
}

40
gtsam_unstable/nonlinear/NonlinearConstraint.h
View File

@ -70,7 +70,7 @@ public:
* @param constraintDim number of dimensions of the constraint error function
*/
NonlinearConstraint1(Key key, Key dualKey, size_t constraintDim = 1) :
Base(noiseModel::Constrained::All(constraintDim), key), NonlinearConstraint(dualKey) {
Base(noiseModel::Constrained::All(constraintDim), key), NonlinearConstraint(dualKey) {
}
virtual ~NonlinearConstraint1() {
@ -108,8 +108,12 @@ public:
lG11sum += -lambda[i] * G11[i];
}
return boost::make_shared<HessianFactor>(Base::key(), lG11sum,
zero(X1Dim), 100.0);
std::cout << "lG11sum: " << lG11sum << std::endl;
HessianFactor::shared_ptr hf(new HessianFactor(Base::key(), lG11sum,
zero(X1Dim), 100.0));
hf->print("HessianFactor: ");
return hf;
}
/** evaluate Hessians for lambda factors */
@ -149,8 +153,8 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
}
};
// \class NonlinearConstraint1
@ -191,7 +195,7 @@ public:
* @param constraintDim number of dimensions of the constraint error function
*/
NonlinearConstraint2(Key j1, Key j2, Key dualKey, size_t constraintDim = 1) :
Base(noiseModel::Constrained::All(constraintDim), j1, j2), NonlinearConstraint(dualKey) {
Base(noiseModel::Constrained::All(constraintDim), j1, j2), NonlinearConstraint(dualKey) {
}
virtual ~NonlinearConstraint2() {
@ -229,7 +233,7 @@ public:
// Combine the Lagrange-multiplier part into this constraint factor
Matrix lG11sum = zeros(G11[0].rows(), G11[0].cols()), lG12sum = zeros(
G12[0].rows(), G12[0].cols()), lG22sum = zeros(G22[0].rows(),
G22[0].cols());
G22[0].cols());
for (size_t i = 0; i < lambda.rows(); ++i) {
lG11sum += -lambda[i] * G11[i];
lG12sum += -lambda[i] * G12[i];
@ -249,7 +253,7 @@ public:
boost::function<Vector(const X1&, const X2&)> vecH1(
boost::bind(&This::vectorizeH1t, this, _1, _2)), vecH2(
boost::bind(&This::vectorizeH2t, this, _1, _2));
boost::bind(&This::vectorizeH2t, this, _1, _2));
Matrix G11all = numericalDerivative21(vecH1, x1, x2, 1e-5);
Matrix G12all = numericalDerivative22(vecH1, x1, x2, 1e-5);
@ -302,8 +306,8 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
}
};
// \class NonlinearConstraint2
@ -346,7 +350,7 @@ public:
* @param constraintDim number of dimensions of the constraint error function
*/
NonlinearConstraint3(Key j1, Key j2, Key j3, Key dualKey, size_t constraintDim = 1) :
Base(noiseModel::Constrained::All(constraintDim), j1, j2, j3), NonlinearConstraint(dualKey) {
Base(noiseModel::Constrained::All(constraintDim), j1, j2, j3), NonlinearConstraint(dualKey) {
}
virtual ~NonlinearConstraint3() {
@ -387,9 +391,9 @@ public:
// Combine the Lagrange-multiplier part into this constraint factor
Matrix lG11sum = zeros(G11[0].rows(), G11[0].cols()), lG12sum = zeros(
G12[0].rows(), G12[0].cols()), lG13sum = zeros(G13[0].rows(),
G13[0].cols()), lG22sum = zeros(G22[0].rows(), G22[0].cols()), lG23sum =
zeros(G23[0].rows(), G23[0].cols()), lG33sum = zeros(G33[0].rows(),
G33[0].cols());
G13[0].cols()), lG22sum = zeros(G22[0].rows(), G22[0].cols()), lG23sum =
zeros(G23[0].rows(), G23[0].cols()), lG33sum = zeros(G33[0].rows(),
G33[0].cols());
for (size_t i = 0; i < lambda.rows(); ++i) {
lG11sum += -lambda[i] * G11[i];
lG12sum += -lambda[i] * G12[i];
@ -467,8 +471,8 @@ public:
boost::function<Vector(const X1&, const X2&, const X3&)> vecH1(
boost::bind(&This::vectorizeH1t, this, _1, _2, _3)), vecH2(
boost::bind(&This::vectorizeH2t, this, _1, _2, _3)), vecH3(
boost::bind(&This::vectorizeH3t, this, _1, _2, _3));
boost::bind(&This::vectorizeH2t, this, _1, _2, _3)), vecH3(
boost::bind(&This::vectorizeH3t, this, _1, _2, _3));
Matrix G11all = numericalDerivative31(vecH1, x1, x2, x3, 1e-5);
Matrix G12all = numericalDerivative32(vecH1, x1, x2, x3, 1e-5);
@ -549,8 +553,8 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
}
};
// \class NonlinearConstraint3

95
gtsam_unstable/nonlinear/NonlinearInequality.h
View File

@ -11,30 +11,11 @@
namespace gtsam {
class NonlinearInequality : public NonlinearConstraint {
bool active_;
typedef NonlinearConstraint Base;
public:
typedef boost::shared_ptr<NonlinearInequality> shared_ptr;
public:
/// Construct with dual key
NonlinearInequality(Key dualKey) : Base(dualKey), active_(true) {}
/**
* compute the HessianFactor of the (-dual * constraintHessian) for the qp subproblem's objective function
*/
virtual GaussianFactor::shared_ptr multipliedHessian(const Values& x,
const VectorValues& duals) const = 0;
};
/* ************************************************************************* */
/** A convenient base class for creating a nonlinear equality constraint with 1
* variables. To derive from this class, implement evaluateError(). */
template<class VALUE>
class NonlinearInequality1: public NonlinearConstraint1<VALUE>, public NonlinearInequality {
class NonlinearInequality1: public NonlinearConstraint1<VALUE> {
public:
@ -62,8 +43,8 @@ public:
* @param j key of the variable
* @param constraintDim number of dimensions of the constraint error function
*/
NonlinearInequality1(Key key, Key dualKey, size_t constraintDim = 1) :
Base(noiseModel::Constrained::All(constraintDim), key), NonlinearConstraint(dualKey) {
NonlinearInequality1(Key key, Key dualKey) :
Base(key, dualKey, 1) {
}
virtual ~NonlinearInequality1() {
@ -74,9 +55,63 @@ public:
* If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2).
*/
virtual double
computeError(const X&, boost::optional<Matrix&> H1 = boost::none) const = 0;
/** predefine evaluateError to return a 1-dimension vector */
virtual Vector
evaluateError(const X&, boost::optional<Matrix&> H1 = boost::none) const {
return (Vector(1) << computeError(X, H1));
evaluateError(const X& x, boost::optional<Matrix&> H1 = boost::none) const {
return (Vector(1) << computeError(x, H1)).finished();
}
//
// virtual GaussianFactor::shared_ptr multipliedHessian(const Values& x,
// const VectorValues& duals) const {
// return Base::multipliedHessian(x, duals);
// }
};
// \class NonlinearConstraint1
/* ************************************************************************* */
/** A convenient base class for creating your own NonlinearConstraint with 2
* variables. To derive from this class, implement evaluateError(). */
template<class VALUE1, class VALUE2>
class NonlinearInequality2: public NonlinearConstraint2<VALUE1, VALUE2> {
public:
// typedefs for value types pulled from keys
typedef VALUE1 X1;
typedef VALUE2 X2;
protected:
typedef NonlinearConstraint2<VALUE1, VALUE2> Base;
typedef NonlinearInequality2<VALUE1, VALUE2> This;
private:
static const int X1Dim = traits::dimension<VALUE1>::value;
static const int X2Dim = traits::dimension<VALUE2>::value;
public:
/**
* Default Constructor for I/O
*/
NonlinearInequality2() {
}
/**
* Constructor
* @param j1 key of the first variable
* @param j2 key of the second variable
* @param constraintDim number of dimensions of the constraint error function
*/
NonlinearInequality2(Key j1, Key j2, Key dualKey) :
Base(j1, j2, 1) {
}
virtual ~NonlinearInequality2() {
}
/**
@ -85,9 +120,17 @@ public:
* both the function evaluation and its derivative(s) in X1 (and/or X2).
*/
virtual double
computeError(const X&, boost::optional<Matrix&> H1 = boost::none) const = 0;
computeError(const X1&, const X2&, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const = 0;
/** predefine evaluateError to return a 1-dimension vector */
virtual Vector
evaluateError(const X1& x1, const X2& x2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return (Vector(1) << computeError(x1, x2, H1, H2)).finished();
}
};
// \class NonlinearConstraint1
// \class NonlinearConstraint2
} /* namespace gtsam */

233
gtsam_unstable/nonlinear/tests/testSQPSimple.cpp
View File

@ -23,6 +23,7 @@
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam_unstable/nonlinear/NonlinearConstraint.h>
#include <gtsam_unstable/nonlinear/NonlinearInequality.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
@ -102,47 +103,31 @@ public:
}
/**
* Return true if the error is <= 0.0
* Return true if the all errors are <= 0.0
*/
bool checkFeasibility(const Values& values, double tol) const {
bool checkFeasibilityAndComplimentary(const Values& values, const VectorValues& duals, double tol) const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
NoiseModelFactor::shared_ptr noiseModelFactor = boost::dynamic_pointer_cast<NoiseModelFactor>(
factor);
Vector error = noiseModelFactor->unwhitenedError(values);
// TODO: Do we need to check if it's active or not?
// Primal feasibility condition: all constraints need to be <= 0.0
if (error[0] > tol) {
return false;
}
}
return true;
}
/**
* Return true if the max absolute error all factors is less than a tolerance
*/
bool checkDualFeasibility(const VectorValues& duals, double tol) const {
BOOST_FOREACH(const Vector& dual, duals){
if (dual[0] < 0.0) {
return false;
}
}
return true;
}
/**
* Return true if the max absolute error all factors is less than a tolerance
*/
bool checkComplimentaryCondition(const Values& values, const VectorValues& duals, double tol) const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
NoiseModelFactor::shared_ptr noiseModelFactor = boost::dynamic_pointer_cast<NoiseModelFactor>(
// Complimentary condition: errors of active constraints need to be 0.0
NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(
factor);
Vector error = noiseModelFactor->unwhitenedError(values);
if (error[0] > 0.0) {
Key dualKey = constraint->dualKey();
if (!duals.exists(dualKey)) continue; // if dualKey doesn't exist, it is an inactive constraint!
if (fabs(error[0]) > tol) // for active constraint, the error should be 0.0
return false;
}
}
return true;
}
};
struct NLP {
@ -181,22 +166,49 @@ public:
}
/// Check if \nabla f(x) - \lambda * \nabla c(x) == 0
bool isDualFeasible(const VectorValues& delta) const {
return delta.vector().lpNorm<Eigen::Infinity>() < errorTol
&& nlp_.linearInequalities.checkDualFeasibility(errorTol);
// return false;
bool isStationary(const VectorValues& delta) const {
return delta.vector().lpNorm<Eigen::Infinity>() < errorTol;
}
/// Check if c(x) == 0
/// Check if c_E(x) == 0
bool isPrimalFeasible(const SQPSimpleState& state) const {
return nlp_.linearEqualities.checkFeasibility(state.values, errorTol)
&& nlp_.nonlinearEqualities.checkFeasibility(state.values, errorTol)
&& nlp_.linearInequalities.checkFeasibility(state.values, errorTol);
&& nlp_.nonlinearEqualities.checkFeasibility(state.values, errorTol);
}
/**
* Dual variables of inequality constraints need to be >=0
* For active inequalities, the dual needs to be > 0
* For inactive inequalities, they need to be == 0. However, we don't compute
* dual variables for inactive constraints in the qp subproblem, so we don't care.
*/
bool isDualFeasible(const VectorValues& duals) const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, nlp_.linearInequalities) {
NonlinearConstraint::shared_ptr inequality = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
Key dualKey = inequality->dualKey();
if (!duals.exists(dualKey)) continue; // should be inactive constraint!
double dual = duals.at(dualKey)[0]; // because we only support single-valued inequalities
if (dual < 0.0)
return false;
}
return true;
}
/**
* Check complimentary slackness condition:
* For all inequality constraints,
* dual * constraintError(primals) == 0.
* If the constraint is active, we need to check constraintError(primals) == 0, and ignore the dual
* If it is inactive, the dual should be 0, regardless of the error. However, we don't compute
* dual variables for inactive constraints in the QP subproblem, so we don't care.
*/
bool isComplementary(const SQPSimpleState& state) const {
return nlp_.linearInequalities.checkFeasibilityAndComplimentary(state.values, state.duals, errorTol);
}
/// Check convergence
bool checkConvergence(const SQPSimpleState& state, const VectorValues& delta) const {
return isPrimalFeasible(state) & isDualFeasible(delta);
return isStationary(delta) && isPrimalFeasible(state) && isDualFeasible(state.duals) && isComplementary(state);
}
/**
@ -275,7 +287,6 @@ public:
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam_unstable/nonlinear/NonlinearInequality.h>
using namespace std;
using namespace gtsam::symbol_shorthand;
@ -403,7 +414,6 @@ TEST_UNSAFE(testSQPSimple, quadraticCostNonlinearConstraint) {
Values actualSolution = sqpSimple.optimize(initialValues).first;
CHECK(assert_equal(expectedSolution, actualSolution, 1e-10));
actualSolution.print("actualSolution: ");
}
//******************************************************************************
@ -417,6 +427,13 @@ public:
return pose.x();
}
void evaluateHessians(const Pose3& pose, std::vector<Matrix>& G11) const {
Matrix G11all = Z_6x6;
Vector rT1 = pose.rotation().matrix().row(0);
G11all.block<3,3>(3,0) = skewSymmetric(rT1);
G11.push_back(G11all);
}
Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H = boost::none) const {
if (H)
*H = (Matrix(1,6) << zeros(1,3), pose.rotation().matrix().row(0)).finished();
@ -445,54 +462,164 @@ TEST(testSQPSimple, poseOnALine) {
Values actualSolution = sqpSimple.optimize(initialValues).first;
CHECK(assert_equal(expectedSolution, actualSolution, 1e-10));
actualSolution.print("actualSolution: ");
Pose3 pose(Rot3::ypr(0.1, 0.2, 0.3), Point3());
Matrix hessian = numericalHessian<Pose3>(boost::bind(&LineConstraintX::computeError, constraint, _1), pose, 1e-2);
cout << "hessian: \n" << hessian << endl;
}
//******************************************************************************
/// x + y - 1 <= 0
class InequalityProblem1 : public NonlinearInequality2<double, double> {
typedef NonlinearInequality2<double, double> Base;
public:
InequalityProblem1(Key xK, Key yK, Key dualKey) : Base(xK, yK, dualKey) {}
double computeError(const double& x, const double& y,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
if (H1) *H1 = eye(1);
if (H2) *H2 = eye(1);
return x + y - 1.0;
}
};
TEST(testSQPSimple, inequalityConstraint) {
const Key dualKey = 0;
// Simple quadratic cost: x^2 + y^2
// Note the Hessian encodes:
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence here we have G11 = 2, G12 = 0, G22 = 2, g1 = 0, g2 = 0, f = 0
HessianFactor hf(X(1), Y(1), 2.0 * ones(1,1), zero(1), zero(1),
2*ones(1,1), zero(1) , 0);
LinearInequalityFactorGraph inequalities;
LinearInequality linearConstraint(X(1), ones(1), Y(1), ones(1), 1.0, dualKey); // x + y - 1 <= 0
inequalities.push_back(linearConstraint);
// Compare against QP
QP qp;
qp.cost.add(hf);
qp.inequalities = inequalities;
// instantiate QPsolver
QPSolver qpSolver(qp);
// create initial values for optimization
VectorValues initialVectorValues;
initialVectorValues.insert(X(1), zero(1));
initialVectorValues.insert(Y(1), zero(1));
VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first;
//Instantiate NLP
NLP nlp;
Values linPoint;
linPoint.insert<Vector1>(X(1), zero(1));
linPoint.insert<Vector1>(Y(1), zero(1));
nlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor
nlp.linearInequalities.add(InequalityProblem1(X(1), Y(1), dualKey));
Values initialValues;
initialValues.insert(X(1), 1.0);
initialValues.insert(Y(1), -10.0);
// Instantiate SQP
SQPSimple sqpSimple(nlp);
Values actualValues = sqpSimple.optimize(initialValues).first;
DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualValues.at<double>(X(1)), 1e-10);
DOUBLES_EQUAL(expectedSolution.at(Y(1))[0], actualValues.at<double>(Y(1)), 1e-10);
}
//******************************************************************************
const size_t X_AXIS = 0;
const size_t Y_AXIS = 1;
const size_t Z_AXIS = 2;
/**
* Inequality boundary constraint
* x <= bound
* Inequality boundary constraint on one axis (x, y or z)
* axis <= bound
*/
class UpperBoundX : public NonlinearInequality1<Pose3> {
class AxisUpperBound : public NonlinearInequality1<Pose3> {
typedef NonlinearInequality1<Pose3> Base;
size_t axis_;
double bound_;
public:
UpperBoundX(Key key, double bound, Key dualKey) : Base(key, dualKey, 1), bound_(bound) {
AxisUpperBound(Key key, size_t axis, double bound, Key dualKey) : Base(key, dualKey), axis_(axis), bound_(bound) {
}
double computeError(const Pose3& pose, boost::optional<Matrix&> H = boost::none) const {
if (H)
*H = (Matrix(1,6) << zeros(1,3), pose.rotation().matrix().row(0)).finished();
return pose.x() - bound_;
*H = (Matrix(1,6) << zeros(1,3), pose.rotation().matrix().row(axis_)).finished();
return pose.translation().vector()[axis_] - bound_;
}
};
TEST(testSQPSimple, poseOnALine) {
const Key dualKey = 0;
/**
* Inequality boundary constraint on one axis (x, y or z)
* bound <= axis
*/
class AxisLowerBound : public NonlinearInequality1<Pose3> {
typedef NonlinearInequality1<Pose3> Base;
size_t axis_;
double bound_;
public:
AxisLowerBound(Key key, size_t axis, double bound, Key dualKey) : Base(key, dualKey), axis_(axis), bound_(bound) {
}
double computeError(const Pose3& pose, boost::optional<Matrix&> H = boost::none) const {
if (H)
*H = (Matrix(1,6) << zeros(1,3), -pose.rotation().matrix().row(axis_)).finished();
return -pose.translation().vector()[axis_] + bound_;
}
};
TEST(testSQPSimple, poseWithABoundary) {
const Key dualKey = 0;
//Instantiate NLP
NLP nlp;
nlp.cost.add(PriorFactor<Pose3>(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(-1, 0, 0)), noiseModel::Unit::Create(6)));
UpperBoundX constraint(X(1), 0, dualKey);
nlp.nonlinearInequalities.add(constraint);
nlp.cost.add(PriorFactor<Pose3>(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)), noiseModel::Unit::Create(6)));
AxisUpperBound constraint(X(1), X_AXIS, 0, dualKey);
nlp.linearInequalities.add(constraint);
Values initialValues;
initialValues.insert(X(1), Pose3(Rot3::ypr(0.3, 0.2, 0.3), Point3(-1,0,0)));
initialValues.insert(X(1), Pose3(Rot3::ypr(0.3, 0.2, 0.3), Point3(1, 0, 0)));
Values expectedSolution;
expectedSolution.insert(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3()));
expectedSolution.insert(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(0, 0, 0)));
// Instantiate SQP
SQPSimple sqpSimple(nlp);
Values actualSolution = sqpSimple.optimize(initialValues).first;
CHECK(assert_equal(expectedSolution, actualSolution, 1e-10));
actualSolution.print("actualSolution: ");
}
TEST(testSQPSimple, poseWithinA2DBox) {
const Key dualKey = 0;
//Instantiate NLP
NLP nlp;
nlp.cost.add(PriorFactor<Pose3>(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(10, 0.5, 0)), noiseModel::Unit::Create(6)));
nlp.linearInequalities.add(AxisLowerBound(X(1), X_AXIS, -1, dualKey));
nlp.linearInequalities.add(AxisUpperBound(X(1), X_AXIS, 1, dualKey));
nlp.linearInequalities.add(AxisLowerBound(X(1), Y_AXIS, -1, dualKey));
nlp.linearInequalities.add(AxisUpperBound(X(1), Y_AXIS, 1, dualKey));
Values initialValues;
initialValues.insert(X(1), Pose3(Rot3::ypr(0.3, 0.2, 0.3), Point3(1, 0, 0)));
Values expectedSolution;
expectedSolution.insert(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 0.5, 0)));
// Instantiate SQP
SQPSimple sqpSimple(nlp);
Values actualSolution = sqpSimple.optimize(initialValues).first;
CHECK(assert_equal(expectedSolution, actualSolution, 1e-10));
//TODO: remove printing, refactoring,
}
//******************************************************************************