diff --git a/gtsam_unstable/linear/InequalityFactorGraph.h b/gtsam_unstable/linear/InequalityFactorGraph.h index 30c0b1e0f..178a0a4d5 100644 --- a/gtsam_unstable/linear/InequalityFactorGraph.h +++ b/gtsam_unstable/linear/InequalityFactorGraph.h @@ -18,7 +18,7 @@ #pragma once -#include +#include #include namespace gtsam { diff --git a/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp b/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp index 2b5d25c57..c2500f23b 100644 --- a/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp +++ b/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp @@ -17,8 +17,10 @@ * @date Dec 15, 2014 */ -#include +#include #include +#include +#include #include namespace gtsam { @@ -38,12 +40,12 @@ bool LinearConstraintSQP::isDualFeasible(const VectorValues& duals) const { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities) { ConstrainedFactor::shared_ptr inequality = boost::dynamic_pointer_cast(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 ? + if (dual > 0.0) // See the explanation in QPSolver::identifyLeavingConstraint, we want dual < 0 ? return false; - } } return true; } @@ -90,9 +92,9 @@ LinearConstraintNLPState LinearConstraintSQP::iterate( VectorValues delta, duals; QPSolver qpSolver(qp); VectorValues zeroInitialValues; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, state.values) { + 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, params_.warmStart); @@ -106,10 +108,8 @@ LinearConstraintNLPState LinearConstraintSQP::iterate( newState.converged = checkConvergence(newState, delta); newState.iterations = state.iterations + 1; - if(params_.verbosity >= NonlinearOptimizerParams::VALUES) { - newState.values.print("Values"); - newState.duals.print("Duals"); - } + if(params_.verbosity >= NonlinearOptimizerParams::VALUES) + newState.print("Values"); return newState; } diff --git a/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp b/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp new file mode 100644 index 000000000..d012caaf9 --- /dev/null +++ b/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp @@ -0,0 +1,52 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LinearEqualityFactorGraph.cpp + * @author Duy-Nguyen Ta + * @author Krunal Chande + * @author Luca Carlone + * @date Dec 15, 2014 + */ + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +EqualityFactorGraph::shared_ptr LinearEqualityFactorGraph::linearize( + const Values& linearizationPoint) const { + EqualityFactorGraph::shared_ptr linearGraph( + new EqualityFactorGraph()); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){ + JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast( + factor->linearize(linearizationPoint)); + ConstrainedFactor::shared_ptr constraint = boost::dynamic_pointer_cast(factor); + linearGraph->add(LinearEquality(*jacobian, constraint->dualKey())); + } + return linearGraph; +} + +/* ************************************************************************* */ +bool LinearEqualityFactorGraph::checkFeasibility(const Values& values, double tol) const { + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){ + NoiseModelFactor::shared_ptr noiseModelFactor + = boost::dynamic_pointer_cast(factor); + Vector error = noiseModelFactor->unwhitenedError(values); + + if (error.lpNorm() > tol) + return false; + } + return true; +} + +} diff --git a/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h b/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h index 098aaae2c..dd481fdba 100644 --- a/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h +++ b/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h @@ -18,8 +18,10 @@ */ #pragma once +#include +#include +#include #include -#include namespace gtsam { @@ -33,33 +35,10 @@ public: } /// Linearize to a EqualityFactorGraph - EqualityFactorGraph::shared_ptr linearize( - const Values& linearizationPoint) const { - EqualityFactorGraph::shared_ptr linearGraph( - new EqualityFactorGraph()); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){ - JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast( - factor->linearize(linearizationPoint)); - ConstrainedFactor::shared_ptr constraint = boost::dynamic_pointer_cast(factor); - linearGraph->add(LinearEquality(*jacobian, constraint->dualKey())); - } - return linearGraph; - } + EqualityFactorGraph::shared_ptr linearize(const Values& linearizationPoint) const; - /** - * Return true if the max absolute error all factors is less than a tolerance - */ - bool checkFeasibility(const Values& values, double tol) const { - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){ - NoiseModelFactor::shared_ptr noiseModelFactor = boost::dynamic_pointer_cast( - factor); - Vector error = noiseModelFactor->unwhitenedError(values); - if (error.lpNorm() > tol) { - return false; - } - } - return true; - } + /// Return true if the max absolute error all factors is less than a tolerance + bool checkFeasibility(const Values& values, double tol) const; }; diff --git a/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp b/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp new file mode 100644 index 000000000..2e1b617c9 --- /dev/null +++ b/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LinearInequalityFactorGraph.cpp + * @author Duy-Nguyen Ta + * @author Krunal Chande + * @author Luca Carlone + * @date Dec 15, 2014 + */ + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +InequalityFactorGraph::shared_ptr LinearInequalityFactorGraph::linearize( + const Values& linearizationPoint) const { + InequalityFactorGraph::shared_ptr linearGraph(new InequalityFactorGraph()); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) { + JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast( + factor->linearize(linearizationPoint)); + ConstrainedFactor::shared_ptr constraint + = boost::dynamic_pointer_cast(factor); + linearGraph->add(LinearInequality(*jacobian, constraint->dualKey())); + } + return linearGraph; +} + +/* ************************************************************************* */ +bool LinearInequalityFactorGraph::checkFeasibilityAndComplimentary( + const Values& values, const VectorValues& dualValues, double tol) const { + + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) { + NoiseModelFactor::shared_ptr noiseModelFactor + = boost::dynamic_pointer_cast(factor); + Vector error = noiseModelFactor->unwhitenedError(values); + + // Primal feasibility condition: all constraints need to be <= 0.0 + if (error[0] > tol) + return false; + + // Complimentary condition: errors of active constraints need to be 0.0 + ConstrainedFactor::shared_ptr constraint + = boost::dynamic_pointer_cast(factor); + Key dualKey = constraint->dualKey(); + + // if dualKey doesn't exist in dualValues, it must be an inactive constraint! + if (!dualValues.exists(dualKey)) + continue; + + // for active constraint, the error should be 0.0 + if (fabs(error[0]) > tol) + return false; + } + + return true; +} + +} diff --git a/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h b/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h index 5195ab86e..5dce1ddc5 100644 --- a/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h +++ b/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h @@ -19,8 +19,9 @@ #pragma once #include +#include +#include #include -#include namespace gtsam { @@ -31,52 +32,15 @@ class LinearInequalityFactorGraph: public FactorGraph { public: /// Default constructor - LinearInequalityFactorGraph() { - } + LinearInequalityFactorGraph() {} /// Linearize to a InequalityFactorGraph - InequalityFactorGraph::shared_ptr linearize(const Values& linearizationPoint) const { - InequalityFactorGraph::shared_ptr linearGraph(new InequalityFactorGraph()); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) { - JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast( - factor->linearize(linearizationPoint)); - ConstrainedFactor::shared_ptr constraint - = boost::dynamic_pointer_cast(factor); - linearGraph->add(LinearInequality(*jacobian, constraint->dualKey())); - } - return linearGraph; - } + InequalityFactorGraph::shared_ptr linearize(const Values& linearizationPoint) const; - /** - * Return true if the all errors are <= 0.0 - */ + /// Return true if the all errors are <= 0.0 bool checkFeasibilityAndComplimentary(const Values& values, - const VectorValues& dualValues, double tol) const { + const VectorValues& dualValues, double tol) const; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) { - NoiseModelFactor::shared_ptr noiseModelFactor - = boost::dynamic_pointer_cast(factor); - Vector error = noiseModelFactor->unwhitenedError(values); - - // Primal feasibility condition: all constraints need to be <= 0.0 - if (error[0] > tol) - return false; - - // Complimentary condition: errors of active constraints need to be 0.0 - ConstrainedFactor::shared_ptr constraint - = boost::dynamic_pointer_cast(factor); - Key dualKey = constraint->dualKey(); - - // if dualKey doesn't exist in dualValues, it must be an inactive constraint! - if (!dualValues.exists(dualKey)) - continue; - - // for active constraint, the error should be 0.0 - if (fabs(error[0]) > tol) - return false; - } - - return true; - } }; -} + +} // namespace gtsam diff --git a/gtsam_unstable/nonlinear/tests/CMakeLists.txt b/gtsam_unstable/nonlinear/tests/CMakeLists.txt index ad14fcd9b..04b560bab 100644 --- a/gtsam_unstable/nonlinear/tests/CMakeLists.txt +++ b/gtsam_unstable/nonlinear/tests/CMakeLists.txt @@ -1 +1 @@ -gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "testCustomChartExpression.cpp" "gtsam_unstable") +gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "testCustomChartExpression.cpp;testLCNLPSolver2.cpp" "gtsam_unstable") diff --git a/gtsam_unstable/nonlinear/tests/testNonlinearInequalityFactorGraph.cpp b/gtsam_unstable/nonlinear/tests/testLinearInequalityFactorGraph.cpp similarity index 78% rename from gtsam_unstable/nonlinear/tests/testNonlinearInequalityFactorGraph.cpp rename to gtsam_unstable/nonlinear/tests/testLinearInequalityFactorGraph.cpp index 503528c85..233ee213b 100644 --- a/gtsam_unstable/nonlinear/tests/testNonlinearInequalityFactorGraph.cpp +++ b/gtsam_unstable/nonlinear/tests/testLinearInequalityFactorGraph.cpp @@ -10,8 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testQPSimple.cpp - * @brief Unit tests for testQPSimple + * @file testLinearInequalityFactorGraph.cpp * @author Duy-Nguyen Ta * @author Krunal Chande * @author Luca Carlone @@ -19,7 +18,7 @@ */ #include -#include +#include #include #include @@ -29,9 +28,9 @@ using namespace gtsam; const double tol = 1e-10; //****************************************************************************** -TEST(NonlinearInequalityFactorGraph, constructor) { - NonlinearInequalityFactorGraph nonlinearInequalities; - CHECK(nonlinearInequalities.empty()); +TEST(LinearInequalityFactorGraph, constructor) { + LinearInequalityFactorGraph linearInequalities; + CHECK(linearInequalities.empty()); } diff --git a/gtsam_unstable/nonlinear/tests/testLinearlyConstrainedNonlinearOptimizer.cpp b/gtsam_unstable/nonlinear/tests/testLinearlyConstrainedNonlinearOptimizer.cpp deleted file mode 100644 index dd716868e..000000000 --- a/gtsam_unstable/nonlinear/tests/testLinearlyConstrainedNonlinearOptimizer.cpp +++ /dev/null @@ -1,138 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testLinearlyConstrainedNonlinearOptimizer.cpp - * @brief Unit tests for LinearlyConstrainedNonlinearOptimizer - * @author Krunal Chande - * @author Duy-Nguyen Ta - * @author Luca Carlone - * @date Dec 15, 2014 - */ - -#include -#include -#include -#include -#include -#include - - -//namespace gtsam { -//struct LinearlyConstrainedNLP { -// NonlinearFactorGraph cost; -// LinearEqualityFactorGraph equalities; -// LinearInequalityFactorGraph inequalities; -//}; -// -//struct LinearlyConstrainedNLPState { -// Values values; -// VectorValues duals; -// bool converged; -// LinearlyConstrainedNLPState(const Values& initialValues) : -// values(initialValues), duals(VectorValues()), converged(false) { -// } -//}; -//class LinearlyConstrainedNonLinearOptimizer { -// LinearlyConstrainedNLP lcNLP_; -//public: -// LinearlyConstrainedNonLinearOptimizer(const LinearlyConstrainedNLP& lcNLP): lcNLP_(lcNLP) {} -// -// LinearlyConstrainedNLPState iterate(const LinearlyConstrainedNLPState& state) const { -// QP qp; -// qp.cost = lcNLP_.cost.linearize(state.values); -// qp.equalities = lcNLP_.equalities; -// qp.inequalities = lcNLP_.inequalities; -// QPSolver qpSolver(qp); -// VectorValues delta, duals; -// boost::tie(delta, duals) = qpSolver.optimize(); -// LinearlyConstrainedNLPState newState; -// newState.values = state.values.retract(delta); -// newState.duals = duals; -// newState.converged = checkConvergence(newState.values, newState.duals); -// return newState; -// } -// -// /** -// * Main optimization function. -// */ -// std::pair optimize(const Values& initialValues) const { -// LinearlyConstrainedNLPState state(initialValues); -// while(!state.converged){ -// state = iterate(state); -// } -// -// return std::make_pair(initialValues, VectorValues()); -// } -//}; -//} -// -//using namespace std; -//using namespace gtsam::symbol_shorthand; -//using namespace gtsam; -//const double tol = 1e-10; -////****************************************************************************** -//TEST(LinearlyConstrainedNonlinearOptimizer, Problem1 ) { -// -// // build a quadratic Objective function x1^2 - x1*x2 + x2^2 - 3*x1 + 5 -// // Note the Hessian encodes: -// // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f -// // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 -// HessianFactor lf(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1), -// 2.0 * ones(1, 1), zero(1), 10.0); -// -// // build linear inequalities -// LinearInequalityFactorGraph inequalities; -// inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 -// inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0 -// inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0 -// inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/2 -// -// // Instantiate LinearlyConstrainedNLP, pretending that the cost is not quadratic -// // (LinearContainerFactor makes a linear factor behave like a nonlinear one) -// LinearlyConstrainedNLP lcNLP; -// lcNLP.cost.add(LinearContainerFactor(lf)); -// lcNLP.inequalities = inequalities; -// -// // Compare against a QP -// QP qp; -// qp.cost.add(lf); -// qp.inequalities = inequalities; -// -// // instantiate QPsolver -// QPSolver qpSolver(qp); -// // create initial values for optimization -// VectorValues initialVectorValues; -// initialVectorValues.insert(X(1), zero(1)); -// initialVectorValues.insert(X(2), zero(1)); -// VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first; -// -// // instantiate LinearlyConstrainedNonLinearOptimizer -// LinearlyConstrainedNonLinearOptimizer lcNLPSolver(lcNLP); -// // create initial values for optimization -// Values initialValues; -// initialValues.insert(X(1), 0.0); -// initialValues.insert(X(2), 0.0); -// Values actualSolution = lcNLPSolver.optimize(initialValues).first; -// -// -// DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualSolution.at(X(1)), tol); -// DOUBLES_EQUAL(expectedSolution.at(X(2))[0], actualSolution.at(X(2)), tol); -//} -// -//****************************************************************************** -int main() { - std::cout<<"here"<