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

rename classes

release/4.3a0
thduynguyen 2015-02-24 23:40:53 -05:00
parent d2f919e632
commit d9773da125
10 changed files with 602 additions and 349 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
gtsam_unstable/nonlinear/ConstrainedFactor.h Normal file
View File

@ -0,0 +1,52 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ConstrainedFactor.h
* @brief
* @author Duy-Nguyen Ta
* @date Sep 30, 2013
*/
#pragma once
#include <gtsam/base/types.h>
#include <boost/shared_ptr.hpp>
namespace gtsam {
/**
* A base class for all ConstrainedFactor factors,
* containing additional information for the constraint,
* e.g., a unique key for the dual variable associated with it,
* and special treatments for nonlinear constraint linearization in SQP.
*
* Derived classes of ConstrainedFactor should also inherit from
* NoiseModelFactorX to reuse the normal linearization procedure in NonlinearFactor
*/
class ConstrainedFactor {
protected:
Key dualKey_; //!< Unique key for the dual variable associated with this constraint
public:
typedef boost::shared_ptr<ConstrainedFactor> shared_ptr;
public:
/// Construct with dual key
ConstrainedFactor(Key dualKey) : dualKey_(dualKey) {}
/// Return the dual key
Key dualKey() const { return dualKey_; }
};
} /* namespace gtsam */

58
gtsam_unstable/nonlinear/LCNLPSolver.cpp → gtsam_unstable/nonlinear/LinearConstraintSQP.cpp
View File

@ -10,34 +10,34 @@
* -------------------------------------------------------------------------- */
/**
* @file LCNLPSolver.cpp
* @file LinearConstraintSQP.cpp
* @author Duy-Nguyen Ta
* @author Krunal Chande
* @author Luca Carlone
* @date Dec 15, 2014
*/
#include <gtsam_unstable/nonlinear/LCNLPSolver.h>
#include <gtsam_unstable/nonlinear/LinearConstraintSQP.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <iostream>
namespace gtsam {
/* ************************************************************************* */
bool LCNLPSolver::isStationary(const VectorValues& delta) const {
bool LinearConstraintSQP::isStationary(const VectorValues& delta) const {
return delta.vector().lpNorm<Eigen::Infinity>() < errorTol;
}
/* ************************************************************************* */
bool LCNLPSolver::isPrimalFeasible(const LCNLPState& state) const {
bool LinearConstraintSQP::isPrimalFeasible(const LinearConstraintNLPState& state) const {
return lcnlp_.linearEqualities.checkFeasibility(state.values, errorTol);
}
/* ************************************************************************* */
bool LCNLPSolver::isDualFeasible(const VectorValues& duals) const {
bool LinearConstraintSQP::isDualFeasible(const VectorValues& duals) const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities){
NonlinearConstraint::shared_ptr inequality
= boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
ConstrainedFactor::shared_ptr inequality
= boost::dynamic_pointer_cast<ConstrainedFactor>(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
@ -49,24 +49,24 @@ bool LCNLPSolver::isDualFeasible(const VectorValues& duals) const {
}
/* ************************************************************************* */
bool LCNLPSolver::isComplementary(const LCNLPState& state) const {
bool LinearConstraintSQP::isComplementary(const LinearConstraintNLPState& state) const {
return lcnlp_.linearInequalities.checkFeasibilityAndComplimentary(
state.values, state.duals, errorTol);
}
/* ************************************************************************* */
bool LCNLPSolver::checkConvergence(const LCNLPState& state,
bool LinearConstraintSQP::checkConvergence(const LinearConstraintNLPState& state,
const VectorValues& delta) const {
return isStationary(delta) && isPrimalFeasible(state)
&& isDualFeasible(state.duals) && isComplementary(state);
}
/* ************************************************************************* */
VectorValues LCNLPSolver::initializeDuals() const {
VectorValues LinearConstraintSQP::initializeDuals() const {
VectorValues duals;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearEqualities){
NonlinearConstraint::shared_ptr constraint
= boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
ConstrainedFactor::shared_ptr constraint
= boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
duals.insert(constraint->dualKey(), zero(factor->dim()));
}
@ -74,22 +74,7 @@ VectorValues LCNLPSolver::initializeDuals() const {
}
/* ************************************************************************* */
std::pair<Values, VectorValues> LCNLPSolver::optimize(
const Values& initialValues, bool useWarmStart, bool debug) const {
LCNLPState state(initialValues);
state.duals = initializeDuals();
while (!state.converged && state.iterations < 100) {
if (debug)
std::cout << "state: iteration " << state.iterations << std::endl;
state = iterate(state, useWarmStart, debug);
}
if (debug)
std::cout << "Number of iterations: " << state.iterations << std::endl;
return std::make_pair(state.values, state.duals);
}
/* ************************************************************************* */
LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart,
LinearConstraintNLPState LinearConstraintSQP::iterate(const LinearConstraintNLPState& state, bool useWarmStart,
bool debug) const {
// construct the qp subproblem
@ -120,7 +105,7 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart,
// duals.print("duals = ");
// update new state
LCNLPState newState;
LinearConstraintNLPState newState;
newState.values = state.values.retract(delta);
newState.duals = duals;
newState.converged = checkConvergence(newState, delta);
@ -132,5 +117,20 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart,
return newState;
}
/* ************************************************************************* */
std::pair<Values, VectorValues> LinearConstraintSQP::optimize(
const Values& initialValues, bool useWarmStart, bool debug) const {
LinearConstraintNLPState state(initialValues);
state.duals = initializeDuals();
while (!state.converged && state.iterations < 100) {
if (debug)
std::cout << "state: iteration " << state.iterations << std::endl;
state = iterate(state, useWarmStart, debug);
}
if (debug)
std::cout << "Number of iterations: " << state.iterations << std::endl;
return std::make_pair(state.values, state.duals);
}
} // namespace gtsam

43
gtsam_unstable/nonlinear/LCNLPSolver.h → gtsam_unstable/nonlinear/LinearConstraintSQP.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file LCNLPSolver.h
* @file LinearConstraintSQP.h
* @author Duy-Nguyen Ta
* @author Krunal Chande
* @author Luca Carlone
@ -19,8 +19,8 @@
#pragma once
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h>
#include <gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h>
#include <gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h>
#include <gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h>
namespace gtsam {
@ -28,26 +28,26 @@ namespace gtsam {
* Nonlinear Programming problem with
* only linear constraints, encoded in factor graphs
*/
struct LCNLP {
struct LinearConstraintNLP {
NonlinearFactorGraph cost;
NonlinearEqualityFactorGraph linearEqualities;
NonlinearInequalityFactorGraph linearInequalities;
LinearEqualityFactorGraph linearEqualities;
LinearInequalityFactorGraph linearInequalities;
};
/**
* State of LCNLPSolver before/after each iteration
* State of LinearConstraintSQP before/after each iteration
*/
struct LCNLPState {
Values values;
VectorValues duals;
bool converged;
size_t iterations;
struct LinearConstraintNLPState {
Values values; //!< current solution
VectorValues duals; //!< current guess of the dual variables
bool converged; //!< convergence flag
size_t iterations; //!< number of iterations
/// Default constructor
LCNLPState() : values(), duals(), converged(false), iterations(0) {}
LinearConstraintNLPState() : values(), duals(), converged(false), iterations(0) {}
/// Constructor with an initialValues
LCNLPState(const Values& initialValues) :
LinearConstraintNLPState(const Values& initialValues) :
values(initialValues), duals(VectorValues()), converged(false), iterations(0) {
}
@ -66,11 +66,11 @@ struct LCNLPState {
* Simple SQP optimizer to solve nonlinear constrained problems
* ONLY linear constraints are supported.
*/
class LCNLPSolver {
LCNLP lcnlp_;
class LinearConstraintSQP {
LinearConstraintNLP lcnlp_;
static const double errorTol = 1e-5;
public:
LCNLPSolver(const LCNLP& lcnlp) :
LinearConstraintSQP(const LinearConstraintNLP& lcnlp) :
lcnlp_(lcnlp) {
}
@ -78,7 +78,7 @@ public:
bool isStationary(const VectorValues& delta) const;
/// Check if c_E(x) == 0
bool isPrimalFeasible(const LCNLPState& state) const;
bool isPrimalFeasible(const LinearConstraintNLPState& state) const;
/**
* Dual variables of inequality constraints need to be >=0
@ -96,16 +96,17 @@ public:
* 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 LCNLPState& state) const;
bool isComplementary(const LinearConstraintNLPState& state) const;
/// Check convergence
bool checkConvergence(const LCNLPState& state, const VectorValues& delta) const;
bool checkConvergence(const LinearConstraintNLPState& state, const VectorValues& delta) const;
/**
* Single iteration of SQP
*/
LCNLPState iterate(const LCNLPState& state, bool useWarmStart = true, bool debug = false) const;
LinearConstraintNLPState iterate(const LinearConstraintNLPState& state, bool useWarmStart = true, bool debug = false) const;
/// Intialize all dual variables to zeros
VectorValues initializeDuals() const;
/**

192
gtsam_unstable/nonlinear/LinearEqualityFactor.h Normal file
View File

@ -0,0 +1,192 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file LinearEqualityFactor.h
* @brief Linear equality factors at the nonlinear level
* @author Duy-Nguyen Ta
* @date Sep 30, 2013
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
namespace gtsam {
/* ************************************************************************* */
/** A convenient base class for creating a linear equality constraint with 1
* variables. To derive from this class, implement evaluateError().
* Warning: It is the user's responsibility to make sure the Hessian is approximately zero
*
* TODO: Should we check Hessian = 0 automatically to make sure it's linear, like SNOPT?
* TODO: Sometimes the true Hessian is not 0, but an approximate one with a different retract is zero
* and that could also work [Absil07fcm]
*/
template<class VALUE>
class LinearEqualityFactor1: public NoiseModelFactor1<VALUE>,
public ConstrainedFactor {
public:
// typedefs for value types pulled from keys
typedef VALUE X;
protected:
typedef NoiseModelFactor1<VALUE> Base;
typedef LinearEqualityFactor1<VALUE> This;
public:
/**
* Default Constructor for I/O
*/
LinearEqualityFactor1() {
}
/**
* Constructor
* @param j key of the variable
* @param constraintDim number of dimensions of the constraint error function
*/
LinearEqualityFactor1(Key key, Key dualKey, size_t constraintDim = 1) :
Base(noiseModel::Constrained::All(constraintDim), key), ConstrainedFactor(
dualKey) {
}
virtual ~LinearEqualityFactor1() {
}
/**
* Override this method to finish implementing a binary factor.
* 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 Vector
evaluateError(const X&, boost::optional<Matrix&> H1 = boost::none) const = 0;
};
// \class LinearEqualityFactor1
/* ************************************************************************* */
/** A convenient base class for creating a linear equality constraint with 2
* variables. To derive from this class, implement evaluateError().
* Warning: It is the user's responsibility to make sure the Hessian is approximately zero
*
* TODO: Should we check Hessian = 0 automatically to make sure it's linear, like SNOPT?
* TODO: Sometimes the true Hessian is not 0, but an approximate one with a different retract is zero
* and that could also work [Absil07fcm]
*/
template<class VALUE1, class VALUE2>
class LinearEqualityFactor2: public NoiseModelFactor2<VALUE1, VALUE2>,
public ConstrainedFactor {
public:
// typedefs for value types pulled from keys
typedef VALUE1 X1;
typedef VALUE2 X2;
protected:
typedef NoiseModelFactor2<VALUE1, VALUE2> Base;
typedef LinearEqualityFactor2<VALUE1, VALUE2> This;
public:
/**
* Default Constructor for I/O
*/
LinearEqualityFactor2() {
}
/**
* 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
*/
LinearEqualityFactor2(Key j1, Key j2, Key dualKey, size_t constraintDim = 1) :
Base(noiseModel::Constrained::All(constraintDim), j1, j2), ConstrainedFactor(
dualKey) {
}
virtual ~LinearEqualityFactor2() {
}
/**
* Override this method to finish implementing a binary factor.
* 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 Vector
evaluateError(const X1&, const X2&, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const = 0;
};
// \class LinearEqualityFactor2
/* ************************************************************************* */
/** A convenient base class for creating a linear equality constraint with 3
* variables. To derive from this class, implement evaluateError().
* Warning: It is the user's responsibility to make sure the Hessian is approximately zero
*
* TODO: Should we check Hessian = 0 automatically to make sure it's linear, like SNOPT?
* TODO: Sometimes the true Hessian is not 0, but an approximate one with a different retract is zero
* and that could also work [Absil07fcm]
*/
template<class VALUE1, class VALUE2, class VALUE3>
class LinearEqualityFactor3: public NoiseModelFactor3<VALUE1, VALUE2, VALUE3>,
public ConstrainedFactor {
public:
// typedefs for value types pulled from keys
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef VALUE3 X3;
protected:
typedef NoiseModelFactor3<VALUE1, VALUE2, VALUE3> Base;
typedef LinearEqualityFactor3<VALUE1, VALUE2, VALUE3> This;
public:
/**
* Default Constructor for I/O
*/
LinearEqualityFactor3() {
}
/**
* 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
*/
LinearEqualityFactor3(Key j1, Key j2, Key j3, Key dualKey,
size_t constraintDim = 1) :
Base(noiseModel::Constrained::All(constraintDim), j1, j2, j3), ConstrainedFactor(
dualKey) {
}
virtual ~LinearEqualityFactor3() {
}
/**
* Override this method to finish implementing a binary factor.
* 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 Vector
evaluateError(const X1&, const X2&, const X3&, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const = 0;
};
// \class LinearEqualityFactor3
} /* namespace gtsam */

35
gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h → gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file NonlinearEqualityFactorGraph.h
* @file LinearEqualityFactorGraph.h
* @author Duy-Nguyen Ta
* @author Krunal Chande
* @author Luca Carlone
@ -18,26 +18,29 @@
*/
#pragma once
#include <gtsam_unstable/linear/LinearEqualityFactorGraph.h>
#include <gtsam_unstable/nonlinear/NonlinearConstraint.h>
#include <gtsam_unstable/linear/EqualityFactorGraph.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
namespace gtsam {
class NonlinearEqualityFactorGraph: public FactorGraph<NonlinearFactor> {
/**
* FactorGraph container for linear equality factors c(x)==0 at the nonlinear level
*/
class LinearEqualityFactorGraph: public FactorGraph<NonlinearFactor> {
public:
/// Default constructor
NonlinearEqualityFactorGraph() {
LinearEqualityFactorGraph() {
}
/// Linearize to a LinearEqualityFactorGraph
LinearEqualityFactorGraph::shared_ptr linearize(
/// Linearize to a EqualityFactorGraph
EqualityFactorGraph::shared_ptr linearize(
const Values& linearizationPoint) const {
LinearEqualityFactorGraph::shared_ptr linearGraph(
new LinearEqualityFactorGraph());
EqualityFactorGraph::shared_ptr linearGraph(
new EqualityFactorGraph());
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast<JacobianFactor>(
factor->linearize(linearizationPoint));
NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
ConstrainedFactor::shared_ptr constraint = boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
linearGraph->add(LinearEquality(*jacobian, constraint->dualKey()));
}
return linearGraph;
@ -58,18 +61,6 @@ public:
return true;
}
// /**
// * Additional cost for -lambda*ConstraintHessian for SQP
// */
// GaussianFactorGraph::shared_ptr multipliedHessians(const Values& values, const VectorValues& duals) const {
// GaussianFactorGraph::shared_ptr constrainedHessians(new GaussianFactorGraph());
// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) {
// NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
// constrainedHessians->push_back(constraint->multipliedHessian(values, duals));
// }
// return constrainedHessians;
// }
};
}

60
gtsam_unstable/nonlinear/NonlinearInequality.h → gtsam_unstable/nonlinear/LinearInequalityFactor.h
View File

@ -9,7 +9,7 @@
* -------------------------------------------------------------------------- */
/**
* @file NonlinearInequality.h
* @file LinearInequalityFactor.h
* @brief
* @author Duy-Nguyen Ta
* @date Sep 30, 2013
@ -17,15 +17,18 @@
#pragma once
#include <gtsam_unstable/nonlinear/NonlinearConstraint.h>
#include <gtsam_unstable/nonlinear/LinearEqualityFactor.h>
namespace gtsam {
/* ************************************************************************* */
/** A convenient base class for creating a nonlinear equality constraint with 1
* variables. To derive from this class, implement evaluateError(). */
/**
* A convenient base class for creating a linear inequality constraint, e.g., Ax <= 0,
* at the nonlinear level with 1 variable.
* To derive from this class, implement computeError().
*/
template<class VALUE>
class NonlinearInequality1: public NonlinearConstraint1<VALUE> {
class LinearInequalityFactor1: public LinearEqualityFactor1<VALUE> {
public:
@ -33,19 +36,15 @@ public:
typedef VALUE X;
protected:
typedef NonlinearConstraint1<VALUE> Base;
typedef NonlinearInequality1<VALUE> This;
private:
static const int X1Dim = traits<VALUE>::dimension;
typedef LinearEqualityFactor1<VALUE> Base;
typedef LinearInequalityFactor1<VALUE> This;
public:
/**
* Default Constructor for I/O
*/
NonlinearInequality1() {
LinearInequalityFactor1() {
}
/**
@ -53,11 +52,11 @@ public:
* @param j key of the variable
* @param constraintDim number of dimensions of the constraint error function
*/
NonlinearInequality1(Key key, Key dualKey) :
LinearInequalityFactor1(Key key, Key dualKey) :
Base(key, dualKey, 1) {
}
virtual ~NonlinearInequality1() {
virtual ~LinearInequalityFactor1() {
}
/**
@ -73,20 +72,18 @@ public:
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
// \class LinearEqualityFactor1
/* ************************************************************************* */
/** A convenient base class for creating your own NonlinearConstraint with 2
* variables. To derive from this class, implement evaluateError(). */
/**
* A convenient base class for creating a linear inequality constraint, e.g., Ax <= 0,
* at the nonlinear level with 1 variable.
* To derive from this class, implement computeError().
*/
template<class VALUE1, class VALUE2>
class NonlinearInequality2: public NonlinearConstraint2<VALUE1, VALUE2> {
class LinearInequalityFactor2: public LinearEqualityFactor2<VALUE1, VALUE2> {
public:
@ -95,20 +92,15 @@ public:
typedef VALUE2 X2;
protected:
typedef NonlinearConstraint2<VALUE1, VALUE2> Base;
typedef NonlinearInequality2<VALUE1, VALUE2> This;
private:
static const int X1Dim = traits<VALUE1>::dimension;
static const int X2Dim = traits<VALUE2>::dimension;
typedef LinearEqualityFactor2<VALUE1, VALUE2> Base;
typedef LinearInequalityFactor2<VALUE1, VALUE2> This;
public:
/**
* Default Constructor for I/O
*/
NonlinearInequality2() {
LinearInequalityFactor2() {
}
/**
@ -117,11 +109,11 @@ public:
* @param j2 key of the second variable
* @param constraintDim number of dimensions of the constraint error function
*/
NonlinearInequality2(Key j1, Key j2, Key dualKey) :
LinearInequalityFactor2(Key j1, Key j2, Key dualKey) :
Base(j1, j2, 1) {
}
virtual ~NonlinearInequality2() {
virtual ~LinearInequalityFactor2() {
}
/**
@ -140,7 +132,7 @@ public:
return (Vector(1) << computeError(x1, x2, H1, H2)).finished();
}
};
// \class NonlinearConstraint2
// \class LinearEqualityFactor2
} /* namespace gtsam */

55
gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h → gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file NonlinearInequalityFactorGraph.h
* @file LinearInequalityFactorGraph.h
* @author Duy-Nguyen Ta
* @author Krunal Chande
* @author Luca Carlone
@ -19,26 +19,29 @@
#pragma once
#include <gtsam/linear/VectorValues.h>
#include <gtsam_unstable/linear/LinearInequalityFactorGraph.h>
#include <gtsam_unstable/nonlinear/NonlinearConstraint.h>
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
namespace gtsam {
class NonlinearInequalityFactorGraph : public FactorGraph<NonlinearFactor> {
/**
* FactorGraph container for linear inequality factors c(x)<=0 at the nonlinear level
*/
class LinearInequalityFactorGraph: public FactorGraph<NonlinearFactor> {
public:
/// Default constructor
NonlinearInequalityFactorGraph() {
LinearInequalityFactorGraph() {
}
/// Linearize to a LinearInequalityFactorGraph
LinearInequalityFactorGraph::shared_ptr linearize(
const Values& linearizationPoint) const {
LinearInequalityFactorGraph::shared_ptr linearGraph(
new LinearInequalityFactorGraph());
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
/// Linearize to a InequalityFactorGraph
InequalityFactorGraph::shared_ptr linearize(const Values& linearizationPoint) const {
InequalityFactorGraph::shared_ptr linearGraph(new InequalityFactorGraph());
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) {
JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast<JacobianFactor>(
factor->linearize(linearizationPoint));
NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
ConstrainedFactor::shared_ptr constraint
= boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
linearGraph->add(LinearInequality(*jacobian, constraint->dualKey()));
}
return linearGraph;
@ -47,26 +50,32 @@ public:
/**
* Return true if the all errors are <= 0.0
*/
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);
bool checkFeasibilityAndComplimentary(const Values& values,
const VectorValues& dualValues, 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);
// Primal feasibility condition: all constraints need to be <= 0.0
if (error[0] > tol) {
if (error[0] > tol)
return false;
}
// Complimentary condition: errors of active constraints need to be 0.0
NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(
factor);
ConstrainedFactor::shared_ptr constraint
= boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
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;
// if dualKey doesn't exist in dualValues, it must be an inactive constraint!
if (!dualValues.exists(dualKey))
continue;
// for active constraint, the error should be 0.0
if (fabs(error[0]) > tol)
return false;
}
return true;
}
};

50
gtsam_unstable/nonlinear/NonlinearConstraint.h
View File

@ -23,47 +23,15 @@
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
namespace gtsam {
/**
* A base class for all NonlinearConstraint factors,
* containing additional information for the constraint,
* e.g., a unique key for the dual variable associated with it,
* and special treatments for nonlinear constraint linearization in SQP.
*
* Derived classes of NonlinearConstraint should also inherit from
* NoiseModelFactorX to reuse the normal linearization procedure in NonlinearFactor
*/
class NonlinearConstraint {
protected:
Key dualKey_; //!< Unique key for the dual variable associated with this constraint
public:
typedef boost::shared_ptr<NonlinearConstraint> shared_ptr;
public:
/// Construct with dual key
NonlinearConstraint(Key dualKey) : dualKey_(dualKey) {}
/// Return the dual key
Key dualKey() const { return dualKey_; }
/**
* Special linearization for nonlinear constraint in SQP
* 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 NonlinearConstraint1: public NoiseModelFactor1<VALUE>, public NonlinearConstraint {
class NonlinearConstraint1: public NoiseModelFactor1<VALUE>, public ConstrainedFactor {
public:
@ -92,7 +60,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), ConstrainedFactor(dualKey) {
}
virtual ~NonlinearConstraint1() {
@ -179,10 +147,10 @@ private:
// \class NonlinearConstraint1
/* ************************************************************************* */
/** A convenient base class for creating your own NonlinearConstraint with 2
/** A convenient base class for creating your own ConstrainedFactor with 2
* variables. To derive from this class, implement evaluateError(). */
template<class VALUE1, class VALUE2>
class NonlinearConstraint2: public NoiseModelFactor2<VALUE1, VALUE2>, public NonlinearConstraint {
class NonlinearConstraint2: public NoiseModelFactor2<VALUE1, VALUE2>, public ConstrainedFactor {
public:
@ -214,7 +182,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), ConstrainedFactor(dualKey) {
}
virtual ~NonlinearConstraint2() {
@ -332,10 +300,10 @@ private:
// \class NonlinearConstraint2
/* ************************************************************************* */
/** A convenient base class for creating your own NonlinearConstraint with 3
/** A convenient base class for creating your own ConstrainedFactor with 3
* variables. To derive from this class, implement evaluateError(). */
template<class VALUE1, class VALUE2, class VALUE3>
class NonlinearConstraint3: public NoiseModelFactor3<VALUE1, VALUE2, VALUE3>, public NonlinearConstraint {
class NonlinearConstraint3: public NoiseModelFactor3<VALUE1, VALUE2, VALUE3>, public ConstrainedFactor {
public:
@ -369,7 +337,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), ConstrainedFactor(dualKey) {
}
virtual ~NonlinearConstraint3() {

192
gtsam_unstable/nonlinear/tests/testLCNLPSolver.cpp → gtsam_unstable/nonlinear/tests/testLCNLPSolver2.cpp
View File

@ -36,178 +36,6 @@ using namespace gtsam::symbol_shorthand;
using namespace gtsam;
const double tol = 1e-10;
//******************************************************************************
// x + y - 1 = 0
class ConstraintProblem1 : public NonlinearConstraint2<double, double> {
typedef NonlinearConstraint2<double, double> Base;
public:
ConstraintProblem1(Key xK, Key yK, Key dualKey) : Base(xK, yK, dualKey, 1) {}
// x + y - 1
Vector evaluateError(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 (Vector(1) << x + y - 1.0).finished();
}
};
TEST_DISABLED(testlcnlpSolver, QPProblem) {
const Key dualKey = 0;
// Simple quadratic cost: x1^2 + x2^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);
LinearEqualityFactorGraph equalities;
LinearEquality linearConstraint(X(1), ones(1), Y(1), ones(1), 1*ones(1), dualKey); // x + y - 1 = 0
equalities.push_back(linearConstraint);
// Compare against QP
QP qp;
qp.cost.add(hf);
qp.equalities = equalities;
// instantiate QPsolver
QPSolver qpSolver(qp);
// create initial values for optimization
VectorValues initialVectorValues;
initialVectorValues.insert(X(1), zero(1));
initialVectorValues.insert(Y(1), ones(1));
VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first;
//Instantiate LCNLP
LCNLP lcnlp;
Values linPoint;
linPoint.insert<Vector1>(X(1), zero(1));
linPoint.insert<Vector1>(Y(1), zero(1));
lcnlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor
lcnlp.linearEqualities.add(ConstraintProblem1(X(1), Y(1), dualKey));
Values initialValues;
initialValues.insert(X(1), 0.0);
initialValues.insert(Y(1), 0.0);
// Instantiate LCNLPSolver
LCNLPSolver lcnlpSolver(lcnlp);
Values actualValues = lcnlpSolver.optimize(initialValues).first;
DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualValues.at<double>(X(1)), 1e-100);
DOUBLES_EQUAL(expectedSolution.at(Y(1))[0], actualValues.at<double>(Y(1)), 1e-100);
}
//******************************************************************************
class LineConstraintX : public NonlinearConstraint1<Pose3> {
typedef NonlinearConstraint1<Pose3> Base;
public:
LineConstraintX(Key key, Key dualKey) : Base(key, dualKey, 1) {
}
double computeError(const Pose3& pose) const {
return pose.x();
}
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();
return (Vector(1) << pose.x()).finished();
}
};
TEST_DISABLED(testlcnlpSolver, poseOnALine) {
const Key dualKey = 0;
//Instantiate LCNLP
LCNLP lcnlp;
lcnlp.cost.add(PriorFactor<Pose3>(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)), noiseModel::Unit::Create(6)));
LineConstraintX constraint(X(1), dualKey);
lcnlp.linearEqualities.add(constraint);
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()));
// Instantiate LCNLPSolver
LCNLPSolver lcnlpSolver(lcnlp);
Values actualSolution = lcnlpSolver.optimize(initialValues).first;
CHECK(assert_equal(expectedSolution, actualSolution, 1e-10));
Pose3 pose(Rot3::ypr(0.1, 0.2, 0.3), Point3());
Matrix hessian = numericalHessian<Pose3>(boost::bind(&LineConstraintX::computeError, constraint, _1), pose, 1e-2);
}
//******************************************************************************
/// 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_DISABLED(testlcnlpSolver, 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 LCNLP
LCNLP lcnlp;
Values linPoint;
linPoint.insert<Vector1>(X(1), zero(1));
linPoint.insert<Vector1>(Y(1), zero(1));
lcnlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor
lcnlp.linearInequalities.add(InequalityProblem1(X(1), Y(1), dualKey));
Values initialValues;
initialValues.insert(X(1), 1.0);
initialValues.insert(Y(1), -10.0);
// Instantiate LCNLPSolver
LCNLPSolver lcnlpSolver(lcnlp);
Values actualValues = lcnlpSolver.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;
@ -253,7 +81,8 @@ public:
}
};
TEST_DISABLED(testlcnlpSolver, poseWithABoundary) {
//******************************************************************************
TEST(testlcnlpSolver, poseWithABoundary) {
const Key dualKey = 0;
//Instantiate LCNLP
@ -275,7 +104,8 @@ TEST_DISABLED(testlcnlpSolver, poseWithABoundary) {
CHECK(assert_equal(expectedSolution, actualSolution, 1e-10));
}
TEST_DISABLED(testlcnlpSolver, poseWithNoConstraints) {
//******************************************************************************
TEST(testlcnlpSolver, poseWithNoConstraints) {
//Instantiate LCNLP
LCNLP lcnlp;
@ -294,7 +124,8 @@ TEST_DISABLED(testlcnlpSolver, poseWithNoConstraints) {
CHECK(assert_equal(expectedSolution, actualSolution, 1e-10));
}
TEST_DISABLED(testlcnlpSolver, poseWithinA2DBox) {
//******************************************************************************
TEST(testlcnlpSolver, poseWithinA2DBox) {
const Key dualKey = 0;
//Instantiate LCNLP
@ -318,7 +149,8 @@ TEST_DISABLED(testlcnlpSolver, poseWithinA2DBox) {
CHECK(assert_equal(expectedSolution, actualSolution, 1e-10));
}
TEST_DISABLED(testlcnlpSolver, posesInA2DBox) {
//******************************************************************************
TEST(testlcnlpSolver, posesInA2DBox) {
const double xLowerBound = -3.0,
xUpperBound = 5.0,
yLowerBound = -1.0,
@ -385,7 +217,8 @@ TEST_DISABLED(testlcnlpSolver, posesInA2DBox) {
CHECK(assert_equal(expectedSolution, actualSolution, 1e-5));
}
TEST_DISABLED(testlcnlpSolver, iterativePoseinBox) {
//******************************************************************************
TEST(testlcnlpSolver, iterativePoseinBox) {
const double xLowerBound = -1.0,
xUpperBound = 1.0,
yLowerBound = -1.0,
@ -471,12 +304,13 @@ TEST_DISABLED(testlcnlpSolver, iterativePoseinBox) {
}
//******************************************************************************
double testHessian(const Pose3& X) {
return X.transform_from(Point3(0,0,0)).x();
}
Pose3 pose(Rot3::ypr(0.1, 0.4, 0.7), Point3(1, 9.5, 7.3));
TEST_DISABLED(testlcnlpSolver, testingHessian) {
TEST(testlcnlpSolver, testingHessian) {
Matrix H = numericalHessian(testHessian, pose, 1e-5);
}
@ -484,7 +318,7 @@ double h3(const Vector6& v) {
return pose.retract(v).translation().x();
}
TEST_DISABLED(testlcnlpSolver, NumericalHessian3) {
TEST(testlcnlpSolver, NumericalHessian3) {
Matrix6 expected;
expected.setZero();
Vector6 z;

214
gtsam_unstable/nonlinear/tests/testLinearConstraintSQP.cpp Normal file
View File

@ -0,0 +1,214 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testLinearConstraintSQP.cpp
* @brief Unit tests for testLinearConstraintSQP
* @author Duy-Nguyen Ta
* @author Krunal Chande
* @author Luca Carlone
* @date Dec 15, 2014
*/
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam_unstable/nonlinear/LinearConstraintSQP.h>
#include <gtsam_unstable/nonlinear/LinearInequalityFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
using namespace std;
using namespace gtsam::symbol_shorthand;
using namespace gtsam;
const double tol = 1e-10;
//******************************************************************************
// x + y - 1 = 0
class ConstraintProblem1 : public LinearEqualityFactor2<double, double> {
typedef LinearEqualityFactor2<double, double> Base;
public:
ConstraintProblem1(Key xK, Key yK, Key dualKey) : Base(xK, yK, dualKey, 1) {}
// x + y - 1
Vector evaluateError(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 (Vector(1) << x + y - 1.0).finished();
}
};
TEST(testlcnlpSolver, QPProblem) {
const Key dualKey = 0;
// Simple quadratic cost: x1^2 + x2^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);
EqualityFactorGraph equalities;
LinearEquality linearConstraint(X(1), ones(1), Y(1), ones(1), 1*ones(1), dualKey); // x + y - 1 = 0
equalities.push_back(linearConstraint);
// Compare against QP
QP qp;
qp.cost.add(hf);
qp.equalities = equalities;
// instantiate QPsolver
QPSolver qpSolver(qp);
// create initial values for optimization
VectorValues initialVectorValues;
initialVectorValues.insert(X(1), zero(1));
initialVectorValues.insert(Y(1), ones(1));
VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first;
//Instantiate LinearConstraintNLP
LinearConstraintNLP lcnlp;
Values linPoint;
linPoint.insert<Vector1>(X(1), zero(1));
linPoint.insert<Vector1>(Y(1), zero(1));
lcnlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor
lcnlp.linearEqualities.add(ConstraintProblem1(X(1), Y(1), dualKey));
Values initialValues;
initialValues.insert(X(1), 0.0);
initialValues.insert(Y(1), 0.0);
// Instantiate LinearConstraintSQP
LinearConstraintSQP lcnlpSolver(lcnlp);
Values actualValues = lcnlpSolver.optimize(initialValues).first;
DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualValues.at<double>(X(1)), 1e-100);
DOUBLES_EQUAL(expectedSolution.at(Y(1))[0], actualValues.at<double>(Y(1)), 1e-100);
}
//******************************************************************************
/**
* A simple linear constraint on Pose3's x coordinate enforcing x==0
*/
class LineConstraintX : public LinearEqualityFactor1<Pose3> {
typedef LinearEqualityFactor1<Pose3> Base;
public:
LineConstraintX(Key key, Key dualKey) : Base(key, dualKey, 1) {
}
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();
return (Vector(1) << pose.x()).finished();
}
};
TEST(testlcnlpSolver, poseOnALine) {
const Key dualKey = 0;
//Instantiate LinearConstraintNLP
LinearConstraintNLP lcnlp;
lcnlp.cost.add(PriorFactor<Pose3>(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)), noiseModel::Unit::Create(6)));
LineConstraintX constraint(X(1), dualKey);
lcnlp.linearEqualities.add(constraint);
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()));
// Instantiate LinearConstraintSQP
LinearConstraintSQP lcnlpSolver(lcnlp);
Values actualSolution = lcnlpSolver.optimize(initialValues).first;
CHECK(assert_equal(expectedSolution, actualSolution, 1e-10));
}
//******************************************************************************
/// x + y - 1 <= 0
class InequalityProblem1 : public LinearInequalityFactor2<double, double> {
typedef LinearInequalityFactor2<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(testlcnlpSolver, 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);
InequalityFactorGraph 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 LinearConstraintNLP
LinearConstraintNLP lcnlp;
Values linPoint;
linPoint.insert<Vector1>(X(1), zero(1));
linPoint.insert<Vector1>(Y(1), zero(1));
lcnlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor
lcnlp.linearInequalities.add(InequalityProblem1(X(1), Y(1), dualKey));
Values initialValues;
initialValues.insert(X(1), 1.0);
initialValues.insert(Y(1), -10.0);
// Instantiate LinearConstraintSQP
LinearConstraintSQP lcnlpSolver(lcnlp);
Values actualValues = lcnlpSolver.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);
}
//******************************************************************************
int main() {
cout<<"here: "<<endl;
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************