From b07b431ac0a5ab3c2d0d1ca1d91e19d36dcb7da2 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Tue, 15 Apr 2014 13:55:24 -0400 Subject: [PATCH] first ineq QP test passed! --- gtsam/linear/tests/testQPSolver.cpp | 290 +++++++++++++++++++--------- 1 file changed, 203 insertions(+), 87 deletions(-) diff --git a/gtsam/linear/tests/testQPSolver.cpp b/gtsam/linear/tests/testQPSolver.cpp index c919daa01..5db463d19 100644 --- a/gtsam/linear/tests/testQPSolver.cpp +++ b/gtsam/linear/tests/testQPSolver.cpp @@ -27,22 +27,24 @@ using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; +#define TEST_DISABLED(testGroup, testName)\ + void testGroup##testName##Test(TestResult& result_, const std::string& name_) + class QPSolver { const GaussianFactorGraph& graph_; //!< the original graph, can't be modified! - GaussianFactorGraph workingGraph_; //!< working set - VectorValues currentSolution_; FastVector constraintIndices_; //!< Indices of constrained factors in the original graph - GaussianFactorGraph::shared_ptr freeHessians_; - VariableIndex freeHessianVarIndex_; + GaussianFactorGraph::shared_ptr freeHessians_; //!< unconstrained Hessians of constrained variables + VariableIndex freeHessianFactorIndex_; //!< indices of unconstrained Hessian factors of constrained variables + // gtsam calls it "VariableIndex", but I think FactorIndex + // makes more sense, because it really stores factor indices. VariableIndex fullFactorIndices_; //!< indices of factors involving each variable. // gtsam calls it "VariableIndex", but I think FactorIndex // makes more sense, because it really stores factor indices. public: /// Constructor - QPSolver(const GaussianFactorGraph& graph, const VectorValues& initials) : - graph_(graph), workingGraph_(graph.clone()), currentSolution_(initials), - fullFactorIndices_(graph) { + QPSolver(const GaussianFactorGraph& graph) : + graph_(graph), fullFactorIndices_(graph) { // Split the original graph into unconstrained and constrained part // and collect indices of constrained factors @@ -64,7 +66,7 @@ public: // Collect unconstrained hessians of constrained vars to build dual graph freeHessians_ = unconstrainedHessiansOfConstrainedVars(graph, constrainedVars); - freeHessianVarIndex_ = VariableIndex(*freeHessians_); + freeHessianFactorIndex_ = VariableIndex(*freeHessians_); } /// Return indices of all constrained factors @@ -169,14 +171,15 @@ public: * - The constant term b is \grad f(xi), which can be computed from all unconstrained * Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi */ - GaussianFactorGraph::shared_ptr buildDualGraph(const VectorValues& x0) const { + GaussianFactorGraph buildDualGraph(const GaussianFactorGraph& graph, + const VectorValues& x0) const { // The dual graph to return - GaussianFactorGraph::shared_ptr dualGraph = boost::make_shared(); + GaussianFactorGraph dualGraph; // For each variable xi involving in some constraint, compute the unconstrained gradient // wrt xi from the prebuilt freeHessian graph // \grad f(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi - BOOST_FOREACH(const VariableIndex::value_type& xiKey_factors, freeHessianVarIndex_) { + BOOST_FOREACH(const VariableIndex::value_type& xiKey_factors, freeHessianFactorIndex_) { Key xiKey = xiKey_factors.first; VariableIndex::Factors xiFactors = xiKey_factors.second; @@ -215,7 +218,7 @@ public: // gradc_k(xi) = \frac{\partial c_k}{\partial xi}' std::vector > lambdaTerms; // collection of lambda_k, and gradc_k BOOST_FOREACH(size_t factorIndex, fullFactorIndices_[xiKey]) { - JacobianFactor::shared_ptr factor = toJacobian(workingGraph_.at(factorIndex)); + JacobianFactor::shared_ptr factor = toJacobian(graph.at(factorIndex)); if (!factor || !factor->isConstrained()) continue; // Gradient is the transpose of the Jacobian: A_k = gradc_k(xi) = \frac{\partial c_k}{\partial xi}' // Each column for each lambda_k corresponds to [the transpose of] each constrained row factor @@ -223,7 +226,9 @@ public: // Deal with mixed sigmas: no information if sigma != 0 Vector sigmas = factor->get_model()->sigmas(); for (size_t sigmaIx = 0; sigmaIx 1e-9) { // if it's either ineq (sigma<0) or unconstrained (sigma>0) + // if it's either ineq (sigma<0) or unconstrained (sigma>0) + // we have no information about it + if (fabs(sigmas[sigmaIx]) > 1e-9) { A_k.col(sigmaIx) = zero(A_k.rows()); } } @@ -232,14 +237,24 @@ public: } // Enforce constrained noise model so lambdas are solved with QR // and should exactly satisfy all the equations - dualGraph->push_back(JacobianFactor(lambdaTerms, gradf_xi, + dualGraph.push_back(JacobianFactor(lambdaTerms, gradf_xi, noiseModel::Constrained::All(gradf_xi.size()))); + + // Add 0 priors on all lambdas to make sure the graph is solvable + // TODO: Can we do for all lambdas like this, or only for those with no information? + BOOST_FOREACH(size_t factorIndex, fullFactorIndices_[xiKey]) { + JacobianFactor::shared_ptr factor = toJacobian(graph.at(factorIndex)); + if (!factor || !factor->isConstrained()) continue; + size_t dim= factor->get_model()->dim(); + // Use factorIndex as the lambda's key. + dualGraph.push_back(JacobianFactor(factorIndex, eye(dim), zero(dim))); + } } return dualGraph; } /// Find max lambda element - std::pair findMostViolatedIneqConstraint(const VectorValues& lambdas) { + std::pair findWeakestViolationIneq(const VectorValues& lambdas) const { int worstFactorIx = -1, worstSigmaIx = -1; double maxLambda = 0.0; BOOST_FOREACH(size_t factorIx, constraintIndices_) { @@ -256,54 +271,127 @@ public: return make_pair(worstFactorIx, worstSigmaIx); } - /** Iterate 1 step */ -// bool iterate() { -// // Obtain the solution from the current working graph -// VectorValues newSolution = workingGraph_.optimize(); -// -// // If we CAN'T move further -// if (newSolution == currentSolution) { -// // Compute lambda from the dual graph -// GaussianFactorGraph dualGraph = buildDualGraph(graph, newSolution); -// VectorValues lambdas = dualGraph.optimize(); -// -// int factorIx, sigmaIx; -// boost::tie(factorIx, sigmaIx) = findMostViolatedIneqConstraint(lambdas); -// // If all constraints are satisfied: We have found the solution!! -// if (factorIx < 0) { -// return true; -// } -// else { -// // If some constraints are violated! -// Vector sigmas = toJacobian(workingGraph.at(factorIx))->get_model()->sigmas(); -// sigmas[sigmaIx] = 0.0; -// toJacobian()->setModel(true, sigmas); -// // No need to update the currentSolution, since we haven't moved anywhere -// } -// } -// else { -// // If we CAN make some progress -// // Adapt stepsize if some inactive inequality constraints complain about this move -// // also add to the working set the one that complains the most -// VectorValues alpha = updateWorkingSetInplace(); -// currentSolution_ = (1 - alpha) * currentSolution_ + alpha * newSolution; -// } -// -// return false; -// } -// -// VectorValues optimize() const { -// bool converged = false; -// while (!converged) { -// converged = iterate(); -// } -// } + /** + * Deactivate or activate an ineq constraint in place + * Warning: modify in-place to avoid copy/clone + * @return true if update successful + */ + bool updateWorkingSetInplace(GaussianFactorGraph& workingGraph, + int factorIx, int sigmaIx, double newSigma) const { + if (factorIx < 0 || sigmaIx < 0) + return false; + Vector sigmas = toJacobian(workingGraph.at(factorIx))->get_model()->sigmas(); + sigmas[sigmaIx] = newSigma; // removing it from the working set + toJacobian(workingGraph.at(factorIx))->setModel(true, sigmas); + return true; + } + + /** + * Compute step size + */ + boost::tuple computeStepSize(const GaussianFactorGraph& workingGraph, + const VectorValues& xk, const VectorValues& p) const { + static bool debug = true; + + double minAlpha = 1.0; + int closestFactorIx = -1, closestSigmaIx = -1; + BOOST_FOREACH(size_t factorIx, constraintIndices_) { + JacobianFactor::shared_ptr jacobian = toJacobian(workingGraph.at(factorIx)); + Vector sigmas = jacobian->get_model()->sigmas(); + Vector b = jacobian->getb(); + for (size_t s = 0; sbegin(); xj != jacobian->end(); ++xj) { + Vector pj = p.at(*xj); + Vector aj = jacobian->getA(xj).row(s); + ajTp += aj.dot(pj); + } + if (debug) { + cout << "s, ajTp: " << s << " " << ajTp << endl; + } + if (ajTp<=0) continue; + double ajTx = 0.0; + for (Factor::const_iterator xj = jacobian->begin(); xj != jacobian->end(); ++xj) { + Vector xkj = xk.at(*xj); + Vector aj = jacobian->getA(xj).row(s); + ajTx += aj.dot(xkj); + } + if (debug) { + cout << "b[s], ajTx: " << b[s] << " " << ajTx << " " << ajTp << endl; + } + double alpha = (b[s] - ajTx)/ajTp; // TODO: compute me! + if (debug) { + cout << "alpha: " << alpha << endl; + } + if (alpha < minAlpha) { + closestFactorIx = factorIx; + closestSigmaIx = s; + minAlpha = alpha; + } + } + } + return boost::make_tuple(minAlpha, closestFactorIx, closestSigmaIx); + } + + /** Iterate 1 step, modify workingGraph and currentSolution in place */ + bool iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& currentSolution) const { + static bool debug = true; + // Obtain the solution from the current working graph + VectorValues newSolution = workingGraph.optimize(); + if (debug) newSolution.print("New solution:"); + + // If we CAN'T move further + if (newSolution.equals(currentSolution, 1e-5)) { + // Compute lambda from the dual graph + GaussianFactorGraph dualGraph = buildDualGraph(workingGraph, newSolution); + if (debug) dualGraph.print("Dual graph: "); + VectorValues lambdas = dualGraph.optimize(); + if (debug) lambdas.print("lambdas :"); + + int factorIx, sigmaIx; + boost::tie(factorIx, sigmaIx) = findWeakestViolationIneq(lambdas); + + // Try to disactivate the weakest violated ineq constraints + // if not successful, i.e. all ineq constraints are satisfied: We have the solution!! + if (!updateWorkingSetInplace(workingGraph, factorIx, sigmaIx, -1.0)) + return true; + } + else { + // If we CAN make some progress + // Adapt stepsize if some inactive inequality constraints complain about this move + double alpha; + int factorIx, sigmaIx; + VectorValues p = newSolution - currentSolution; + boost::tie(alpha, factorIx, sigmaIx) = computeStepSize(workingGraph, currentSolution, p); + if (debug) { + cout << "alpha, factorIx, sigmaIx: " << alpha << " " << factorIx << " " << sigmaIx << endl; + } + // also add to the working set the one that complains the most + updateWorkingSetInplace(workingGraph, factorIx, sigmaIx, 0.0); + // step! + currentSolution = currentSolution + alpha * p; + } + + return false; + } + + VectorValues optimize(const VectorValues& initials) const { + GaussianFactorGraph workingGraph = graph_.clone(); + VectorValues currentSolution = initials; + bool converged = false; + while (!converged) { + converged = iterateInPlace(workingGraph, currentSolution); + } + return currentSolution; + } }; /* ************************************************************************* */ // Create test graph according to Forst10book_pg171Ex5 -std::pair createTestCase() { +GaussianFactorGraph createTestCase() { GaussianFactorGraph graph; // Objective functions x1^2 - x1*x2 + x2^2 - 3*x1 @@ -312,31 +400,25 @@ std::pair createTestCase() { // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 0 graph.push_back( HessianFactor(X(1), X(2), 2.0*ones(1, 1), -ones(1, 1), 3.0*ones(1), - 2.0*ones(1, 1), zero(1), 1.0)); + 2.0*ones(1, 1), zero(1), 10.0)); // Inequality constraints - // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, hence we negate the b vector + // Jacobian factors represent Ax-b, ehnce + // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 Matrix A1 = (Matrix(4, 1)<<1, -1, 0, 1); Matrix A2 = (Matrix(4, 1)<<1, 0, -1, 0); - Vector b = -(Vector(4)<<2, 0, 0, 1.5); + Vector b = (Vector(4)<<2, 0, 0, 1.5); // Special constrained noise model denoting <= inequalities with negative sigmas noiseModel::Constrained::shared_ptr noise = noiseModel::Constrained::MixedSigmas((Vector(4)<<-1, -1, -1, -1)); graph.push_back(JacobianFactor(X(1), A1, X(2), A2, b, noise)); - // Initials values - VectorValues initials; - initials.insert(X(1), ones(1)); - initials.insert(X(2), ones(1)); - - return make_pair(graph, initials); + return graph; } -TEST(QPSolver, constraintsAux) { - GaussianFactorGraph graph; - VectorValues initials; - boost::tie(graph, initials)= createTestCase(); - QPSolver solver(graph, initials); +TEST_DISABLED(QPSolver, constraintsAux) { + GaussianFactorGraph graph = createTestCase(); + QPSolver solver(graph); FastVector constraintIx = solver.constraintIndices(); LONGS_EQUAL(1, constraintIx.size()); LONGS_EQUAL(1, constraintIx[0]); @@ -344,14 +426,14 @@ TEST(QPSolver, constraintsAux) { VectorValues lambdas; lambdas.insert(constraintIx[0], (Vector(4)<< -0.5, 0.0, 0.3, 0.1)); int factorIx, lambdaIx; - boost::tie(factorIx, lambdaIx) = solver.findMostViolatedIneqConstraint(lambdas); + boost::tie(factorIx, lambdaIx) = solver.findWeakestViolationIneq(lambdas); LONGS_EQUAL(1, factorIx); LONGS_EQUAL(2, lambdaIx); VectorValues lambdas2; lambdas2.insert(constraintIx[0], (Vector(4)<< -0.5, 0.0, -0.3, -0.1)); int factorIx2, lambdaIx2; - boost::tie(factorIx2, lambdaIx2) = solver.findMostViolatedIneqConstraint(lambdas2); + boost::tie(factorIx2, lambdaIx2) = solver.findWeakestViolationIneq(lambdas2); LONGS_EQUAL(-1, factorIx2); LONGS_EQUAL(-1, lambdaIx2); @@ -364,8 +446,8 @@ TEST(QPSolver, constraintsAux) { } /* ************************************************************************* */ -// Create test graph according to Forst10book_pg171Ex5 -std::pair createEqualityConstrainedTest() { +// Create a simple test graph with one equality constraint +GaussianFactorGraph createEqualityConstrainedTest() { GaussianFactorGraph graph; // Objective functions x1^2 + x2^2 @@ -386,28 +468,62 @@ std::pair createEqualityConstrainedTest() { noiseModel::Constrained::MixedSigmas((Vector(1)<<0.0)); graph.push_back(JacobianFactor(X(1), A1, X(2), A2, b, noise)); + return graph; +} + +TEST(QPSolver, dual) { + GaussianFactorGraph graph = createEqualityConstrainedTest(); + // Initials values VectorValues initials; initials.insert(X(1), ones(1)); initials.insert(X(2), ones(1)); - return make_pair(graph, initials); -} + QPSolver solver(graph); -TEST(QPSolver, dual) { - GaussianFactorGraph graph; - VectorValues initials; - boost::tie(graph, initials)= createEqualityConstrainedTest(); - QPSolver solver(graph, initials); - - GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph(initials); - dualGraph->print("Dual graph: "); - VectorValues dual = dualGraph->optimize(); + GaussianFactorGraph dualGraph = solver.buildDualGraph(graph, initials); + VectorValues dual = dualGraph.optimize(); VectorValues expectedDual; expectedDual.insert(1, (Vector(1)<<2.0)); CHECK(assert_equal(expectedDual, dual, 1e-100)); } +/* ************************************************************************* */ + +TEST(QPSolver, iterate) { + GaussianFactorGraph graph = createTestCase(); + QPSolver solver(graph); + + GaussianFactorGraph workingGraph = graph.clone(); + + VectorValues currentSolution; + currentSolution.insert(X(1), zeros(1,1)); + currentSolution.insert(X(2), zeros(1,1)); + + workingGraph.print("workingGraph: "); + bool converged = false; + int it = 0; + while (!converged) { + cout << "+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + cout << "Iteration " << it << " :" << endl; + converged = solver.iterateInPlace(workingGraph, currentSolution); + workingGraph.print("workingGraph: "); + currentSolution.print("currentSolution: "); + } +} + +/* ************************************************************************* */ + +TEST(QPSolver, optimize) { + GaussianFactorGraph graph = createTestCase(); + QPSolver solver(graph); + VectorValues initials; + initials.insert(X(1), zeros(1,1)); + initials.insert(X(2), zeros(1,1)); + VectorValues solution = solver.optimize(initials); + solution.print("Solution: "); +} + /* ************************************************************************* */ int main() { TestResult tr;