From c99a8481482034327531d9869bd70d82fae734ec Mon Sep 17 00:00:00 2001 From: krunalchande Date: Mon, 22 Dec 2014 18:35:22 -0500 Subject: [PATCH] Refactoring. --- .../LinearEqualityManifoldFactorGraph.h | 50 +++++++++ .../nonlinear/NonlinearEqualityFactorGraph.h | 27 +++++ .../NonlinearInequalityFactorGraph.h | 58 ++++++++++ .../nonlinear/tests/testSQPSimple.cpp | 104 +----------------- 4 files changed, 138 insertions(+), 101 deletions(-) create mode 100644 gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h create mode 100644 gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h create mode 100644 gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h diff --git a/gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h b/gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h new file mode 100644 index 000000000..50f9c3245 --- /dev/null +++ b/gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h @@ -0,0 +1,50 @@ +/** + * @file LinearEqualityManifoldFactorGraph.h + * @author Duy-Nguyen Ta + * @author Krunal Chande + * @author Luca Carlone + * @date Dec 15, 2014 + */ + +#pragma once +#include + + +namespace gtsam { + +class LinearEqualityManifoldFactorGraph: public FactorGraph { +public: + /// default constructor + LinearEqualityManifoldFactorGraph() { + } + + /// linearize to a LinearEqualityFactorGraph + LinearEqualityFactorGraph::shared_ptr linearize( + const Values& linearizationPoint) const { + LinearEqualityFactorGraph::shared_ptr linearGraph( + new LinearEqualityFactorGraph()); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){ + JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast( + factor->linearize(linearizationPoint)); + NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast(factor); + linearGraph->add(LinearEquality(*jacobian, constraint->dualKey())); + } + return linearGraph; + } + + /** + * 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; + } +}; +} diff --git a/gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h b/gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h new file mode 100644 index 000000000..116b273fb --- /dev/null +++ b/gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h @@ -0,0 +1,27 @@ +/** + * @file NonlinearEqualityFactorGraph.h + * @author Krunal Chande + * @date Dec 22, 2014 + */ + +#pragma once +#include + + +namespace gtsam { +class NonlinearEqualityFactorGraph: public LinearEqualityManifoldFactorGraph { +public: + /// default constructor + NonlinearEqualityFactorGraph() { + } + + GaussianFactorGraph::shared_ptr multipliedHessians(const Values& values, const VectorValues& duals) const { + GaussianFactorGraph::shared_ptr constrainedHessians(new GaussianFactorGraph()); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) { + NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast(factor); + constrainedHessians->push_back(constraint->multipliedHessian(values, duals)); + } + return constrainedHessians; + } +}; +} diff --git a/gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h b/gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h new file mode 100644 index 000000000..a76caeeb6 --- /dev/null +++ b/gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h @@ -0,0 +1,58 @@ +/** + * @file NonlinearInequalityFactorGraph.h + * @author Krunal Chande + * @date Dec 22, 2014 + */ + +#pragma once +#include + +namespace gtsam { +class NonlinearInequalityFactorGraph : public FactorGraph { + +public: + /// default constructor + NonlinearInequalityFactorGraph() { + } + + /// linearize to a LinearEqualityFactorGraph + LinearInequalityFactorGraph::shared_ptr linearize( + const Values& linearizationPoint) const { + LinearInequalityFactorGraph::shared_ptr linearGraph( + new LinearInequalityFactorGraph()); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){ + JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast( + factor->linearize(linearizationPoint)); + NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast(factor); + linearGraph->add(LinearInequality(*jacobian, constraint->dualKey())); + } + return linearGraph; + } + + /** + * Return true if the all errors are <= 0.0 + */ + bool checkFeasibilityAndComplimentary(const Values& values, const VectorValues& duals, 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 + NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast( + factor); + Key dualKey = constraint->dualKey(); + if (!duals.exists(dualKey)) continue; // if dualKey doesn't exist, it is an inactive constraint! + if (fabs(error[0]) > tol) // for active constraint, the error should be 0.0 + return false; + + } + return true; + } +}; +} diff --git a/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp b/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp index b2be55289..e8a14203b 100644 --- a/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp +++ b/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp @@ -24,112 +24,14 @@ #include #include #include +#include +#include +#include #include #include namespace gtsam { -class LinearEqualityManifoldFactorGraph: public FactorGraph { -public: - /// default constructor - LinearEqualityManifoldFactorGraph() { - } - - /// linearize to a LinearEqualityFactorGraph - LinearEqualityFactorGraph::shared_ptr linearize( - const Values& linearizationPoint) const { - LinearEqualityFactorGraph::shared_ptr linearGraph( - new LinearEqualityFactorGraph()); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){ - JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast( - factor->linearize(linearizationPoint)); - NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast(factor); - linearGraph->add(LinearEquality(*jacobian, constraint->dualKey())); - } - return linearGraph; - } - - /** - * 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; - } -}; - -class NonlinearEqualityFactorGraph: public LinearEqualityManifoldFactorGraph { -public: - /// default constructor - NonlinearEqualityFactorGraph() { - } - - GaussianFactorGraph::shared_ptr multipliedHessians(const Values& values, const VectorValues& duals) const { - GaussianFactorGraph::shared_ptr constrainedHessians(new GaussianFactorGraph()); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) { - NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast(factor); - constrainedHessians->push_back(constraint->multipliedHessian(values, duals)); - } - return constrainedHessians; - } -}; - -class NonlinearInequalityFactorGraph : public FactorGraph { - -public: - /// default constructor - NonlinearInequalityFactorGraph() { - } - - /// linearize to a LinearEqualityFactorGraph - LinearInequalityFactorGraph::shared_ptr linearize( - const Values& linearizationPoint) const { - LinearInequalityFactorGraph::shared_ptr linearGraph( - new LinearInequalityFactorGraph()); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){ - JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast( - factor->linearize(linearizationPoint)); - NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast(factor); - linearGraph->add(LinearInequality(*jacobian, constraint->dualKey())); - } - return linearGraph; - } - - /** - * Return true if the all errors are <= 0.0 - */ - bool checkFeasibilityAndComplimentary(const Values& values, const VectorValues& duals, 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 - NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast( - factor); - Key dualKey = constraint->dualKey(); - if (!duals.exists(dualKey)) continue; // if dualKey doesn't exist, it is an inactive constraint! - if (fabs(error[0]) > tol) // for active constraint, the error should be 0.0 - return false; - - } - return true; - } - -}; - struct NLP { NonlinearFactorGraph cost; NonlinearEqualityFactorGraph linearEqualities;