diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h index cb7545028..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: @@ -92,4 +89,20 @@ protected: const InequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p, const double& startAlpha) const; }; + +/** + * Find the max key in a problem. + * Useful to determine unique keys for additional slack variables + */ +template +Key maxKey(const PROBLEM& problem) { + auto keys = problem.cost.keys(); + Key maxKey = *std::max_element(keys.begin(), keys.end()); + if (!problem.equalities.empty()) + maxKey = std::max(maxKey, *problem.equalities.keys().rbegin()); + if (!problem.inequalities.empty()) + maxKey = std::max(maxKey, *problem.inequalities.keys().rbegin()); + return maxKey; +} + } // namespace gtsam diff --git a/gtsam_unstable/linear/LP.h b/gtsam_unstable/linear/LP.h index 4e5866695..632b41e96 100644 --- a/gtsam_unstable/linear/LP.h +++ b/gtsam_unstable/linear/LP.h @@ -16,11 +16,36 @@ namespace gtsam { using namespace std; +/// Mapping between variable's key and its corresponding dimensionality +using KeyDimMap = std::map; +/* + * Iterates through every factor in a linear graph and generates a + * mapping between every factor key and it's corresponding dimensionality. + */ +template +KeyDimMap collectKeyDim(const LinearGraph& linearGraph) { + KeyDimMap keyDimMap; + for (const typename LinearGraph::sharedFactor& factor : linearGraph) { + if (!factor) continue; + for (Key key : factor->keys()) + keyDimMap[key] = factor->getDim(factor->find(key)); + } + return keyDimMap; +} + +/** + * Data structure of a Linear Program + */ struct LP { + using shared_ptr = boost::shared_ptr; + LinearCost cost; //!< Linear cost factor EqualityFactorGraph equalities; //!< Linear equality constraints: cE(x) = 0 InequalityFactorGraph inequalities; //!< Linear inequality constraints: cI(x) <= 0 +private: + mutable KeyDimMap cachedConstrainedKeyDimMap_; //!< cached key-dim map of all variables in the constraints +public: /// check feasibility bool isFeasible(const VectorValues& x) const { return (equalities.error(x) == 0 && inequalities.error(x) == 0); @@ -40,10 +65,26 @@ struct LP { && inequalities.equals(other.inequalities); } - typedef boost::shared_ptr shared_ptr; + const KeyDimMap& constrainedKeyDimMap() const { + if (!cachedConstrainedKeyDimMap_.empty()) + return cachedConstrainedKeyDimMap_; + // Collect key-dim map of all variables in the constraints + cachedConstrainedKeyDimMap_ = collectKeyDim(equalities); + KeyDimMap keysDim2 = collectKeyDim(inequalities); + 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 template<> struct traits : public Testable { }; + } diff --git a/gtsam_unstable/linear/LPInitSolver.h b/gtsam_unstable/linear/LPInitSolver.h index 3b7f6026e..56e5a2c74 100644 --- a/gtsam_unstable/linear/LPInitSolver.h +++ b/gtsam_unstable/linear/LPInitSolver.h @@ -40,12 +40,10 @@ namespace gtsam { */ class LPInitSolver { private: - const LPSolver& lpSolver_; const LP& lp_; public: - LPInitSolver(const LPSolver& lpSolver) : - lpSolver_(lpSolver), lp_(lpSolver.lp()) { + LPInitSolver(const LP& lp) : lp_(lp) { } virtual ~LPInitSolver() { @@ -56,7 +54,7 @@ public: GaussianFactorGraph::shared_ptr initOfInitGraph = buildInitOfInitGraph(); VectorValues x0 = initOfInitGraph->optimize(); double y0 = compute_y0(x0); - Key yKey = maxKey(lpSolver_.keysDim()) + 1; // the unique key for y0 + Key yKey = maxKey(lp_) + 1; // the unique key for y0 VectorValues xy0(x0); xy0.insert(yKey, Vector::Constant(1, y0)); @@ -85,15 +83,6 @@ private: return initLP; } - /// Find the max key in the problem to determine unique keys for additional slack variables - Key maxKey(const KeyDimMap& keysDim) const { - Key maxK = 0; - for (Key key : keysDim | boost::adaptors::map_keys) - if (maxK < key) - maxK = key; - return maxK; - } - /** * Build the following graph to solve for an initial value of the initial problem * min ||x||^2 s.t. Ax = b @@ -104,9 +93,9 @@ private: new GaussianFactorGraph(lp_.equalities)); // create factor ||x||^2 and add to the graph - const KeyDimMap& keysDim = lpSolver_.keysDim(); - for (Key key : keysDim | boost::adaptors::map_keys) { - size_t dim = keysDim.at(key); + const KeyDimMap& constrainedKeyDim = lp_.constrainedKeyDimMap(); + for (Key key : constrainedKeyDim | boost::adaptors::map_keys) { + size_t dim = constrainedKeyDim.at(key); initGraph->push_back( JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim))); } diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index b9f5492af..042b62ea1 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -12,19 +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); - - // Collect key-dim map of all variables in the constraints to create their - // zero priors later - keysDim_ = collectKeysDim(lp_.equalities); - KeyDimMap keysDim2 = collectKeysDim(lp_.inequalities); - keysDim_.insert(keysDim2.begin(), keysDim2.end()); - + lp_(lp) { // Variable index equalityVariableIndex_ = VariableIndex(lp_.equalities); inequalityVariableIndex_ = VariableIndex(lp_.inequalities); @@ -32,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 @@ -45,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(); @@ -70,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; @@ -89,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()); @@ -101,30 +93,28 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors( KeySet allKeys = lp_.inequalities.keys(); allKeys.merge(lp_.equalities.keys()); allKeys.merge(KeySet(lp_.cost.keys())); - // add the corresponding factors for all variables that are not explicitly in the - // cost function for vars that are not in the cost, the cost gradient is zero (g=0), so b=xk + // Add corresponding factors for all variables that are not explicitly in the + // cost function. Gradients of the cost function wrt to these variables are + // zero (g=0), so b=xk if (cost.keys().size() != allKeys.size()) { KeySet difference; std::set_difference(allKeys.begin(), allKeys.end(), lp_.cost.begin(), lp_.cost.end(), std::inserter(difference, difference.end())); for (Key k : difference) { - size_t dim = keysDim_.at(k); + size_t dim = lp_.constrainedKeyDimMap().at(k); graph->push_back(JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k))); } } 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); @@ -132,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 @@ -179,6 +168,7 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints( return workingSet; } +//****************************************************************************** std::pair LPSolver::optimize( const VectorValues &initialValues, const VectorValues &duals) const { { @@ -195,6 +185,7 @@ std::pair LPSolver::optimize( } } +//****************************************************************************** boost::tuples::tuple LPSolver::computeStepSize( const InequalityFactorGraph &workingSet, const VectorValues &xk, const VectorValues &p) const { @@ -202,8 +193,9 @@ boost::tuples::tuple LPSolver::computeStepSize( std::numeric_limits::infinity()); } +//****************************************************************************** pair LPSolver::optimize() const { - LPInitSolver initSolver(*this); + LPInitSolver initSolver(lp_); VectorValues initValues = initSolver.solve(); return optimize(initValues); } diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 3223d7a59..bf081c05e 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -18,12 +18,9 @@ namespace gtsam { -typedef std::map KeyDimMap; class LPSolver: public ActiveSetSolver { const LP &lp_; //!< the linear programming problem - KeyDimMap keysDim_; //!< key-dim map of all variables in the constraints, used to create zero priors - std::vector addedZeroPriorsIndex_; public: /// Constructor LPSolver(const LP &lp); @@ -32,30 +29,6 @@ public: return lp_; } - const KeyDimMap &keysDim() const { - return keysDim_; - } - - /* - * Iterates through every factor in a linear graph and generates a - * mapping between every factor key and it's corresponding dimensionality. - */ - template - KeyDimMap collectKeysDim(const LinearGraph &linearGraph) const { - KeyDimMap keysDim; - for (const typename LinearGraph::sharedFactor &factor : linearGraph) { - if (!factor) - continue; - for (Key key : factor->keys()) - keysDim[key] = factor->getDim(factor->find(key)); - } - return keysDim; - } - - /// Create a zero prior for any keys in the graph that don't exist in the cost - GaussianFactorGraph::shared_ptr createZeroPriors(const KeyVector &costKeys, - const KeyDimMap &keysDim) const; - /* * This function performs an iteration of the Active Set Method for solving * LP problems. At the end of this iteration the problem should either be found 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 eb2f40e05..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(); } @@ -139,25 +132,17 @@ InequalityFactorGraph QPSolver::identifyActiveConstraints( InequalityFactorGraph workingSet; for (const LinearInequality::shared_ptr& factor : inequalities) { LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); - if (useWarmStart == true && duals.exists(workingFactor->dualKey())) { - workingFactor->activate(); + if (useWarmStart && duals.size() > 0) { + if (duals.exists(workingFactor->dualKey())) workingFactor->activate(); + else workingFactor->inactivate(); } else { - if (useWarmStart == true && duals.size() > 0) { + double error = workingFactor->error(initialValues); + // Safety guard. This should not happen unless users provide a bad init + if (error > 0) throw InfeasibleInitialValues(); + if (fabs(error) < 1e-7) + workingFactor->activate(); + else workingFactor->inactivate(); - } else { - double error = workingFactor->error(initialValues); - // TODO: find a feasible initial point for QPSolver. - // For now, we just throw an exception, since we don't have an LPSolver - // to do this yet - if (error > 0) - throw InfeasibleInitialValues(); - - if (fabs(error) < 1e-7) { - workingFactor->activate(); - } else { - workingFactor->inactivate(); - } - } } workingSet.push_back(workingFactor); } @@ -180,25 +165,19 @@ pair QPSolver::optimize( return make_pair(state.values, state.duals); } +//****************************************************************************** pair QPSolver::optimize() const { //Make an LP with any linear cost function. It doesn't matter for initialization. LP initProblem; - // make an unrelated key for a random variable cost: max key + 1 - Key newKey = *qp_.cost.keys().rbegin(); - if (!qp_.equalities.empty()) - newKey = std::max(newKey, *qp_.equalities.keys().rbegin()); - if (!qp_.inequalities.empty()) - newKey = std::max(newKey, *qp_.inequalities.keys().rbegin()); - ++newKey; + // make an unrelated key for a random variable cost + Key newKey = maxKey(qp_) + 1; initProblem.cost = LinearCost(newKey, Vector::Ones(1)); initProblem.equalities = qp_.equalities; initProblem.inequalities = qp_.inequalities; - LPSolver solver(initProblem); - LPInitSolver initSolver(solver); + LPInitSolver initSolver(initProblem); VectorValues initValues = initSolver.solve(); return optimize(initValues); } -; } /* namespace gtsam */ diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index 74e775225..be1759428 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -106,8 +106,7 @@ TEST(LPInitSolver, infinite_loop_multi_var) { TEST(LPInitSolver, initialization) { LP lp = simpleLP1(); - LPSolver lpSolver(lp); - LPInitSolver initSolver(lpSolver); + LPInitSolver initSolver(lp); GaussianFactorGraph::shared_ptr initOfInitGraph = initSolver.buildInitOfInitGraph(); VectorValues x0 = initOfInitGraph->optimize();