build dualgraph supports least-squares multipliers

release/4.3a0
thduynguyen 2014-04-17 12:01:29 -04:00
parent 075817b31a
commit 4681c05063
3 changed files with 36 additions and 15 deletions

View File

@ -95,7 +95,7 @@ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph,
const VectorValues& x0) const { const VectorValues& x0, bool useLeastSquare) const {
static const bool debug = false; static const bool debug = false;
// The dual graph to return // The dual graph to return
@ -114,7 +114,9 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph,
size_t xiDim = xiFactor0->getDim(xiFactor0->find(xiKey)); size_t xiDim = xiFactor0->getDim(xiFactor0->find(xiKey));
if (debug) cout << "xiDim: " << xiDim << endl; 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); Vector gradf_xi = zero(xiDim);
BOOST_FOREACH(size_t factorIx, xiFactors) { BOOST_FOREACH(size_t factorIx, xiFactors) {
HessianFactor::shared_ptr factor = toHessian(freeHessians_->at(factorIx)); HessianFactor::shared_ptr factor = toHessian(freeHessians_->at(factorIx));
@ -140,9 +142,13 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph,
gradf_xi += -factor->linearTerm(xi); 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 // 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<std::pair<Key, Matrix> > lambdaTerms; // collection of lambda_k, and gradc_k std::vector<std::pair<Key, Matrix> > lambdaTerms; // collection of lambda_k, and gradc_k
typedef std::pair<size_t, size_t> FactorIx_SigmaIx;
std::vector<FactorIx_SigmaIx> unconstrainedIndex; // pairs of factorIx,sigmaIx of unconstrained rows
BOOST_FOREACH(size_t factorIndex, fullFactorIndices_[xiKey]) { BOOST_FOREACH(size_t factorIndex, fullFactorIndices_[xiKey]) {
JacobianFactor::shared_ptr factor = toJacobian(graph.at(factorIndex)); JacobianFactor::shared_ptr factor = toJacobian(graph.at(factorIndex));
if (!factor || !factor->isConstrained()) continue; if (!factor || !factor->isConstrained()) continue;
@ -157,26 +163,41 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph,
// we have no information about it // we have no information about it
if (fabs(sigmas[sigmaIx]) > 1e-9) { if (fabs(sigmas[sigmaIx]) > 1e-9) {
A_k.col(sigmaIx) = zero(A_k.rows()); 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. // Use factorIndex as the lambda's key.
lambdaTerms.push_back(make_pair(factorIndex, A_k)); 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? // Create and add factors to the dual graph
BOOST_FOREACH(size_t factorIndex, fullFactorIndices_[xiKey]) { // If least square approximation is desired, use unit noise model.
JacobianFactor::shared_ptr factor = toJacobian(graph.at(factorIndex)); if (useLeastSquare) {
if (!factor || !factor->isConstrained()) continue; 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(); 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. // 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; return dualGraph;
} }

View File

@ -71,7 +71,7 @@ public:
* Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi * Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi
*/ */
GaussianFactorGraph buildDualGraph(const GaussianFactorGraph& graph, GaussianFactorGraph buildDualGraph(const GaussianFactorGraph& graph,
const VectorValues& x0) const; const VectorValues& x0, bool useLeastSquare = false) const;
/** /**

View File

@ -124,7 +124,7 @@ TEST(QPSolver, dual) {
VectorValues dual = dualGraph.optimize(); VectorValues dual = dualGraph.optimize();
VectorValues expectedDual; VectorValues expectedDual;
expectedDual.insert(1, (Vector(1)<<2.0)); expectedDual.insert(1, (Vector(1)<<2.0));
CHECK(assert_equal(expectedDual, dual, 1e-100)); CHECK(assert_equal(expectedDual, dual, 1e-10));
} }
/* ************************************************************************* */ /* ************************************************************************* */