diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index 04cf947fe..b63699fa5 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -114,6 +114,7 @@ typename EliminationTree::shared_ptr EliminationTree::Create( const VariableIndex& structure) { static const bool debug = false; + gttic(ET_Create1); gttic(ET_ComputeParents); // Compute the tree structure @@ -166,10 +167,9 @@ template typename EliminationTree::shared_ptr EliminationTree::Create(const FactorGraph& factorGraph) { + gttic(ET_Create2); // Build variable index - gttic(ET_Create_variable_index); const VariableIndex variableIndex(factorGraph); - gttoc(ET_Create_variable_index); // Build elimination tree return Create(factorGraph, variableIndex); diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 171b2f078..ec7052b94 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -30,20 +30,23 @@ namespace gtsam { /* ************************************************************************* */ template - GenericSequentialSolver::GenericSequentialSolver( - const FactorGraph& factorGraph) : - factors_(new FactorGraph(factorGraph)), structure_( - new VariableIndex(factorGraph)), eliminationTree_( - EliminationTree::Create(*factors_, *structure_)) { + GenericSequentialSolver::GenericSequentialSolver(const FactorGraph& factorGraph) { + gttic(GenericSequentialSolver_constructor1); + factors_.reset(new FactorGraph(factorGraph)); + structure_.reset(new VariableIndex(factorGraph)); + eliminationTree_ = EliminationTree::Create(*factors_, *structure_); } /* ************************************************************************* */ template GenericSequentialSolver::GenericSequentialSolver( const sharedFactorGraph& factorGraph, - const boost::shared_ptr& variableIndex) : - factors_(factorGraph), structure_(variableIndex), eliminationTree_( - EliminationTree::Create(*factors_, *structure_)) { + const boost::shared_ptr& variableIndex) + { + gttic(GenericSequentialSolver_constructor2); + factors_ = factorGraph; + structure_ = variableIndex; + eliminationTree_ = EliminationTree::Create(*factors_, *structure_); } /* ************************************************************************* */ @@ -89,8 +92,9 @@ namespace gtsam { template typename GenericSequentialSolver::sharedBayesNet // GenericSequentialSolver::eliminate(const Permutation& permutation, - Eliminate function, boost::optional nrToEliminate) const { - + Eliminate function, boost::optional nrToEliminate) const + { + gttic(GenericSequentialSolver_eliminate); // Create inverse permutation Permutation::shared_ptr permutationInverse(permutation.inverse()); @@ -126,8 +130,9 @@ namespace gtsam { typename GenericSequentialSolver::sharedBayesNet // GenericSequentialSolver::conditionalBayesNet( const std::vector& js, size_t nrFrontals, - Eliminate function) const { - + Eliminate function) const + { + gttic(GenericSequentialSolver_conditionalBayesNet); // Compute a COLAMD permutation with the marginal variables constrained to the end. // TODO in case of nrFrontals, the order of js has to be respected here ! Permutation::shared_ptr permutation( @@ -169,8 +174,9 @@ namespace gtsam { template typename GenericSequentialSolver::sharedBayesNet // GenericSequentialSolver::jointBayesNet(const std::vector& js, - Eliminate function) const { - + Eliminate function) const + { + gttic(GenericSequentialSolver_jointBayesNet); // Compute a COLAMD permutation with the marginal variables constrained to the end. Permutation::shared_ptr permutation( inference::PermutationCOLAMD(*structure_, js)); @@ -190,11 +196,11 @@ namespace gtsam { template typename FactorGraph::shared_ptr // GenericSequentialSolver::jointFactorGraph( - const std::vector& js, Eliminate function) const { - + const std::vector& js, Eliminate function) const + { + gttic(GenericSequentialSolver_jointFactorGraph); // Eliminate all variables - typename BayesNet::shared_ptr bayesNet = // - jointBayesNet(js, function); + typename BayesNet::shared_ptr bayesNet = jointBayesNet(js, function); return boost::make_shared >(*bayesNet); } @@ -202,8 +208,8 @@ namespace gtsam { /* ************************************************************************* */ template typename boost::shared_ptr // - GenericSequentialSolver::marginalFactor(Index j, - Eliminate function) const { + GenericSequentialSolver::marginalFactor(Index j, Eliminate function) const { + gttic(GenericSequentialSolver_marginalFactor); // Create a container for the one variable index std::vector js(1); js[0] = j; diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 89c48bd85..762a05cc8 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -24,6 +24,7 @@ #include #include +#include namespace gtsam { @@ -162,7 +163,7 @@ protected: /* ************************************************************************* */ template void VariableIndex::fill(const FG& factorGraph) { - + gttic(VariableIndex_fill); // Build index mapping from variable id to factor index for(size_t fi=0; fi VariableIndex::VariableIndex(const FG& factorGraph) : - nFactors_(0), nEntries_(0) { - + nFactors_(0), nEntries_(0) +{ + gttic(VariableIndex_constructor); // If the factor graph is empty, return an empty index because inside this // if block we assume at least one factor. if(factorGraph.size() > 0) { + gttic(VariableIndex_constructor_find_highest); // Find highest-numbered variable Index maxVar = 0; BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) { @@ -195,9 +198,12 @@ VariableIndex::VariableIndex(const FG& factorGraph) : } } } + gttoc(VariableIndex_constructor_find_highest); // Allocate array + gttic(VariableIndex_constructor_allocate); index_.resize(maxVar+1); + gttoc(VariableIndex_constructor_allocate); fill(factorGraph); } @@ -206,13 +212,18 @@ VariableIndex::VariableIndex(const FG& factorGraph) : /* ************************************************************************* */ template VariableIndex::VariableIndex(const FG& factorGraph, Index nVariables) : - index_(nVariables), nFactors_(0), nEntries_(0) { + nFactors_(0), nEntries_(0) +{ + gttic(VariableIndex_constructor_allocate); + index_.resize(nVariables); + gttoc(VariableIndex_constructor_allocate); fill(factorGraph); } /* ************************************************************************* */ template void VariableIndex::augment(const FG& factors) { + gttic(VariableIndex_augment); // If the factor graph is empty, return an empty index because inside this // if block we assume at least one factor. if(factors.size() > 0) { @@ -247,6 +258,7 @@ void VariableIndex::augment(const FG& factors) { /* ************************************************************************* */ template void VariableIndex::remove(const CONTAINER& indices, const FG& factors) { + gttic(VariableIndex_remove); // NOTE: We intentionally do not decrement nFactors_ because the factor // indices need to remain consistent. Removing factors from a factor graph // does not shift the indices of other factors. Also, we keep nFactors_ diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 11a45cdd4..ba13e23c1 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -27,6 +28,7 @@ namespace gtsam { /* ************************************************************************* */ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization) { + gttic(MarginalsConstructor); // Compute COLAMD ordering ordering_ = *graph.orderingCOLAMD(solution); @@ -60,6 +62,7 @@ Matrix Marginals::marginalCovariance(Key variable) const { /* ************************************************************************* */ Matrix Marginals::marginalInformation(Key variable) const { + gttic(marginalInformation); // Get linear key Index index = ordering_[variable]; @@ -71,6 +74,7 @@ Matrix Marginals::marginalInformation(Key variable) const { marginalFactor = bayesTree_.marginalFactor(index, EliminateQR); // Get information matrix (only store upper-right triangle) + gttic(AsMatrix); return marginalFactor->information(); }