From bdc2d8f9967c73da59b478235ba0a83f1d0feb60 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 10 Nov 2011 19:44:03 +0000 Subject: [PATCH] Fixed switching to QR for constrained graphs --- gtsam/linear/GaussianFactorGraph.cpp | 26 +++++++++++---------- gtsam/linear/GaussianFactorGraph.h | 6 +++++ gtsam/linear/GaussianMultifrontalSolver.cpp | 4 ++-- gtsam/linear/GaussianSequentialSolver.cpp | 4 ++-- tests/testGaussianFactorGraph.cpp | 18 ++++++++++++++ 5 files changed, 42 insertions(+), 16 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 5eace0094..46462f60f 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -623,6 +623,18 @@ namespace gtsam { return make_pair(conditionals, combinedFactor); } // \EliminateLDL + /* ************************************************************************* */ + bool hasConstraints(const FactorGraph& factors) { + typedef JacobianFactor J; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); + if (jacobian && jacobian->get_model()->isConstrained()) { + return true; + } + } + return false; + } + /* ************************************************************************* */ GaussianFactorGraph::EliminationResult EliminatePreferLDL( const FactorGraph& factors, size_t nrFrontals) { @@ -637,18 +649,8 @@ namespace gtsam { // Decide whether to use QR or LDL // Check if any JacobianFactors have constrained noise models. - bool useQR = false; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); - if (jacobian && jacobian->get_model()->isConstrained()) { - useQR = true; - break; - } - } - - // Convert all factors to the appropriate type - // and call the type-specific EliminateGaussian. - if (useQR) return EliminateQR(factors, nrFrontals); + if (hasConstraints(factors)) + EliminateQR(factors, nrFrontals); GaussianFactorGraph::EliminationResult ret; #ifdef NDEBUG diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 77a1ed68c..6b937c214 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -196,6 +196,12 @@ namespace gtsam { const FactorGraph& factors, const VariableSlots& variableSlots); + /** + * Evaluates whether linear factors have any constrained noise models + * @return true if any factor is constrained. + */ + bool hasConstraints(const FactorGraph& factors); + /** * Densely combine and partially eliminate JacobianFactors to produce a * single conditional with nrFrontals frontal variables and a remaining diff --git a/gtsam/linear/GaussianMultifrontalSolver.cpp b/gtsam/linear/GaussianMultifrontalSolver.cpp index 9e80b7c29..23c017f41 100644 --- a/gtsam/linear/GaussianMultifrontalSolver.cpp +++ b/gtsam/linear/GaussianMultifrontalSolver.cpp @@ -24,12 +24,12 @@ namespace gtsam { /* ************************************************************************* */ GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph& factorGraph, bool useQR) : - Base(factorGraph), useQR_(useQR) {} + Base(factorGraph), useQR_(useQR || hasConstraints(factorGraph)) {} /* ************************************************************************* */ GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex, bool useQR) : - Base(factorGraph, variableIndex), useQR_(useQR) {} + Base(factorGraph, variableIndex), useQR_(useQR || hasConstraints(*factorGraph)) {} /* ************************************************************************* */ GaussianMultifrontalSolver::shared_ptr diff --git a/gtsam/linear/GaussianSequentialSolver.cpp b/gtsam/linear/GaussianSequentialSolver.cpp index bfff98e0f..655598587 100644 --- a/gtsam/linear/GaussianSequentialSolver.cpp +++ b/gtsam/linear/GaussianSequentialSolver.cpp @@ -26,14 +26,14 @@ namespace gtsam { /* ************************************************************************* */ GaussianSequentialSolver::GaussianSequentialSolver( const FactorGraph& factorGraph, bool useQR) : - Base(factorGraph), useQR_(useQR) { + Base(factorGraph), useQR_(useQR || hasConstraints(factorGraph)) { } /* ************************************************************************* */ GaussianSequentialSolver::GaussianSequentialSolver( const FactorGraph::shared_ptr& factorGraph, const VariableIndex::shared_ptr& variableIndex, bool useQR) : - Base(factorGraph, variableIndex), useQR_(useQR) { + Base(factorGraph, variableIndex), useQR_(useQR || hasConstraints(*factorGraph)) { } /* ************************************************************************* */ diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index 2db9ed9b5..d95a05830 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -774,6 +774,8 @@ TEST( GaussianFactorGraph, constrained_simple ) { // get a graph with a constraint in it GaussianFactorGraph fg = createSimpleConstraintGraph(); + EXPECT(hasConstraints(fg)); + // eliminate and solve VectorValues actual = *GaussianSequentialSolver(fg).optimize(); @@ -788,6 +790,7 @@ TEST( GaussianFactorGraph, constrained_single ) { // get a graph with a constraint in it GaussianFactorGraph fg = createSingleConstraintGraph(); + EXPECT(hasConstraints(fg)); // eliminate and solve VectorValues actual = *GaussianSequentialSolver(fg).optimize(); @@ -818,6 +821,7 @@ TEST( GaussianFactorGraph, constrained_multi1 ) { // get a graph with a constraint in it GaussianFactorGraph fg = createMultiConstraintGraph(); + EXPECT(hasConstraints(fg)); // eliminate and solve VectorValues actual = *GaussianSequentialSolver(fg).optimize(); @@ -1011,6 +1015,20 @@ TEST(GaussianFactorGraph, createSmoother2) CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry } +/* ************************************************************************* */ +TEST(GaussianFactorGraph, hasConstraints) +{ + FactorGraph fgc1 = createMultiConstraintGraph(); + EXPECT(hasConstraints(fgc1)); + + FactorGraph fgc2 = createSimpleConstraintGraph() ; + EXPECT(hasConstraints(fgc2)); + + Ordering ordering; ordering += "x1", "x2", "l1"; + FactorGraph fg = createGaussianFactorGraph(ordering); + EXPECT(!hasConstraints(fg)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */