diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h index e5f2d2d3a..4519ad5a8 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.h +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -21,10 +21,7 @@ namespace gtsam { class ActiveSetSolver { protected: KeySet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs - GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. - //!< used to initialize the working set factor graph, - //!< to which active inequalities will be added - VariableIndex costVariableIndex_, equalityVariableIndex_, + VariableIndex equalityVariableIndex_, inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs public: diff --git a/gtsam_unstable/linear/LP.h b/gtsam_unstable/linear/LP.h index cb4fdc19d..632b41e96 100644 --- a/gtsam_unstable/linear/LP.h +++ b/gtsam_unstable/linear/LP.h @@ -74,6 +74,13 @@ public: cachedConstrainedKeyDimMap_.insert(keysDim2.begin(), keysDim2.end()); return cachedConstrainedKeyDimMap_; } + + Vector costGradient(Key key, const VectorValues& delta) const { + Vector g = Vector::Zero(delta.at(key).size()); + Factor::const_iterator it = cost.find(key); + if (it != cost.end()) g = cost.getA(it).transpose(); + return g; + } }; /// traits diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index 4de97c348..042b62ea1 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -12,13 +12,9 @@ #include namespace gtsam { +//****************************************************************************** LPSolver::LPSolver(const LP &lp) : - lp_(lp), addedZeroPriorsIndex_() { - // Push back factors that are the same in every iteration to the base graph. - // Those include the equality constraints and zero priors for keys that are - // not in the cost - baseGraph_.push_back(lp_.equalities); - + lp_(lp) { // Variable index equalityVariableIndex_ = VariableIndex(lp_.equalities); inequalityVariableIndex_ = VariableIndex(lp_.inequalities); @@ -26,6 +22,7 @@ LPSolver::LPSolver(const LP &lp) : constrainedKeys_.merge(lp_.inequalities.keys()); } +//****************************************************************************** LPState LPSolver::iterate(const LPState &state) const { // Solve with the current working set // LP: project the objective neg. gradient to the constraint's null space @@ -39,7 +36,7 @@ LPState LPSolver::iterate(const LPState &state) const { // Compute lambda from the dual graph // LP: project the objective's gradient onto each constraint gradient to // obtain the dual scaling factors - // is it true?? + // is it true?? GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, newValues); VectorValues duals = dualGraph->optimize(); @@ -64,8 +61,8 @@ LPState LPSolver::iterate(const LPState &state) const { // If we CAN make some progress, i.e. p_k != 0 // Adapt stepsize if some inactive constraints complain about this move // LP: projection on nullspace is NOT zero: - // find and put a blocking inactive constraint to the working set, - // otherwise the problem is unbounded!!! + // find and put a blocking inactive constraint to the working set, + // otherwise the problem is unbounded!!! double alpha; int factorIx; VectorValues p = newValues - state.values; @@ -83,6 +80,7 @@ LPState LPSolver::iterate(const LPState &state) const { } } +//****************************************************************************** GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors( const LinearCost &cost, const VectorValues &xk) const { GaussianFactorGraph::shared_ptr graph(new GaussianFactorGraph()); @@ -110,16 +108,13 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors( return graph; } +//****************************************************************************** VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk, const InequalityFactorGraph &workingSet) const { - GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2 -// We remove the old zero priors from the base graph we are going to use to solve - //This iteration's problem -// for (size_t index : addedZeroPriorsIndex_) { -// workingGraph.remove(index); -// } - + GaussianFactorGraph workingGraph; + // || X - Xk + g ||^2 workingGraph.push_back(*createLeastSquareFactors(lp_.cost, xk)); + workingGraph.push_back(lp_.equalities); for (const LinearInequality::shared_ptr &factor : workingSet) { if (factor->active()) workingGraph.push_back(factor); @@ -127,37 +122,36 @@ VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk, return workingGraph.optimize(); } -boost::shared_ptr LPSolver::createDualFactor(Key key, - const InequalityFactorGraph &workingSet, const VectorValues &delta) const { +//****************************************************************************** +boost::shared_ptr LPSolver::createDualFactor( + Key key, const InequalityFactorGraph &workingSet, + const VectorValues &delta) const { // Transpose the A matrix of constrained factors to have the jacobian of the // dual key - TermsContainer Aterms = collectDualJacobians < LinearEquality - > (key, lp_.equalities, equalityVariableIndex_); - TermsContainer AtermsInequalities = collectDualJacobians < LinearInequality - > (key, workingSet, inequalityVariableIndex_); + TermsContainer Aterms = collectDualJacobians( + key, lp_.equalities, equalityVariableIndex_); + TermsContainer AtermsInequalities = collectDualJacobians( + key, workingSet, inequalityVariableIndex_); Aterms.insert(Aterms.end(), AtermsInequalities.begin(), - AtermsInequalities.end()); + AtermsInequalities.end()); // Collect the gradients of unconstrained cost factors to the b vector if (Aterms.size() > 0) { - Vector b = Vector::Zero(delta.at(key).size()); - Factor::const_iterator it = lp_.cost.find(key); - if (it != lp_.cost.end()) - b = lp_.cost.getA(it).transpose(); - return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables + Vector b = lp_.costGradient(key, delta); + // to compute the least-square approximation of dual variables + return boost::make_shared(Aterms, b); } else { return boost::make_shared(); } } +//****************************************************************************** InequalityFactorGraph LPSolver::identifyActiveConstraints( const InequalityFactorGraph &inequalities, const VectorValues &initialValues, const VectorValues &duals) const { InequalityFactorGraph workingSet; for (const LinearInequality::shared_ptr &factor : inequalities) { LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); - GTSAM_PRINT(*workingFactor); - GTSAM_PRINT(initialValues); double error = workingFactor->error(initialValues); // TODO: find a feasible initial point for LPSolver. // For now, we just throw an exception @@ -174,6 +168,7 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints( return workingSet; } +//****************************************************************************** std::pair LPSolver::optimize( const VectorValues &initialValues, const VectorValues &duals) const { { @@ -190,6 +185,7 @@ std::pair LPSolver::optimize( } } +//****************************************************************************** boost::tuples::tuple LPSolver::computeStepSize( const InequalityFactorGraph &workingSet, const VectorValues &xk, const VectorValues &p) const { @@ -197,6 +193,7 @@ boost::tuples::tuple LPSolver::computeStepSize( std::numeric_limits::infinity()); } +//****************************************************************************** pair LPSolver::optimize() const { LPInitSolver initSolver(lp_); VectorValues initValues = initSolver.solve(); diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 6ad8f8344..bf081c05e 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -21,7 +21,6 @@ namespace gtsam { class LPSolver: public ActiveSetSolver { const LP &lp_; //!< the linear programming problem - std::vector addedZeroPriorsIndex_; public: /// Constructor LPSolver(const LP &lp); diff --git a/gtsam_unstable/linear/QP.h b/gtsam_unstable/linear/QP.h index 457a859de..e610eb934 100644 --- a/gtsam_unstable/linear/QP.h +++ b/gtsam_unstable/linear/QP.h @@ -33,6 +33,10 @@ struct QP { EqualityFactorGraph equalities; //!< linear equality constraints: cE(x) = 0 InequalityFactorGraph inequalities; //!< linear inequality constraints: cI(x) <= 0 +private: + mutable VariableIndex cachedCostVariableIndex_; + +public: /** default constructor */ QP() : cost(), equalities(), inequalities() { @@ -53,6 +57,23 @@ struct QP { equalities.print("Linear equality factors: "); inequalities.print("Linear inequality factors: "); } + + const VariableIndex& costVariableIndex() const { + if (cachedCostVariableIndex_.size() == 0) + cachedCostVariableIndex_ = VariableIndex(cost); + return cachedCostVariableIndex_; + } + + Vector costGradient(Key key, const VectorValues& delta) const { + Vector g = Vector::Zero(delta.at(key).size()); + if (costVariableIndex().find(key) != costVariableIndex().end()) { + for (size_t factorIx : costVariableIndex()[key]) { + GaussianFactor::shared_ptr factor = cost.at(factorIx); + g += factor->gradient(key, delta); + } + } + return g; + } }; } // namespace gtsam diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index 7dfebfecb..a8ef028e8 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -31,9 +31,6 @@ namespace gtsam { //****************************************************************************** QPSolver::QPSolver(const QP& qp) : qp_(qp) { - baseGraph_ = qp_.cost; - baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end()); - costVariableIndex_ = VariableIndex(qp_.cost); equalityVariableIndex_ = VariableIndex(qp_.equalities); inequalityVariableIndex_ = VariableIndex(qp_.inequalities); constrainedKeys_ = qp_.equalities.keys(); @@ -43,7 +40,8 @@ QPSolver::QPSolver(const QP& qp) : //***************************************************cc*************************** VectorValues QPSolver::solveWithCurrentWorkingSet( const InequalityFactorGraph& workingSet) const { - GaussianFactorGraph workingGraph = baseGraph_; + GaussianFactorGraph workingGraph = qp_.cost; + workingGraph.push_back(qp_.equalities); for (const LinearInequality::shared_ptr& factor : workingSet) { if (factor->active()) workingGraph.push_back(factor); @@ -52,28 +50,23 @@ VectorValues QPSolver::solveWithCurrentWorkingSet( } //****************************************************************************** -JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, - const InequalityFactorGraph& workingSet, const VectorValues& delta) const { +JacobianFactor::shared_ptr QPSolver::createDualFactor( + Key key, const InequalityFactorGraph& workingSet, + const VectorValues& delta) const { // Transpose the A matrix of constrained factors to have the jacobian of the // dual key - std::vector < std::pair > Aterms = collectDualJacobians - < LinearEquality > (key, qp_.equalities, equalityVariableIndex_); - std::vector < std::pair > AtermsInequalities = - collectDualJacobians < LinearInequality - > (key, workingSet, inequalityVariableIndex_); + TermsContainer Aterms = collectDualJacobians( + key, qp_.equalities, equalityVariableIndex_); + TermsContainer AtermsInequalities = collectDualJacobians( + key, workingSet, inequalityVariableIndex_); Aterms.insert(Aterms.end(), AtermsInequalities.begin(), - AtermsInequalities.end()); + AtermsInequalities.end()); // Collect the gradients of unconstrained cost factors to the b vector if (Aterms.size() > 0) { - Vector b = Vector::Zero(delta.at(key).size()); - if (costVariableIndex_.find(key) != costVariableIndex_.end()) { - for (size_t factorIx : costVariableIndex_[key]) { - GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); - b += factor->gradient(key, delta); - } - } - return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables + Vector b = qp_.costGradient(key, delta); + // to compute the least-square approximation of dual variables + return boost::make_shared(Aterms, b); } else { return boost::make_shared(); }