diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index a072b95b8..389066ead 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -278,7 +278,7 @@ namespace gtsam { * linearize the non-linear graph around the current config */ boost::shared_ptr linearize(const NonlinearGraph& g, const Config& config) const { - return g.linearize_(config); + return g.linearize(config); } }; } diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h index 24134536b..f5c1e0940 100644 --- a/cpp/ISAM2-inl.h +++ b/cpp/ISAM2-inl.h @@ -67,9 +67,10 @@ namespace gtsam { /** Create a Bayes Tree from a nonlinear factor graph */ template ISAM2::ISAM2(const NonlinearFactorGraph& nlfg, const Ordering& ordering, const Config& config) - : BayesTree(nlfg.linearize(config).eliminate(ordering)), theta_(config), thetaFuture_(config), nonlinearFactors_(nlfg) { + : BayesTree(nlfg.linearize(config)->eliminate(ordering)), theta_(config), thetaFuture_(config), nonlinearFactors_(nlfg) { // todo: repeats calculation above, just to set "cached" - _eliminate_const(nlfg.linearize(config), cached_, ordering); + // De-referencing shared pointer can be quite expensive because creates temporary + _eliminate_const(*nlfg.linearize(config), cached_, ordering); } /* ************************************************************************* */ @@ -90,7 +91,8 @@ namespace gtsam { // retrieve all factors that ONLY contain the affected variables // (note that the remaining stuff is summarized in the cached factors) template - FactorGraph ISAM2::relinearizeAffectedFactors(const set& affectedKeys) const { + boost::shared_ptr ISAM2::relinearizeAffectedFactors + (const set& affectedKeys) const { list affectedKeysList; // todo: shouldn't have to convert back to list... affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end()); @@ -110,6 +112,7 @@ namespace gtsam { nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]); } + // TODO: temporary might be expensive, return shared pointer ? return nonlinearAffectedFactors.linearize(theta_); } @@ -190,7 +193,7 @@ namespace gtsam { //// 6 - find factors connected to affected variables //// 7 - linearize - FactorGraph factors; + boost::shared_ptr factors; if (relinFromLast) { // ordering provides all keys in conditionals, there cannot be others because path to root included @@ -207,25 +210,26 @@ namespace gtsam { factors = relinearizeAffectedFactors(affectedKeys); // Save number of affected factors - lastAffectedFactorCount = factors.size(); + lastAffectedFactorCount = factors->size(); // add the cached intermediate results from the boundary of the orphans ... FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - factors.push_back(cachedBoundary); + factors->push_back(cachedBoundary); } else { // reuse the old factors FactorGraph tmp(affectedBayesNet); - factors.push_back(tmp); - factors.push_back(newFactors.linearize(theta_)); + factors.reset(new GaussianFactorGraph); + factors->push_back(tmp); + factors->push_back(*newFactors.linearize(theta_)); // avoid temporary ? } //// 8 - eliminate and add orphans back in // create an ordering for the new and contaminated factors - Ordering ordering = factors.getOrdering(); + Ordering ordering = factors->getOrdering(); // eliminate into a Bayes net - BayesNet bayesNet = _eliminate(factors, cached_, ordering); + BayesNet bayesNet = _eliminate(*factors, cached_, ordering); // Create Index from ordering IndexTable index(ordering); diff --git a/cpp/ISAM2.h b/cpp/ISAM2.h index 8ef2a3259..7242345f3 100644 --- a/cpp/ISAM2.h +++ b/cpp/ISAM2.h @@ -96,7 +96,7 @@ namespace gtsam { private: std::list getAffectedFactors(const std::list& keys) const; - FactorGraph relinearizeAffectedFactors(const std::set& affectedKeys) const; + boost::shared_ptr relinearizeAffectedFactors(const std::set& affectedKeys) const; FactorGraph getCachedBoundaryFactors(Cliques& orphans); }; // ISAM2 diff --git a/cpp/NonlinearFactorGraph-inl.h b/cpp/NonlinearFactorGraph-inl.h index 0816fc09d..01ef0a82c 100644 --- a/cpp/NonlinearFactorGraph-inl.h +++ b/cpp/NonlinearFactorGraph-inl.h @@ -42,24 +42,22 @@ namespace gtsam { /* ************************************************************************* */ template - boost::shared_ptr NonlinearFactorGraph::linearize_( + boost::shared_ptr NonlinearFactorGraph::linearize( const Config& config) const{ // create an empty linear FG - boost::shared_ptr linearFG(new GaussianFactorGraph); + boost::shared_ptr linearFG(new GaussianFactorGraph); // linearize all factors - BOOST_FOREACH(typename NonlinearFactorGraph::sharedFactor factor, this->factors_) { + typedef typename NonlinearFactorGraph::sharedFactor Factor; + BOOST_FOREACH(const Factor& factor, this->factors_) { boost::shared_ptr lf = factor->linearize(config); linearFG->push_back(lf); } + return linearFG; } + /* ************************************************************************* */ - template - GaussianFactorGraph NonlinearFactorGraph::linearize( - const Config& config) const { - return *linearize_(config); - } } // namespace gtsam diff --git a/cpp/NonlinearFactorGraph.h b/cpp/NonlinearFactorGraph.h index a413e7c7d..ddcecd459 100644 --- a/cpp/NonlinearFactorGraph.h +++ b/cpp/NonlinearFactorGraph.h @@ -49,13 +49,8 @@ namespace gtsam { /** * linearize a nonlinear factor graph */ - GaussianFactorGraph linearize(const Config& config) const; - - /** - * shared pointer versions for MATLAB - */ boost::shared_ptr - linearize_(const Config& config) const; + linearize(const Config& config) const; }; diff --git a/cpp/SubgraphPreconditioner-inl.h b/cpp/SubgraphPreconditioner-inl.h index bf6f1f56c..bc81b9229 100644 --- a/cpp/SubgraphPreconditioner-inl.h +++ b/cpp/SubgraphPreconditioner-inl.h @@ -52,16 +52,18 @@ namespace gtsam { /* ************************************************************************* */ template boost::shared_ptr SubgraphPCG::linearize(const G& g, const T& theta_bar) const { - SubgraphPreconditioner::sharedFG Ab1 = T_.linearize_(theta_bar); - SubgraphPreconditioner::sharedFG Ab2 = C_.linearize_(theta_bar); + SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar); + SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar); #ifdef TIMING SubgraphPreconditioner::sharedBayesNet Rc1; SubgraphPreconditioner::sharedConfig xbar; #else - GaussianFactorGraph sacrificialAb1 = T_.linearize(theta_bar); // duplicate !!!!! + GaussianFactorGraph sacrificialAb1 = *Ab1; // duplicate !!!!! SubgraphPreconditioner::sharedBayesNet Rc1 = sacrificialAb1.eliminate_(*ordering_); SubgraphPreconditioner::sharedConfig xbar = gtsam::optimize_(*Rc1); #endif + // TODO: there does not seem to be a good reason to have Ab1_ + // It seems only be used to provide an ordering for creating sparse matrices return boost::shared_ptr(new SubgraphPreconditioner(Ab1, Ab2, Rc1, xbar)); } diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index be522c578..c8d485e4e 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -266,8 +266,7 @@ namespace example { Config poses; boost::tie(nlfg, poses) = createNonlinearSmoother(T); - GaussianFactorGraph lfg = nlfg.linearize(poses); - return lfg; + return *nlfg.linearize(poses); } /* ************************************************************************* */ @@ -556,9 +555,7 @@ namespace example { } // linearize around zero - GaussianFactorGraph A = nlfg.linearize(zeros); - - return make_pair(A, xtrue); + return make_pair(*nlfg.linearize(zeros), xtrue); } /* ************************************************************************* */ diff --git a/cpp/testIterative.cpp b/cpp/testIterative.cpp index 35d451cde..1f841fae9 100644 --- a/cpp/testIterative.cpp +++ b/cpp/testIterative.cpp @@ -122,8 +122,8 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) zeros.insert("x1",zero(3)); zeros.insert("x2",zero(3)); - GaussianFactorGraph fg = graph.linearize(config); - VectorConfig actual = conjugateGradientDescent(fg, zeros, verbose, 1e-3, 1e-5, 100); + boost::shared_ptr fg = graph.linearize(config); + VectorConfig actual = conjugateGradientDescent(*fg, zeros, verbose, 1e-3, 1e-5, 100); VectorConfig expected; expected.insert("x1", zero(3)); @@ -157,10 +157,10 @@ TEST( Iterative, subgraphPCG ) graph.split(tree, T, C); // build the subgraph PCG system - GaussianFactorGraph Ab1_ = T.linearize(theta_bar); - SubgraphPreconditioner::sharedFG Ab1 = T.linearize_(theta_bar); - SubgraphPreconditioner::sharedFG Ab2 = C.linearize_(theta_bar); - SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1_.eliminate_(ordering); + boost::shared_ptr Ab1_ = T.linearize(theta_bar); + SubgraphPreconditioner::sharedFG Ab1 = T.linearize(theta_bar); + SubgraphPreconditioner::sharedFG Ab2 = C.linearize(theta_bar); + SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1_->eliminate_(ordering); SubgraphPreconditioner::sharedConfig xbar = optimize_(*Rc1); SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar); diff --git a/cpp/testNonlinearFactorGraph.cpp b/cpp/testNonlinearFactorGraph.cpp index 058a5cbfa..3b91cbcdd 100644 --- a/cpp/testNonlinearFactorGraph.cpp +++ b/cpp/testNonlinearFactorGraph.cpp @@ -73,9 +73,9 @@ TEST( Graph, linearize ) { Graph fg = createNonlinearFactorGraph(); Config initial = createNoisyConfig(); - GaussianFactorGraph linearized = fg.linearize(initial); + boost::shared_ptr linearized = fg.linearize(initial); GaussianFactorGraph expected = createGaussianFactorGraph(); - CHECK(assert_equal(expected,linearized)); // Needs correct linearizations + CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations } /* ************************************************************************* */ diff --git a/cpp/testPose2SLAM.cpp b/cpp/testPose2SLAM.cpp index 8912597c2..305ee7b8e 100644 --- a/cpp/testPose2SLAM.cpp +++ b/cpp/testPose2SLAM.cpp @@ -63,8 +63,8 @@ TEST( Pose2Graph, linearization ) config.insert(1,p1); config.insert(2,p2); // Linearize - GaussianFactorGraph lfg_linearized = graph.linearize(config); - //lfg_linearized.print("lfg_actual"); + boost::shared_ptr lfg_linearized = graph.linearize(config); + //lfg_linearized->print("lfg_actual"); // the expected linear factor GaussianFactorGraph lfg_expected; @@ -83,7 +83,7 @@ TEST( Pose2Graph, linearization ) SharedDiagonal probModel1 = noiseModel::Unit::Create(3); lfg_expected.add("x1", A1, "x2", A2, b, probModel1); - CHECK(assert_equal(lfg_expected, lfg_linearized)); + CHECK(assert_equal(lfg_expected, *lfg_linearized)); } /* ************************************************************************* */ diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 53d6c2d6e..4bb5b7ace 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -430,16 +430,16 @@ TEST (SQP, two_pose ) { Config2D state(*initialEstimate); // linearize the graph - GaussianFactorGraph fg = graph->linearize(state); + boost::shared_ptr fg = graph->linearize(state); - if (verbose) fg.print("Linearized graph"); + if (verbose) fg->print("Linearized graph"); // create an ordering Ordering ordering; ordering += "x1", "x2", "l1", "l2", "L1"; // optimize linear graph to get full delta config - GaussianBayesNet cbn = fg.eliminate(ordering); + GaussianBayesNet cbn = fg->eliminate(ordering); if (verbose) cbn.print("ChordalBayesNet"); VectorConfig delta = optimize(cbn); //fg.optimize(ordering); diff --git a/cpp/testVSLAMFactor.cpp b/cpp/testVSLAMFactor.cpp index 2927054d4..1bc3c162b 100644 --- a/cpp/testVSLAMFactor.cpp +++ b/cpp/testVSLAMFactor.cpp @@ -62,8 +62,8 @@ TEST( ProjectionFactor, error ) graph.push_back(factor); GaussianFactorGraph expected_lfg; expected_lfg.push_back(actual); - GaussianFactorGraph actual_lfg = graph.linearize(config); - CHECK(assert_equal(expected_lfg,actual_lfg)); + boost::shared_ptr actual_lfg = graph.linearize(config); + CHECK(assert_equal(expected_lfg,*actual_lfg)); // expmap on a config VectorConfig delta;