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

support params and verbosity debug info for LinearConstraintSQP

release/4.3a0
thduynguyen 2015-02-25 08:07:40 -05:00
parent d80caeb44b
commit bd16c52e7c
3 changed files with 43 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
gtsam_unstable/nonlinear/LinearConstraintSQP.cpp
View File

@ -25,17 +25,17 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
bool LinearConstraintSQP::isStationary(const VectorValues& delta) const { bool LinearConstraintSQP::isStationary(const VectorValues& delta) const {
return delta.vector().lpNorm<Eigen::Infinity>() < errorTol; return delta.vector().lpNorm<Eigen::Infinity>() < params_.errorTol;
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool LinearConstraintSQP::isPrimalFeasible(const LinearConstraintNLPState& state) const { bool LinearConstraintSQP::isPrimalFeasible(const LinearConstraintNLPState& state) const {
return lcnlp_.linearEqualities.checkFeasibility(state.values, errorTol); return lcnlp_.linearEqualities.checkFeasibility(state.values, params_.errorTol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool LinearConstraintSQP::isDualFeasible(const VectorValues& duals) const { bool LinearConstraintSQP::isDualFeasible(const VectorValues& duals) const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities){ BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities) {
ConstrainedFactor::shared_ptr inequality ConstrainedFactor::shared_ptr inequality
= boost::dynamic_pointer_cast<ConstrainedFactor>(factor); = boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
Key dualKey = inequality->dualKey(); Key dualKey = inequality->dualKey();
@ -51,7 +51,7 @@ bool LinearConstraintSQP::isDualFeasible(const VectorValues& duals) const {
/* ************************************************************************* */ /* ************************************************************************* */
bool LinearConstraintSQP::isComplementary(const LinearConstraintNLPState& state) const { bool LinearConstraintSQP::isComplementary(const LinearConstraintNLPState& state) const {
return lcnlp_.linearInequalities.checkFeasibilityAndComplimentary( return lcnlp_.linearInequalities.checkFeasibilityAndComplimentary(
state.values, state.duals, errorTol); state.values, state.duals, params_.errorTol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -74,8 +74,8 @@ VectorValues LinearConstraintSQP::initializeDuals() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
LinearConstraintNLPState LinearConstraintSQP::iterate(const LinearConstraintNLPState& state, bool useWarmStart, LinearConstraintNLPState LinearConstraintSQP::iterate(
bool debug) const { const LinearConstraintNLPState& state) const {
// construct the qp subproblem // construct the qp subproblem
QP qp; QP qp;
@ -83,8 +83,8 @@ LinearConstraintNLPState LinearConstraintSQP::iterate(const LinearConstraintNLPS
qp.equalities.add(*lcnlp_.linearEqualities.linearize(state.values)); qp.equalities.add(*lcnlp_.linearEqualities.linearize(state.values));
qp.inequalities.add(*lcnlp_.linearInequalities.linearize(state.values)); qp.inequalities.add(*lcnlp_.linearInequalities.linearize(state.values));
// if (debug) if(params_.verbosity >= NonlinearOptimizerParams::LINEAR)
// qp.print("QP subproblem:"); qp.print("QP subproblem:");
// solve the QP subproblem // solve the QP subproblem
VectorValues delta, duals; VectorValues delta, duals;
@ -94,15 +94,10 @@ LinearConstraintNLPState LinearConstraintSQP::iterate(const LinearConstraintNLPS
zeroInitialValues.insert(key_value.key, zero(key_value.value.dim())); zeroInitialValues.insert(key_value.key, zero(key_value.value.dim()));
} }
boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals, boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals,
useWarmStart); params_.warmStart);
if (debug) if(params_.verbosity >= NonlinearOptimizerParams::DELTA)
state.values.print("state.values: "); delta.print("Delta");
if (debug)
delta.print("delta = ");
// if (debug)
// duals.print("duals = ");
// update new state // update new state
LinearConstraintNLPState newState; LinearConstraintNLPState newState;
@ -111,23 +106,25 @@ LinearConstraintNLPState LinearConstraintSQP::iterate(const LinearConstraintNLPS
newState.converged = checkConvergence(newState, delta); newState.converged = checkConvergence(newState, delta);
newState.iterations = state.iterations + 1; newState.iterations = state.iterations + 1;
if (debug) if(params_.verbosity >= NonlinearOptimizerParams::VALUES) {
newState.print("newState: "); newState.values.print("Values");
newState.duals.print("Duals");
}
return newState; return newState;
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<Values, VectorValues> LinearConstraintSQP::optimize( std::pair<Values, VectorValues> LinearConstraintSQP::optimize(
const Values& initialValues, bool useWarmStart, bool debug) const { const Values& initialValues) const {
LinearConstraintNLPState state(initialValues); LinearConstraintNLPState state(initialValues);
state.duals = initializeDuals(); state.duals = initializeDuals();
while (!state.converged && state.iterations < 100) { while (!state.converged && state.iterations < params_.maxIterations) {
if (debug) if(params_.verbosity >= NonlinearOptimizerParams::ERROR)
std::cout << "state: iteration " << state.iterations << std::endl; std::cout << "Iteration # " << state.iterations << std::endl;
state = iterate(state, useWarmStart, debug); state = iterate(state);
} }
if (debug) if(params_.verbosity >= NonlinearOptimizerParams::TERMINATION)
std::cout << "Number of iterations: " << state.iterations << std::endl; std::cout << "Number of iterations: " << state.iterations << std::endl;
return std::make_pair(state.values, state.duals); return std::make_pair(state.values, state.duals);
} }

27
gtsam_unstable/nonlinear/LinearConstraintSQP.h
View File

@ -19,6 +19,7 @@
#pragma once #pragma once
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizerParams.h>
#include <gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h> #include <gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h>
#include <gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h> #include <gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h>
@ -62,16 +63,32 @@ struct LinearConstraintNLPState {
} }
}; };
/** Parameters for Gauss-Newton optimization, inherits from
* NonlinearOptimizationParams.
*/
class GTSAM_EXPORT LinearConstraintSQPParams : public NonlinearOptimizerParams {
public:
bool warmStart;
LinearConstraintSQPParams() : NonlinearOptimizerParams(), warmStart(false) {}
void setWarmStart(bool _warmStart) {
_warmStart = warmStart;
}
};
/** /**
* Simple SQP optimizer to solve nonlinear constrained problems * Simple SQP optimizer to solve nonlinear constrained problems
* ONLY linear constraints are supported. * ONLY linear constraints are supported.
*/ */
class LinearConstraintSQP { class LinearConstraintSQP {
LinearConstraintNLP lcnlp_; LinearConstraintNLP lcnlp_;
static const double errorTol = 1e-5; LinearConstraintSQPParams params_;
public: public:
LinearConstraintSQP(const LinearConstraintNLP& lcnlp) : LinearConstraintSQP(const LinearConstraintNLP& lcnlp,
lcnlp_(lcnlp) { const LinearConstraintSQPParams& params = LinearConstraintSQPParams()) :
lcnlp_(lcnlp), params_(params) {
} }
/// Check if \nabla f(x) - \lambda * \nabla c(x) == 0 /// Check if \nabla f(x) - \lambda * \nabla c(x) == 0
@ -104,7 +121,7 @@ public:
/** /**
* Single iteration of SQP * Single iteration of SQP
*/ */
LinearConstraintNLPState iterate(const LinearConstraintNLPState& state, bool useWarmStart = true, bool debug = false) const; LinearConstraintNLPState iterate(const LinearConstraintNLPState& state) const;
/// Intialize all dual variables to zeros /// Intialize all dual variables to zeros
VectorValues initializeDuals() const; VectorValues initializeDuals() const;
@ -112,7 +129,7 @@ public:
/** /**
* Main optimization function. new * Main optimization function. new
*/ */
std::pair<Values, VectorValues> optimize(const Values& initialValues, bool useWarmStart = true, bool debug = false) const; std::pair<Values, VectorValues> optimize(const Values& initialValues) const;
}; };
} }

1
gtsam_unstable/nonlinear/tests/testLinearConstraintSQP.cpp
View File

@ -207,7 +207,6 @@ TEST(testlcnlpSolver, inequalityConstraint) {
//****************************************************************************** //******************************************************************************
int main() { int main() {
cout<<"here: "<<endl;
TestResult tr; TestResult tr;
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }