diff --git a/gtsam_unstable/linear/ActiveSetSolver.cpp b/gtsam_unstable/linear/ActiveSetSolver.cpp index 29d5f96a8..fab50ac7d 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.cpp +++ b/gtsam_unstable/linear/ActiveSetSolver.cpp @@ -89,7 +89,6 @@ GaussianFactorGraph::shared_ptr ActiveSetSolver::buildDualGraph( } return dualGraph; } - /* * Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1] * @@ -100,8 +99,8 @@ GaussianFactorGraph::shared_ptr ActiveSetSolver::buildDualGraph( */ boost::tuple ActiveSetSolver::computeStepSize( const InequalityFactorGraph& workingSet, const VectorValues& xk, - const VectorValues& p, const double& startAlpha) const { - double minAlpha = startAlpha; + const VectorValues& p) const { + double minAlpha = startAlpha_; int closestFactorIx = -1; for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h index 6594ac3b6..3e981fe50 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.h +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -23,7 +23,7 @@ protected: KeySet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs VariableIndex equalityVariableIndex_, inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs - + double startAlpha_; public: typedef std::vector > TermsContainer; //!< vector of key matrix pairs //Matrices are usually the A term for a factor. @@ -86,8 +86,8 @@ protected: * Protected constructor because this class doesn't have any meaning without * a concrete Programming problem to solve. */ - ActiveSetSolver() : - constrainedKeys_() { + ActiveSetSolver(double startAlpha) : + constrainedKeys_(), startAlpha_(startAlpha) { } /** @@ -97,7 +97,7 @@ protected: */ boost::tuple computeStepSize( const InequalityFactorGraph& workingSet, const VectorValues& xk, - const VectorValues& p, const double& startAlpha) const; + const VectorValues& p) const; }; /** diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index 30e6c3ce6..7e1a57370 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -14,7 +14,7 @@ namespace gtsam { //****************************************************************************** LPSolver::LPSolver(const LP &lp) : - lp_(lp) { + ActiveSetSolver(std::numeric_limits::infinity()), lp_(lp) { // Variable index equalityVariableIndex_ = VariableIndex(lp_.equalities); inequalityVariableIndex_ = VariableIndex(lp_.inequalities); @@ -162,14 +162,6 @@ std::pair LPSolver::optimize( } } -//****************************************************************************** -boost::tuples::tuple LPSolver::computeStepSize( - const InequalityFactorGraph &workingSet, const VectorValues &xk, - const VectorValues &p) const { - return ActiveSetSolver::computeStepSize(workingSet, xk, p, - std::numeric_limits::infinity()); -} - //****************************************************************************** pair LPSolver::optimize() const { LPInitSolver initSolver(lp_); diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index b6e463164..59c10f1a5 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -67,12 +67,6 @@ public: JacobianFactor::shared_ptr createDualFactor(Key key, const InequalityFactorGraph &workingSet, const VectorValues &delta) const; - /// TODO(comment) - //SAME - boost::tuple computeStepSize( - const InequalityFactorGraph &workingSet, const VectorValues &xk, - const VectorValues &p) const; - /** Optimize with the provided feasible initial values * TODO: throw exception if the initial values is not feasible wrt inequality constraints * TODO: comment duals diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index f80bdc14a..2cbd9c437 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -30,7 +30,7 @@ namespace gtsam { //****************************************************************************** QPSolver::QPSolver(const QP& qp) : - qp_(qp) { + ActiveSetSolver(1.0), qp_(qp) { equalityVariableIndex_ = VariableIndex(qp_.equalities); inequalityVariableIndex_ = VariableIndex(qp_.inequalities); constrainedKeys_ = qp_.equalities.keys(); @@ -71,14 +71,7 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor( return boost::make_shared(); } } - -//****************************************************************************** -boost::tuple QPSolver::computeStepSize( - const InequalityFactorGraph& workingSet, const VectorValues& xk, - const VectorValues& p) const { - return ActiveSetSolver::computeStepSize(workingSet, xk, p, 1); -} - + //****************************************************************************** QPState QPSolver::iterate(const QPState& state) const { // Algorithm 16.3 from Nocedal06book. diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 64ced2630..af3109300 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -68,10 +68,6 @@ public: * * We want the minimum of all those alphas among all inactive inequality. */ - //SAME - boost::tuple computeStepSize( - const InequalityFactorGraph& workingSet, const VectorValues& xk, - const VectorValues& p) const; /// Iterate 1 step, return a new state with a new workingSet and values QPState iterate(const QPState& state) const;