From 6d1b86c2e04ffd185171cc8d156deb650f8285f3 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 6 Sep 2012 14:33:44 +0000 Subject: [PATCH] Moved JacobianFactor type check/conversion functions into SubgraphSolver instead of GaussianFactorGraph and JacobianFactor --- gtsam/linear/GaussianFactorGraph.cpp | 17 ++++++++--------- gtsam/linear/GaussianFactorGraph.h | 3 --- gtsam/linear/JacobianFactor.cpp | 9 --------- gtsam/linear/JacobianFactor.h | 3 --- gtsam/linear/SubgraphPreconditioner.cpp | 13 +++++++++++++ gtsam/linear/SubgraphPreconditioner.h | 1 - gtsam/linear/SubgraphSolver.cpp | 12 +++++------- 7 files changed, 26 insertions(+), 32 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index eda9732b2..222f025a0 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -523,6 +523,14 @@ break; + /* ************************************************************************* */ + static JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { + JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast(gf); + if( !result ) { + result = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + } + return result; + } /* ************************************************************************* */ Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) { @@ -635,13 +643,4 @@ break; return e; } - /* ************************************************************************* */ - GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { - GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); - BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { - result->push_back(convertToJacobianFactorPtr(gf)); - } - return result; - } - } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index b6486b3a6..5e6e792be 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -354,7 +354,4 @@ namespace gtsam { inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) { return *gaussianErrors_(fg, x); } - /** Convert all factors to JacobianFactor */ - GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg); - } // namespace gtsam diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index c0aefab39..759be4948 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -489,13 +489,4 @@ namespace gtsam { return description_.c_str(); } - /* ************************************************************************* */ - JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { - JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast(gf); - if( !result ) { - result = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - } - return result; - } - } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 1c73f621a..f2bd9c75d 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -324,8 +324,5 @@ namespace gtsam { } }; // JacobianFactor - /** cast from GaussianFactor::shared_ptr to JacobianFactor::shared_ptr */ - JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf); - } // gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 04eed29d2..8b5a3708f 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -22,6 +22,19 @@ using namespace std; namespace gtsam { + + /* ************************************************************************* */ + static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { + GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); + BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { + JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); + if( !jf ) { + jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + } + result->push_back(jf); + } + return result; + } /* ************************************************************************* */ SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index db7c023ce..f266928d7 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -33,7 +33,6 @@ namespace gtsam { class SubgraphPreconditioner { public: - typedef boost::shared_ptr shared_ptr; typedef boost::shared_ptr sharedBayesNet; typedef boost::shared_ptr sharedFG; diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 4cb95c404..9986fb76c 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -111,26 +111,24 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { size_t t = 0; BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) { - JacobianFactor::shared_ptr jf = convertToJacobianFactorPtr(gf); - - if ( jf->keys().size() > 2 ) { + if ( gf->keys().size() > 2 ) { throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); } bool augment = false ; /* check whether this factor should be augmented to the "tree" graph */ - if ( jf->keys().size() == 1 ) augment = true; + if ( gf->keys().size() == 1 ) augment = true; else { - const Index u = jf->keys()[0], v = jf->keys()[1], + const Index u = gf->keys()[0], v = gf->keys()[1], u_root = D.findSet(u), v_root = D.findSet(v); if ( u_root != v_root ) { t++; augment = true ; D.makeUnionInPlace(u_root, v_root); } } - if ( augment ) At->push_back(jf); - else Ac->push_back(jf); + if ( augment ) At->push_back(gf); + else Ac->push_back(gf); } return boost::tie(At, Ac);