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

Added warm start for initializing active set.

release/4.3a0
krunalchande 2014-12-23 18:16:22 -05:00 committed by thduynguyen
parent f4a4ce4325
commit 3e352f109e
3 changed files with 51 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
gtsam_unstable/linear/QPSolver.cpp
View File

@ -181,13 +181,13 @@ QPState QPSolver::iterate(const QPState& state) const {
// If all inequality constraints are satisfied: We have the solution!!
if (leavingFactor < 0) {
return QPState(newValues, duals, state.workingSet, true);
return QPState(newValues, duals, state.workingSet, true, state.iterations+1);
}
else {
// Inactivate the leaving constraint
LinearInequalityFactorGraph newWorkingSet = state.workingSet;
newWorkingSet.at(leavingFactor)->inactivate();
return QPState(newValues, duals, newWorkingSet, false);
return QPState(newValues, duals, newWorkingSet, false, state.iterations+1);
}
}
else {
@ -210,23 +210,33 @@ QPState QPSolver::iterate(const QPState& state) const {
// step!
newValues = state.values + alpha * p;
return QPState(newValues, state.duals, newWorkingSet, false);
return QPState(newValues, state.duals, newWorkingSet, false, state.iterations+1);
}
}
//******************************************************************************
LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
const LinearInequalityFactorGraph& inequalities,
const VectorValues& initialValues) const {
const VectorValues& initialValues, const VectorValues& duals) const {
LinearInequalityFactorGraph workingSet;
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities){
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities) {
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
double error = workingFactor->error(initialValues);
if (fabs(error)>1e-7){
workingFactor->inactivate();
} else {
if (duals.exists(workingFactor->dualKey())) {
workingFactor->activate();
}
else {
if (duals.size() > 0) {
workingFactor->inactivate();
} else {
double error = workingFactor->error(initialValues);
if (fabs(error)<1e-7) {
workingFactor->activate();
}
else {
workingFactor->inactivate();
}
}
}
workingSet.push_back(workingFactor);
}
return workingSet;
@ -234,18 +244,18 @@ LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
//******************************************************************************
pair<VectorValues, VectorValues> QPSolver::optimize(
const VectorValues& initialValues) const {
const VectorValues& initialValues, const VectorValues& duals) const {
// Initialize workingSet from the feasible initialValues
LinearInequalityFactorGraph workingSet =
identifyActiveConstraints(qp_.inequalities, initialValues);
QPState state(initialValues, VectorValues(), workingSet, false);
identifyActiveConstraints(qp_.inequalities, initialValues, duals);
QPState state(initialValues, duals, workingSet, false, 0);
/// main loop of the solver
while (!state.converged) {
state = iterate(state);
}
std::cout << "Number of inner iterations: " << state.iterations << std::endl;
return make_pair(state.values, state.duals);
}

12
gtsam_unstable/linear/QPSolver.h
View File

@ -31,17 +31,18 @@ struct QPState {
VectorValues duals;
LinearInequalityFactorGraph workingSet;
bool converged;
size_t iterations;
/// default constructor
QPState() :
values(), duals(), workingSet(), converged(false) {
values(), duals(), workingSet(), converged(false), iterations(0) {
}
/// constructor with initial values
QPState(const VectorValues& initialValues, const VectorValues& initialDuals,
const LinearInequalityFactorGraph& initialWorkingSet, bool _converged) :
const LinearInequalityFactorGraph& initialWorkingSet, bool _converged, size_t _iterations) :
values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged(
_converged) {
_converged), iterations(_iterations) {
}
};
@ -183,7 +184,8 @@ public:
*/
LinearInequalityFactorGraph identifyActiveConstraints(
const LinearInequalityFactorGraph& inequalities,
const VectorValues& initialValues) const;
const VectorValues& initialValues,
const VectorValues& duals = VectorValues()) const;
/** Optimize with a provided initial values
* For this version, it is the responsibility of the caller to provide
@ -191,7 +193,7 @@ public:
* @return a pair of <primal, dual> solutions
*/
std::pair<VectorValues, VectorValues> optimize(
const VectorValues& initialValues) const;
const VectorValues& initialValues, const VectorValues& duals = VectorValues()) const;
};

27
gtsam_unstable/nonlinear/LCNLPSolver.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam_unstable/nonlinear/LCNLPSolver.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <iostream>
namespace gtsam {
@ -75,12 +76,13 @@ std::pair<Values, VectorValues> LCNLPSolver::optimize(const Values& initialValue
while (!state.converged && state.iterations < 100) {
state = iterate(state);
}
std::cout << "Number of iterations: " << state.iterations << std::endl;
return std::make_pair(state.values, state.duals);
}
/* ************************************************************************* */
LCNLPState LCNLPSolver::iterate(const LCNLPState& state) const {
static const bool debug = false;
static const bool debug = true;
// construct the qp subproblem
QP qp;
@ -88,19 +90,29 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state) const {
qp.equalities.add(*lcnlp_.linearEqualities.linearize(state.values));
qp.inequalities.add(*lcnlp_.linearInequalities.linearize(state.values));
if (debug)
qp.print("QP subproblem:");
// if (debug)
// qp.print("QP subproblem:");
// solve the QP subproblem
VectorValues delta, duals;
QPSolver qpSolver(qp);
boost::tie(delta, duals) = qpSolver.optimize();
if (state.iterations == 0)
boost::tie(delta, duals) = qpSolver.optimize();
else {
VectorValues zeroInitialValues;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, state.values) {
zeroInitialValues.insert(key_value.key, zero(key_value.value.dim()));
}
boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals);
}
if (debug)
state.values.print("state.values: ");
if (debug)
delta.print("delta = ");
if (debug)
duals.print("duals = ");
// if (debug)
// duals.print("duals = ");
// update new state
LCNLPState newState;
@ -109,6 +121,9 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state) const {
newState.converged = checkConvergence(newState, delta);
newState.iterations = state.iterations + 1;
if (debug)
newState.print("newState: ");
return newState;
}
}