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

27
gtsam_unstable/nonlinear/LinearConstraintSQP.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizerParams.h>
#include <gtsam_unstable/nonlinear/LinearEqualityFactorGraph.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
* ONLY linear constraints are supported.
*/
class LinearConstraintSQP {
LinearConstraintNLP lcnlp_;
static const double errorTol = 1e-5;
LinearConstraintSQPParams params_;
public:
LinearConstraintSQP(const LinearConstraintNLP& lcnlp) :
lcnlp_(lcnlp) {
LinearConstraintSQP(const LinearConstraintNLP& lcnlp,
const LinearConstraintSQPParams& params = LinearConstraintSQPParams()) :
lcnlp_(lcnlp), params_(params) {
}
/// Check if \nabla f(x) - \lambda * \nabla c(x) == 0
@ -104,7 +121,7 @@ public:
/**
* 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
VectorValues initializeDuals() const;
@ -112,7 +129,7 @@ public:
/**
* 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() {
cout<<"here: "<<endl;
TestResult tr;
return TestRegistry::runAllTests(tr);
}