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

Added warmStart flag.

release/4.3a0
krunalchande 2015-01-03 12:32:49 -05:00 committed by thduynguyen
parent 6b2b96ca2a
commit 37fe405872
6 changed files with 1639 additions and 1677 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3284
.cproject
View File

File diff suppressed because it is too large Load Diff

10
gtsam_unstable/linear/QPSolver.cpp
View File

@ -217,15 +217,15 @@ QPState QPSolver::iterate(const QPState& state) const {
//******************************************************************************
LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
const LinearInequalityFactorGraph& inequalities,
const VectorValues& initialValues, const VectorValues& duals) const {
const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const {
LinearInequalityFactorGraph workingSet;
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities) {
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
if (duals.exists(workingFactor->dualKey())) {
if (useWarmStart == true && duals.exists(workingFactor->dualKey())) {
workingFactor->activate();
}
else {
if (duals.size() > 0) {
if (useWarmStart == true && duals.size() > 0) {
workingFactor->inactivate();
} else {
double error = workingFactor->error(initialValues);
@ -244,11 +244,11 @@ LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
//******************************************************************************
pair<VectorValues, VectorValues> QPSolver::optimize(
const VectorValues& initialValues, const VectorValues& duals) const {
const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const {
// Initialize workingSet from the feasible initialValues
LinearInequalityFactorGraph workingSet =
identifyActiveConstraints(qp_.inequalities, initialValues, duals);
identifyActiveConstraints(qp_.inequalities, initialValues, duals, useWarmStart);
QPState state(initialValues, duals, workingSet, false, 0);
/// main loop of the solver

4
gtsam_unstable/linear/QPSolver.h
View File

@ -185,7 +185,7 @@ public:
LinearInequalityFactorGraph identifyActiveConstraints(
const LinearInequalityFactorGraph& inequalities,
const VectorValues& initialValues,
const VectorValues& duals = VectorValues()) const;
const VectorValues& duals = VectorValues(), bool useWarmStart = true) const;
/** Optimize with a provided initial values
* For this version, it is the responsibility of the caller to provide
@ -193,7 +193,7 @@ public:
* @return a pair of <primal, dual> solutions
*/
std::pair<VectorValues, VectorValues> optimize(
const VectorValues& initialValues, const VectorValues& duals = VectorValues()) const;
const VectorValues& initialValues, const VectorValues& duals = VectorValues(), bool useWarmStart = true) const;
};

10
gtsam_unstable/nonlinear/LCNLPSolver.cpp
View File

@ -70,13 +70,13 @@ VectorValues LCNLPSolver::initializeDuals() const {
}
/* ************************************************************************* */
std::pair<Values, VectorValues> LCNLPSolver::optimize(const Values& initialValues, bool debug) 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, debug);
state = iterate(state, useWarmStart, debug);
}
if (debug)
std::cout << "Number of iterations: " << state.iterations << std::endl;
@ -84,7 +84,7 @@ std::pair<Values, VectorValues> LCNLPSolver::optimize(const Values& initialValue
}
/* ************************************************************************* */
LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool debug) const {
LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart, bool debug) const {
// construct the qp subproblem
QP qp;
@ -98,14 +98,14 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool debug) const {
// solve the QP subproblem
VectorValues delta, duals;
QPSolver qpSolver(qp);
if (state.iterations == 0)
if (useWarmStart == false || 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);
boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals, useWarmStart);
}
if (debug)

4
gtsam_unstable/nonlinear/LCNLPSolver.h
View File

@ -104,13 +104,13 @@ public:
/**
* Single iteration of SQP
*/
LCNLPState iterate(const LCNLPState& state, bool debug = false) const;
LCNLPState iterate(const LCNLPState& state, bool useWarmStart = true, bool debug = false) const;
VectorValues initializeDuals() const;
/**
* Main optimization function. new
*/
std::pair<Values, VectorValues> optimize(const Values& initialValues, bool debug = false) const;
std::pair<Values, VectorValues> optimize(const Values& initialValues, bool useWarmStart = true, bool debug = false) const;
};
}

4
gtsam_unstable/nonlinear/tests/testLCNLPSolver.cpp
View File

@ -348,7 +348,8 @@ TEST(testlcnlpSolver, posesInA2DBox) {
// Instantiate LCNLPSolver
LCNLPSolver lcnlpSolver(lcnlp);
Values actualSolution = lcnlpSolver.optimize(initialValues).first;
bool useWarmStart = true;
Values actualSolution = lcnlpSolver.optimize(initialValues, useWarmStart).first;
// cout << "Rotation angles: " << endl;
// for (size_t i = 1; i<=3; i++) {
@ -367,6 +368,7 @@ TEST(testlcnlpSolver, posesInA2DBox) {
//******************************************************************************
int main() {
cout<<"here: "<<endl;
TestResult tr;
return TestRegistry::runAllTests(tr);
}