Added additional timing instrumentation in elimination data structures

gtsam/inference/EliminationTree-inl.h

@@ -114,6 +114,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
     const VariableIndex& structure) {
 
   static const bool debug = false;
+  gttic(ET_Create1);
 
   gttic(ET_ComputeParents);
   // Compute the tree structure
@@ -166,10 +167,9 @@ template<class DERIVEDFACTOR>
 typename EliminationTree<FACTOR>::shared_ptr
 EliminationTree<FACTOR>::Create(const FactorGraph<DERIVEDFACTOR>& 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);

gtsam/inference/GenericSequentialSolver-inl.h

@@ -30,20 +30,23 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class FACTOR>
-  GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
-      const FactorGraph<FACTOR>& factorGraph) :
-      factors_(new FactorGraph<FACTOR>(factorGraph)), structure_(
-          new VariableIndex(factorGraph)), eliminationTree_(
-          EliminationTree<FACTOR>::Create(*factors_, *structure_)) {
+  GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph) {
+    gttic(GenericSequentialSolver_constructor1);
+    factors_.reset(new FactorGraph<FACTOR>(factorGraph));
+    structure_.reset(new VariableIndex(factorGraph));
+    eliminationTree_ = EliminationTree<FACTOR>::Create(*factors_, *structure_);
   }
 
   /* ************************************************************************* */
   template<class FACTOR>
   GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
       const sharedFactorGraph& factorGraph,
-      const boost::shared_ptr<VariableIndex>& variableIndex) :
-      factors_(factorGraph), structure_(variableIndex), eliminationTree_(
-          EliminationTree<FACTOR>::Create(*factors_, *structure_)) {
+      const boost::shared_ptr<VariableIndex>& variableIndex)
+  {
+    gttic(GenericSequentialSolver_constructor2);
+    factors_ = factorGraph;
+    structure_ = variableIndex;
+    eliminationTree_ = EliminationTree<FACTOR>::Create(*factors_, *structure_);
   }
 
   /* ************************************************************************* */
@@ -89,8 +92,9 @@ namespace gtsam {
   template<class FACTOR>
   typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
   GenericSequentialSolver<FACTOR>::eliminate(const Permutation& permutation,
-      Eliminate function, boost::optional<size_t> nrToEliminate) const {
-
+      Eliminate function, boost::optional<size_t> nrToEliminate) const
+  {
+    gttic(GenericSequentialSolver_eliminate);
     // Create inverse permutation
     Permutation::shared_ptr permutationInverse(permutation.inverse());
 
@@ -126,8 +130,9 @@ namespace gtsam {
   typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
   GenericSequentialSolver<FACTOR>::conditionalBayesNet(
       const std::vector<Index>& 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<class FACTOR>
   typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
   GenericSequentialSolver<FACTOR>::jointBayesNet(const std::vector<Index>& 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<class FACTOR>
   typename FactorGraph<FACTOR>::shared_ptr //
   GenericSequentialSolver<FACTOR>::jointFactorGraph(
-      const std::vector<Index>& js, Eliminate function) const {
-
+      const std::vector<Index>& js, Eliminate function) const
+  {
+    gttic(GenericSequentialSolver_jointFactorGraph);
     // Eliminate all variables
-    typename BayesNet<Conditional>::shared_ptr bayesNet = //
-          jointBayesNet(js, function);
+    typename BayesNet<Conditional>::shared_ptr bayesNet = jointBayesNet(js, function);
 
     return boost::make_shared<FactorGraph<FACTOR> >(*bayesNet);
   }
@@ -202,8 +208,8 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class FACTOR>
   typename boost::shared_ptr<FACTOR> //
-  GenericSequentialSolver<FACTOR>::marginalFactor(Index j,
-      Eliminate function) const {
+  GenericSequentialSolver<FACTOR>::marginalFactor(Index j, Eliminate function) const {
+    gttic(GenericSequentialSolver_marginalFactor);
     // Create a container for the one variable index
     std::vector<Index> js(1);
     js[0] = j;

gtsam/inference/VariableIndex.h

@@ -24,6 +24,7 @@
 
 #include <gtsam/base/FastList.h>
 #include <gtsam/base/types.h>
+#include <gtsam/base/timing.h>
 
 namespace gtsam {
 
@@ -162,7 +163,7 @@ protected:
 /* ************************************************************************* */
 template<class FG>
 void VariableIndex::fill(const FG& factorGraph) {
-
+  gttic(VariableIndex_fill);
   // Build index mapping from variable id to factor index
   for(size_t fi=0; fi<factorGraph.size(); ++fi) {
     if(factorGraph[fi]) {
@@ -180,11 +181,13 @@ void VariableIndex::fill(const FG& factorGraph) {
 /* ************************************************************************* */
 template<class FG>
 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<class FG>
 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<class FG>
 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<typename CONTAINER, class FG>
 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_

gtsam/nonlinear/Marginals.cpp

@@ -17,6 +17,7 @@
  */
 
 #include <gtsam/3rdparty/Eigen/Eigen/Dense>
+#include <gtsam/base/timing.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
 #include <gtsam/linear/GaussianMultifrontalSolver.h>
 #include <gtsam/nonlinear/Marginals.h>
@@ -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();
 }