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 {
/* ************************************************************************* */
bool LCNLPSolver::isStationary(const VectorValues& delta) const {
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 {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities) {
NonlinearConstraint::shared_ptr inequality = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities){
NonlinearConstraint::shared_ptr inequality
= boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
Key dualKey = inequality->dualKey();
if (!duals.exists(dualKey)) continue; // should be inactive constraint!
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 ?
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 ?
return false;
}
}
@ -50,19 +50,23 @@ bool LCNLPSolver::isDualFeasible(const VectorValues& duals) 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 {
return isStationary(delta) && isPrimalFeasible(state) && isDualFeasible(state.duals) && isComplementary(state);
bool LCNLPSolver::checkConvergence(const LCNLPState& state,
const VectorValues& delta) const {
return isStationary(delta) && isPrimalFeasible(state)
&& isDualFeasible(state.duals) && isComplementary(state);
}
/* ************************************************************************* */
VectorValues LCNLPSolver::initializeDuals() const {
VectorValues duals;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearEqualities) {
NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearEqualities){
NonlinearConstraint::shared_ptr constraint
= boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
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);
state.duals = initializeDuals();
while (!state.converged && state.iterations < 100) {
if (debug)
std::cout << "state: iteration " << state.iterations << std::endl;
std::cout << "state: iteration " << state.iterations << std::endl;
state = iterate(state, useWarmStart, 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);
}
/* ************************************************************************* */
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
QP qp;
@ -98,15 +104,12 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart, bool
// solve the QP subproblem
VectorValues delta, duals;
QPSolver qpSolver(qp);
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, useWarmStart);
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);
if (debug)
state.values.print("state.values: ");
@ -128,7 +131,6 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart, bool
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;
VectorValues initializeDuals() const;
/**
* Main optimization function. new
*/

22
gtsam_unstable/nonlinear/NonlinearConstraint.h
View File

@ -26,10 +26,19 @@
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 {
protected:
Key dualKey_;
Key dualKey_; //!< Unique key for the dual variable associated with this constraint
public:
typedef boost::shared_ptr<NonlinearConstraint> shared_ptr;
@ -37,14 +46,17 @@ public:
/// Construct with dual key
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,
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
* @author Duy-Nguyen Ta
* @date Sep 30, 2013
@ -38,7 +38,7 @@ protected:
typedef NonlinearInequality1<VALUE> This;
private:
static const int X1Dim = traits::dimension<VALUE>::value;
static const int X1Dim = traits<VALUE>::dimension;
public:
@ -100,8 +100,8 @@ protected:
typedef NonlinearInequality2<VALUE1, VALUE2> This;
private:
static const int X1Dim = traits::dimension<VALUE1>::value;
static const int X2Dim = traits::dimension<VALUE2>::value;
static const int X1Dim = traits<VALUE1>::dimension;
static const int X2Dim = traits<VALUE2>::dimension;
public:

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

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