diff --git a/gtsam_unstable/linear/LinearEqualityFactorGraph.h b/gtsam_unstable/linear/EqualityFactorGraph.h similarity index 71% rename from gtsam_unstable/linear/LinearEqualityFactorGraph.h rename to gtsam_unstable/linear/EqualityFactorGraph.h index 619a707ed..22bce33d4 100644 --- a/gtsam_unstable/linear/LinearEqualityFactorGraph.h +++ b/gtsam_unstable/linear/EqualityFactorGraph.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * LinearEqualityFactorGraph.h + * EqualityFactorGraph.h * @brief: Factor graph of all LinearEquality factors * @date: Dec 8, 2014 * @author: Duy-Nguyen Ta @@ -23,14 +23,14 @@ namespace gtsam { -class LinearEqualityFactorGraph : public FactorGraph { +class EqualityFactorGraph : public FactorGraph { public: - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; }; /// traits -template<> struct traits : public Testable< - LinearEqualityFactorGraph> { +template<> struct traits : public Testable< + EqualityFactorGraph> { }; } // \ namespace gtsam diff --git a/gtsam_unstable/linear/LinearInequalityFactorGraph.h b/gtsam_unstable/linear/InequalityFactorGraph.h similarity index 74% rename from gtsam_unstable/linear/LinearInequalityFactorGraph.h rename to gtsam_unstable/linear/InequalityFactorGraph.h index beb7ee43f..aead5e268 100644 --- a/gtsam_unstable/linear/LinearInequalityFactorGraph.h +++ b/gtsam_unstable/linear/InequalityFactorGraph.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * LinearInequalityFactorGraph.h + * InequalityFactorGraph.h * @brief: Factor graph of all LinearInequality factors * @date: Dec 8, 2014 * @author: Duy-Nguyen Ta @@ -23,12 +23,12 @@ namespace gtsam { -class LinearInequalityFactorGraph: public FactorGraph { +class InequalityFactorGraph: public FactorGraph { private: typedef FactorGraph Base; public: - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /** print */ void print(const std::string& str, const KeyFormatter& keyFormatter = @@ -37,15 +37,15 @@ public: } /** equals */ - bool equals(const LinearInequalityFactorGraph& other, + bool equals(const InequalityFactorGraph& other, double tol = 1e-9) const { return Base::equals(other, tol); } }; /// traits -template<> struct traits : public Testable< - LinearInequalityFactorGraph> { +template<> struct traits : public Testable< + InequalityFactorGraph> { }; } // \ namespace gtsam diff --git a/gtsam_unstable/linear/QP.h b/gtsam_unstable/linear/QP.h index f1324e4d0..a34061f58 100644 --- a/gtsam_unstable/linear/QP.h +++ b/gtsam_unstable/linear/QP.h @@ -19,8 +19,8 @@ #pragma once #include -#include -#include +#include +#include namespace gtsam { @@ -29,8 +29,8 @@ namespace gtsam { */ struct QP { GaussianFactorGraph cost; //!< Quadratic cost factors - LinearEqualityFactorGraph equalities; //!< linear equality constraints: f(x) = 0 - LinearInequalityFactorGraph inequalities; //!< linear inequality constraints: g(x) <= 0 + EqualityFactorGraph equalities; //!< linear equality constraints: f(x) = 0 + InequalityFactorGraph inequalities; //!< linear inequality constraints: g(x) <= 0 /** default constructor */ QP() : @@ -39,8 +39,8 @@ struct QP { /** constructor */ QP(const GaussianFactorGraph& _cost, - const LinearEqualityFactorGraph& _linearEqualities, - const LinearInequalityFactorGraph& _linearInequalities) : + const EqualityFactorGraph& _linearEqualities, + const InequalityFactorGraph& _linearInequalities) : cost(_cost), equalities(_linearEqualities), inequalities( _linearInequalities) { } diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index b7ff3780a..d2041da7c 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -39,7 +39,7 @@ QPSolver::QPSolver(const QP& qp) : qp_(qp) { //****************************************************************************** VectorValues QPSolver::solveWithCurrentWorkingSet( - const LinearInequalityFactorGraph& workingSet) const { + const InequalityFactorGraph& workingSet) const { GaussianFactorGraph workingGraph = baseGraph_; BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) { if (factor->active()) @@ -50,7 +50,7 @@ VectorValues QPSolver::solveWithCurrentWorkingSet( //****************************************************************************** JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, - const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const { + const InequalityFactorGraph& workingSet, const VectorValues& delta) const { // Transpose the A matrix of constrained factors to have the jacobian of the dual key std::vector > Aterms = collectDualJacobians @@ -78,7 +78,7 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, //****************************************************************************** GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph( - const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const { + const InequalityFactorGraph& workingSet, const VectorValues& delta) const { GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); BOOST_FOREACH(Key key, constrainedKeys_) { // Each constrained key becomes a factor in the dual graph @@ -91,7 +91,7 @@ GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph( //****************************************************************************** int QPSolver::identifyLeavingConstraint( - const LinearInequalityFactorGraph& workingSet, + const InequalityFactorGraph& workingSet, const VectorValues& lambdas) const { int worstFactorIx = -1; // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either @@ -127,7 +127,7 @@ int QPSolver::identifyLeavingConstraint( * We want the minimum of all those alphas among all inactive inequality. */ boost::tuple QPSolver::computeStepSize( - const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, + const InequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p) const { static bool debug = false; @@ -198,7 +198,7 @@ QPState QPSolver::iterate(const QPState& state) const { } else { // Inactivate the leaving constraint - LinearInequalityFactorGraph newWorkingSet = state.workingSet; + InequalityFactorGraph newWorkingSet = state.workingSet; newWorkingSet.at(leavingFactor)->inactivate(); return QPState(newValues, duals, newWorkingSet, false, state.iterations+1); } @@ -216,7 +216,7 @@ QPState QPSolver::iterate(const QPState& state) const { << endl; // also add to the working set the one that complains the most - LinearInequalityFactorGraph newWorkingSet = state.workingSet; + InequalityFactorGraph newWorkingSet = state.workingSet; if (factorIx >= 0) newWorkingSet.at(factorIx)->activate(); @@ -228,10 +228,10 @@ QPState QPSolver::iterate(const QPState& state) const { } //****************************************************************************** -LinearInequalityFactorGraph QPSolver::identifyActiveConstraints( - const LinearInequalityFactorGraph& inequalities, +InequalityFactorGraph QPSolver::identifyActiveConstraints( + const InequalityFactorGraph& inequalities, const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const { - LinearInequalityFactorGraph workingSet; + InequalityFactorGraph workingSet; BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities) { LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); if (useWarmStart == true && duals.exists(workingFactor->dualKey())) { @@ -265,7 +265,7 @@ pair QPSolver::optimize( const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const { // Initialize workingSet from the feasible initialValues - LinearInequalityFactorGraph workingSet = + InequalityFactorGraph workingSet = identifyActiveConstraints(qp_.inequalities, initialValues, duals, useWarmStart); QPState state(initialValues, duals, workingSet, false, 0); diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index b354dcccb..c060b3883 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -30,7 +30,7 @@ namespace gtsam { struct QPState { VectorValues values; VectorValues duals; - LinearInequalityFactorGraph workingSet; + InequalityFactorGraph workingSet; bool converged; size_t iterations; @@ -41,7 +41,7 @@ struct QPState { /// constructor with initial values QPState(const VectorValues& initialValues, const VectorValues& initialDuals, - const LinearInequalityFactorGraph& initialWorkingSet, bool _converged, size_t _iterations) : + const InequalityFactorGraph& initialWorkingSet, bool _converged, size_t _iterations) : values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged( _converged), iterations(_iterations) { } @@ -68,7 +68,7 @@ public: /// Find solution with the current working set VectorValues solveWithCurrentWorkingSet( - const LinearInequalityFactorGraph& workingSet) const; + const InequalityFactorGraph& workingSet) const; /// @name Build the dual graph /// @{ @@ -92,7 +92,7 @@ public: /// Create a dual factor JacobianFactor::shared_ptr createDualFactor(Key key, - const LinearInequalityFactorGraph& workingSet, + const InequalityFactorGraph& workingSet, const VectorValues& delta) const; /** @@ -123,7 +123,7 @@ public: * Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi */ GaussianFactorGraph::shared_ptr buildDualGraph( - const LinearInequalityFactorGraph& workingSet, + const InequalityFactorGraph& workingSet, const VectorValues& delta) const; /// @} @@ -162,7 +162,7 @@ public: * And we want to remove the worst one with the largest lambda from the active set. * */ - int identifyLeavingConstraint(const LinearInequalityFactorGraph& workingSet, + int identifyLeavingConstraint(const InequalityFactorGraph& workingSet, const VectorValues& lambdas) const; /** @@ -174,7 +174,7 @@ public: * in the next iteration */ boost::tuple computeStepSize( - const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, + const InequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p) const; /** Iterate 1 step, return a new state with a new workingSet and values */ @@ -183,8 +183,8 @@ public: /** * Identify active constraints based on initial values. */ - LinearInequalityFactorGraph identifyActiveConstraints( - const LinearInequalityFactorGraph& inequalities, + InequalityFactorGraph identifyActiveConstraints( + const InequalityFactorGraph& inequalities, const VectorValues& initialValues, const VectorValues& duals = VectorValues(), bool useWarmStart = true) const; diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp index bf41743a2..fb8e743f6 100644 --- a/gtsam_unstable/linear/tests/testLinearEquality.cpp +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -12,7 +12,7 @@ /** * @file testLinearEquality.cpp * @brief Unit tests for LinearEquality - * @author thduynguyen + * @author Duy-Nguyen Ta **/ #include diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 37dcf0bc0..d8de4fd7a 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -132,7 +132,7 @@ TEST(QPSolver, indentifyActiveConstraints) { currentSolution.insert(X(1), zero(1)); currentSolution.insert(X(2), zero(1)); - LinearInequalityFactorGraph workingSet = + InequalityFactorGraph workingSet = solver.identifyActiveConstraints(qp.inequalities, currentSolution); CHECK(!workingSet.at(0)->active()); // inactive @@ -173,7 +173,7 @@ TEST(QPSolver, iterate) { expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished()); expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished()); - LinearInequalityFactorGraph workingSet = + InequalityFactorGraph workingSet = solver.identifyActiveConstraints(qp.inequalities, currentSolution); QPState state(currentSolution, VectorValues(), workingSet, false, 100);