From 4681c050634f489f1fddcfad7ca01b10c34c3c21 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Thu, 17 Apr 2014 12:01:29 -0400 Subject: [PATCH] build dualgraph supports least-squares multipliers --- gtsam/linear/QPSolver.cpp | 47 +++++++++++++++++++++-------- gtsam/linear/QPSolver.h | 2 +- gtsam/linear/tests/testQPSolver.cpp | 2 +- 3 files changed, 36 insertions(+), 15 deletions(-) diff --git a/gtsam/linear/QPSolver.cpp b/gtsam/linear/QPSolver.cpp index 12eb604d8..0bb28645f 100644 --- a/gtsam/linear/QPSolver.cpp +++ b/gtsam/linear/QPSolver.cpp @@ -95,7 +95,7 @@ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars /* ************************************************************************* */ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, - const VectorValues& x0) const { + const VectorValues& x0, bool useLeastSquare) const { static const bool debug = false; // The dual graph to return @@ -114,7 +114,9 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, size_t xiDim = xiFactor0->getDim(xiFactor0->find(xiKey)); if (debug) cout << "xiDim: " << xiDim << endl; - // Compute gradf(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + // Compute the b-vector for the dual factor Ax-b + // b = gradf(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi Vector gradf_xi = zero(xiDim); BOOST_FOREACH(size_t factorIx, xiFactors) { HessianFactor::shared_ptr factor = toHessian(freeHessians_->at(factorIx)); @@ -140,9 +142,13 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, gradf_xi += -factor->linearTerm(xi); } + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + // Compute the Jacobian A for the dual factor Ax-b // Obtain the jacobians for lambda variables from their corresponding constraints - // gradc_k(xi) = \frac{\partial c_k}{\partial xi}' + // A = gradc_k(xi) = \frac{\partial c_k}{\partial xi}' std::vector > lambdaTerms; // collection of lambda_k, and gradc_k + typedef std::pair FactorIx_SigmaIx; + std::vector unconstrainedIndex; // pairs of factorIx,sigmaIx of unconstrained rows BOOST_FOREACH(size_t factorIndex, fullFactorIndices_[xiKey]) { JacobianFactor::shared_ptr factor = toJacobian(graph.at(factorIndex)); if (!factor || !factor->isConstrained()) continue; @@ -157,26 +163,41 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, // we have no information about it if (fabs(sigmas[sigmaIx]) > 1e-9) { A_k.col(sigmaIx) = zero(A_k.rows()); + // remember to add a zero prior on this lambda, otherwise the graph is under-determined + unconstrainedIndex.push_back(make_pair(factorIndex, sigmaIx)); } } // Use factorIndex as the lambda's key. lambdaTerms.push_back(make_pair(factorIndex, A_k)); } - // Enforce constrained noise model so lambdas are solved with QR - // and should exactly satisfy all the equations - 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; + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// + // Create and add factors to the dual graph + // If least square approximation is desired, use unit noise model. + if (useLeastSquare) { + dualGraph.push_back(JacobianFactor(lambdaTerms, gradf_xi, + noiseModel::Unit::Create(gradf_xi.size()))); + } + else { + // Enforce constrained noise model so lambdas are solved with QR + // and should exactly satisfy all the equations + dualGraph.push_back(JacobianFactor(lambdaTerms, gradf_xi, + noiseModel::Constrained::All(gradf_xi.size()))); + } + + // Add 0 priors on all lambdas of the unconstrained rows to make sure the graph is solvable + BOOST_FOREACH(FactorIx_SigmaIx factorIx_sigmaIx, unconstrainedIndex) { + size_t factorIx = factorIx_sigmaIx.first; + JacobianFactor::shared_ptr factor = toJacobian(graph.at(factorIx)); size_t dim= factor->get_model()->dim(); + Matrix J = zeros(dim, dim); + size_t sigmaIx = factorIx_sigmaIx.second; + J(sigmaIx,sigmaIx) = 1.0; // Use factorIndex as the lambda's key. - dualGraph.push_back(JacobianFactor(factorIndex, eye(dim), zero(dim))); + dualGraph.push_back(JacobianFactor(factorIx, J, zero(dim))); } } + return dualGraph; } diff --git a/gtsam/linear/QPSolver.h b/gtsam/linear/QPSolver.h index b397aa513..4f6c81898 100644 --- a/gtsam/linear/QPSolver.h +++ b/gtsam/linear/QPSolver.h @@ -71,7 +71,7 @@ public: * Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi */ GaussianFactorGraph buildDualGraph(const GaussianFactorGraph& graph, - const VectorValues& x0) const; + const VectorValues& x0, bool useLeastSquare = false) const; /** diff --git a/gtsam/linear/tests/testQPSolver.cpp b/gtsam/linear/tests/testQPSolver.cpp index d7ec97a24..1434f9b2f 100644 --- a/gtsam/linear/tests/testQPSolver.cpp +++ b/gtsam/linear/tests/testQPSolver.cpp @@ -124,7 +124,7 @@ TEST(QPSolver, dual) { VectorValues dual = dualGraph.optimize(); VectorValues expectedDual; expectedDual.insert(1, (Vector(1)<<2.0)); - CHECK(assert_equal(expectedDual, dual, 1e-100)); + CHECK(assert_equal(expectedDual, dual, 1e-10)); } /* ************************************************************************* */