12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed switching to QR for constrained graphs

release/4.3a0
Alex Cunningham 2011-11-10 19:44:03 +00:00
parent af9320e468
commit bdc2d8f996
5 changed files with 42 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -623,6 +623,18 @@ namespace gtsam {
return make_pair(conditionals, combinedFactor);
} // \EliminateLDL
/* ************************************************************************* */
bool hasConstraints(const FactorGraph<GaussianFactor>& factors) {
typedef JacobianFactor J;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian && jacobian->get_model()->isConstrained()) {
return true;
}
}
return false;
}
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminatePreferLDL(
const FactorGraph<GaussianFactor>& 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<J>(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

6
gtsam/linear/GaussianFactorGraph.h
View File

@ -196,6 +196,12 @@ namespace gtsam {
const FactorGraph<JacobianFactor>& factors,
const VariableSlots& variableSlots);
/**
* Evaluates whether linear factors have any constrained noise models
* @return true if any factor is constrained.
*/
bool hasConstraints(const FactorGraph<GaussianFactor>& factors);
/**
* Densely combine and partially eliminate JacobianFactors to produce a
* single conditional with nrFrontals frontal variables and a remaining

4
gtsam/linear/GaussianMultifrontalSolver.cpp
View File

@ -24,12 +24,12 @@ namespace gtsam {
/* ************************************************************************* */
GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>& factorGraph, bool useQR) :
Base(factorGraph), useQR_(useQR) {}
Base(factorGraph), useQR_(useQR || hasConstraints(factorGraph)) {}
/* ************************************************************************* */
GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>::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

4
gtsam/linear/GaussianSequentialSolver.cpp
View File

@ -26,14 +26,14 @@ namespace gtsam {
/* ************************************************************************* */
GaussianSequentialSolver::GaussianSequentialSolver(
const FactorGraph<GaussianFactor>& factorGraph, bool useQR) :
Base(factorGraph), useQR_(useQR) {
Base(factorGraph), useQR_(useQR || hasConstraints(factorGraph)) {
}
/* ************************************************************************* */
GaussianSequentialSolver::GaussianSequentialSolver(
const FactorGraph<GaussianFactor>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex, bool useQR) :
Base(factorGraph, variableIndex), useQR_(useQR) {
Base(factorGraph, variableIndex), useQR_(useQR || hasConstraints(*factorGraph)) {
}
/* ************************************************************************* */

18
tests/testGaussianFactorGraph.cpp
View File

@ -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<GaussianFactor> fgc1 = createMultiConstraintGraph();
EXPECT(hasConstraints(fgc1));
FactorGraph<GaussianFactor> fgc2 = createSimpleConstraintGraph() ;
EXPECT(hasConstraints(fgc2));
Ordering ordering; ordering += "x1", "x2", "l1";
FactorGraph<GaussianFactor> fg = createGaussianFactorGraph(ordering);
EXPECT(!hasConstraints(fg));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */