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

add comments, fix format, make test compiled

release/4.3a0
thduynguyen 2015-02-19 08:51:54 -05:00
parent d8e184fadd
commit ba4698bf51
5 changed files with 56 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
gtsam_unstable/nonlinear/LCNLPSolver.cpp
View File

@ -23,7 +23,6 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
bool LCNLPSolver::isStationary(const VectorValues& delta) const { bool LCNLPSolver::isStationary(const VectorValues& delta) const {
return delta.vector().lpNorm<Eigen::Infinity>() < errorTol; return delta.vector().lpNorm<Eigen::Infinity>() < errorTol;
@ -36,12 +35,13 @@ bool LCNLPSolver::isPrimalFeasible(const LCNLPState& state) const {
/* ************************************************************************* */ /* ************************************************************************* */
bool LCNLPSolver::isDualFeasible(const VectorValues& duals) const { bool LCNLPSolver::isDualFeasible(const VectorValues& duals) const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities){
NonlinearConstraint::shared_ptr inequality = boost::dynamic_pointer_cast<NonlinearConstraint>(factor); NonlinearConstraint::shared_ptr inequality
= boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
Key dualKey = inequality->dualKey(); Key dualKey = inequality->dualKey();
if (!duals.exists(dualKey)) continue; // should be inactive constraint! if (!duals.exists(dualKey)) continue; // should be inactive constraint!
double dual = duals.at(dualKey)[0]; // because we only support single-valued inequalities double dual = duals.at(dualKey)[0];// because we only support single-valued inequalities
if (dual > 0.0) { // See the explanation in QPSolver::identifyLeavingConstraint, we want dual < 0 ? if (dual > 0.0) { // See the explanation in QPSolver::identifyLeavingConstraint, we want dual < 0 ?
return false; return false;
} }
} }
@ -50,19 +50,23 @@ bool LCNLPSolver::isDualFeasible(const VectorValues& duals) const {
/* ************************************************************************* */ /* ************************************************************************* */
bool LCNLPSolver::isComplementary(const LCNLPState& state) const { bool LCNLPSolver::isComplementary(const LCNLPState& state) const {
return lcnlp_.linearInequalities.checkFeasibilityAndComplimentary(state.values, state.duals, errorTol); return lcnlp_.linearInequalities.checkFeasibilityAndComplimentary(
state.values, state.duals, errorTol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool LCNLPSolver::checkConvergence(const LCNLPState& state, const VectorValues& delta) const { bool LCNLPSolver::checkConvergence(const LCNLPState& state,
return isStationary(delta) && isPrimalFeasible(state) && isDualFeasible(state.duals) && isComplementary(state); const VectorValues& delta) const {
return isStationary(delta) && isPrimalFeasible(state)
&& isDualFeasible(state.duals) && isComplementary(state);
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues LCNLPSolver::initializeDuals() const { VectorValues LCNLPSolver::initializeDuals() const {
VectorValues duals; VectorValues duals;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearEqualities) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearEqualities){
NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(factor); NonlinearConstraint::shared_ptr constraint
= boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
duals.insert(constraint->dualKey(), zero(factor->dim())); duals.insert(constraint->dualKey(), zero(factor->dim()));
} }
@ -70,21 +74,23 @@ VectorValues LCNLPSolver::initializeDuals() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<Values, VectorValues> LCNLPSolver::optimize(const Values& initialValues, bool useWarmStart, bool debug) const { std::pair<Values, VectorValues> LCNLPSolver::optimize(
const Values& initialValues, bool useWarmStart, bool debug) const {
LCNLPState state(initialValues); LCNLPState state(initialValues);
state.duals = initializeDuals(); state.duals = initializeDuals();
while (!state.converged && state.iterations < 100) { while (!state.converged && state.iterations < 100) {
if (debug) if (debug)
std::cout << "state: iteration " << state.iterations << std::endl; std::cout << "state: iteration " << state.iterations << std::endl;
state = iterate(state, useWarmStart, debug); state = iterate(state, useWarmStart, debug);
} }
if (debug) if (debug)
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);
} }
/* ************************************************************************* */ /* ************************************************************************* */
LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart, bool debug) const { LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart,
bool debug) const {
// construct the qp subproblem // construct the qp subproblem
QP qp; QP qp;
@ -98,15 +104,12 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart, bool
// solve the QP subproblem // solve the QP subproblem
VectorValues delta, duals; VectorValues delta, duals;
QPSolver qpSolver(qp); QPSolver qpSolver(qp);
if (useWarmStart == false || state.iterations == 0) VectorValues zeroInitialValues;
boost::tie(delta, duals) = qpSolver.optimize(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, state.values) {
else { zeroInitialValues.insert(key_value.key, zero(key_value.value.dim()));
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, useWarmStart);
} }
boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals,
useWarmStart);
if (debug) if (debug)
state.values.print("state.values: "); state.values.print("state.values: ");
@ -128,7 +131,6 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart, bool
return newState; return newState;
} }
}
} // namespace gtsam

1
gtsam_unstable/nonlinear/LCNLPSolver.h
View File

@ -107,6 +107,7 @@ public:
LCNLPState iterate(const LCNLPState& state, bool useWarmStart = true, bool debug = false) const; LCNLPState iterate(const LCNLPState& state, bool useWarmStart = true, bool debug = false) const;
VectorValues initializeDuals() const; VectorValues initializeDuals() const;
/** /**
* Main optimization function. new * Main optimization function. new
*/ */

22
gtsam_unstable/nonlinear/NonlinearConstraint.h
View File

@ -26,10 +26,19 @@
namespace gtsam { 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 { class NonlinearConstraint {
protected: protected:
Key dualKey_; Key dualKey_; //!< Unique key for the dual variable associated with this constraint
public: public:
typedef boost::shared_ptr<NonlinearConstraint> shared_ptr; typedef boost::shared_ptr<NonlinearConstraint> shared_ptr;
@ -37,14 +46,17 @@ public:
/// Construct with dual key /// Construct with dual key
NonlinearConstraint(Key dualKey) : dualKey_(dualKey) {} NonlinearConstraint(Key dualKey) : dualKey_(dualKey) {}
/// Return the dual key
Key dualKey() const { return dualKey_; }
/** /**
* compute the HessianFactor of the (-dual * constraintHessian) for the qp subproblem's objective function * 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, virtual GaussianFactor::shared_ptr multipliedHessian(const Values& x,
const VectorValues& duals) const = 0; const VectorValues& duals) const = 0;
/// return the dual key
Key dualKey() const { return dualKey_; }
}; };
/* ************************************************************************* */ /* ************************************************************************* */

8
gtsam_unstable/nonlinear/NonlinearInequality.h
View File

@ -9,7 +9,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file NonlinearConstraint.h * @file NonlinearInequality.h
* @brief * @brief
* @author Duy-Nguyen Ta * @author Duy-Nguyen Ta
* @date Sep 30, 2013 * @date Sep 30, 2013
@ -38,7 +38,7 @@ protected:
typedef NonlinearInequality1<VALUE> This; typedef NonlinearInequality1<VALUE> This;
private: private:
static const int X1Dim = traits::dimension<VALUE>::value; static const int X1Dim = traits<VALUE>::dimension;
public: public:
@ -100,8 +100,8 @@ protected:
typedef NonlinearInequality2<VALUE1, VALUE2> This; typedef NonlinearInequality2<VALUE1, VALUE2> This;
private: private:
static const int X1Dim = traits::dimension<VALUE1>::value; static const int X1Dim = traits<VALUE1>::dimension;
static const int X2Dim = traits::dimension<VALUE2>::value; static const int X2Dim = traits<VALUE2>::dimension;
public: public:

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

@ -436,15 +436,15 @@ TEST_DISABLED(testlcnlpSolver, iterativePoseinBox) {
// Instantiate LCNLPSolver // Instantiate LCNLPSolver
LCNLPSolver lcnlpSolver(lcnlp); LCNLPSolver lcnlpSolver(lcnlp);
if (firstTime) { // if (firstTime) {
solutionAndDuals = lcnlpSolver.optimize(isamValues, useWarmStart); solutionAndDuals = lcnlpSolver.optimize(isamValues, useWarmStart);
firstTime = false; // firstTime = false;
} // }
else { // else {
cout << " using this \n "; // cout << " using this \n ";
solutionAndDuals = lcnlpSolver.optimize(isamValues, solutionAndDuals.second, useWarmStart); // solutionAndDuals = lcnlpSolver.optimize(isamValues, solutionAndDuals.second, useWarmStart);
//
} // }
cout << " ************************** \n"; cout << " ************************** \n";
} }
actualSolution = solutionAndDuals.first; actualSolution = solutionAndDuals.first;