Merge pull request #1948 from borglab/hybrid-timing

cmake/GtsamBuildTypes.cmake

@@ -168,6 +168,12 @@ foreach(build_type "common" ${GTSAM_CMAKE_CONFIGURATION_TYPES})
   append_config_if_not_empty(GTSAM_COMPILE_DEFINITIONS_PUBLIC   ${build_type})
 endforeach()
 
+# Check if timing is enabled and add appropriate definition flag
+if(GTSAM_ENABLE_TIMING AND(NOT ${CMAKE_BUILD_TYPE} EQUAL "Timing"))
+  message(STATUS "Enabling timing for non-timing build")
+  list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE "ENABLE_TIMING")
+endif()
+
 # Linker flags:
 set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING  "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
 set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING  "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")

cmake/HandleGeneralOptions.cmake

@@ -33,6 +33,8 @@ option(GTSAM_USE_QUATERNIONS                "Enable/Disable using an internal Qu
 option(GTSAM_POSE3_EXPMAP                   "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
 option(GTSAM_ROT3_EXPMAP                    "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
 option(GTSAM_DT_MERGING                     "Enable/Disable merging of equal leaf nodes in DecisionTrees. This leads to significant speed up and memory savings." ON)
+option(GTSAM_ENABLE_TIMING                  "Enable the timing tools (gttic/gttoc)" OFF)
+option(GTSAM_HYBRID_TIMING                  "Enable the timing of hybrid factor graph machinery" OFF)
 option(GTSAM_ENABLE_CONSISTENCY_CHECKS      "Enable/Disable expensive consistency checks" OFF)
 option(GTSAM_ENABLE_MEMORY_SANITIZER        "Enable/Disable memory sanitizer" OFF)
 option(GTSAM_WITH_TBB                       "Use Intel Threaded Building Blocks (TBB) if available" ON)

cmake/HandlePrintConfiguration.cmake

@@ -91,6 +91,7 @@ print_enabled_config(${GTSAM_ENABLE_MEMORY_SANITIZER}     "Build with Memory San
 print_enabled_config(${GTSAM_ROT3_EXPMAP}                 "Rot3 retract is full ExpMap     ")
 print_enabled_config(${GTSAM_POSE3_EXPMAP}                "Pose3 retract is full ExpMap    ")
 print_enabled_config(${GTSAM_DT_MERGING}                  "Enable branch merging in DecisionTree")
+print_enabled_config(${GTSAM_ENABLE_TIMING}               "Enable timing machinery")
 print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V43}  "Allow features deprecated in GTSAM 4.3")
 print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION}   "Metis-based Nested Dissection   ")
 print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION}      "Use tangent-space preintegration")

gtsam/base/timing.cpp

@@ -31,7 +31,9 @@
 
 namespace gtsam {
 namespace internal {
-
+
+using ChildOrder = FastMap<size_t, std::shared_ptr<TimingOutline>>;
+
 // a static shared_ptr to TimingOutline with nullptr as the pointer
 const static std::shared_ptr<TimingOutline> nullTimingOutline;
 
@@ -91,7 +93,6 @@ void TimingOutline::print(const std::string& outline) const {
       << n_ << " times, " << wall() << " wall, " << secs() << " children, min: "
       << min() << " max: " << max() << ")\n";
   // Order children
-  typedef FastMap<size_t, std::shared_ptr<TimingOutline> > ChildOrder;
   ChildOrder childOrder;
   for(const ChildMap::value_type& child: children_) {
     childOrder[child.second->myOrder_] = child.second;
@@ -106,6 +107,54 @@ void TimingOutline::print(const std::string& outline) const {
 #endif
 }
 
+/* ************************************************************************* */
+void TimingOutline::printCsvHeader(bool addLineBreak) const {
+#ifdef GTSAM_USE_BOOST_FEATURES
+  // Order is (CPU time, number of times, wall time, time + children in seconds,
+  // min time, max time)
+  std::cout << label_ + " cpu time (s)" << "," << label_ + " #calls" << ","
+            << label_ + " wall time(s)" << "," << label_ + " subtree time (s)"
+            << "," << label_ + " min time (s)" << "," << label_ + "max time(s)"
+            << ",";
+  // Order children
+  ChildOrder childOrder;
+  for (const ChildMap::value_type& child : children_) {
+    childOrder[child.second->myOrder_] = child.second;
+  }
+  // Print children
+  for (const ChildOrder::value_type& order_child : childOrder) {
+    order_child.second->printCsvHeader();
+  }
+  if (addLineBreak) {
+    std::cout << std::endl;
+  }
+  std::cout.flush();
+#endif
+}
+
+/* ************************************************************************* */
+void TimingOutline::printCsv(bool addLineBreak) const {
+#ifdef GTSAM_USE_BOOST_FEATURES
+  // Order is (CPU time, number of times, wall time, time + children in seconds,
+  // min time, max time)
+  std::cout << self() << "," << n_ << "," << wall() << "," << secs() << ","
+            << min() << "," << max() << ",";
+  // Order children
+  ChildOrder childOrder;
+  for (const ChildMap::value_type& child : children_) {
+    childOrder[child.second->myOrder_] = child.second;
+  }
+  // Print children
+  for (const ChildOrder::value_type& order_child : childOrder) {
+    order_child.second->printCsv(false);
+  }
+  if (addLineBreak) {
+    std::cout << std::endl;
+  }
+  std::cout.flush();
+#endif
+}
+
 void TimingOutline::print2(const std::string& outline,
     const double parentTotal) const {
 #if GTSAM_USE_BOOST_FEATURES

gtsam/base/timing.h

@@ -199,6 +199,29 @@ namespace gtsam {
 #endif
       GTSAM_EXPORT void print(const std::string& outline = "") const;
       GTSAM_EXPORT void print2(const std::string& outline = "", const double parentTotal = -1.0) const;
+
+      /**
+       * @brief Print the CSV header.
+       * Order is
+       * (CPU time, number of times, wall time, time + children in seconds, min
+       * time, max time)
+       *
+       * @param addLineBreak Flag indicating if a line break should be added at
+       * the end. Only used at the top-leve.
+       */
+      GTSAM_EXPORT void printCsvHeader(bool addLineBreak = false) const;
+
+      /**
+       * @brief Print the times recursively from parent to child in CSV format.
+       * For each timing node, the output is
+       * (CPU time, number of times, wall time, time + children in seconds, min
+       * time, max time)
+       *
+       * @param addLineBreak Flag indicating if a line break should be added at
+       * the end. Only used at the top-leve.
+       */
+      GTSAM_EXPORT void printCsv(bool addLineBreak = false) const;
+
       GTSAM_EXPORT const std::shared_ptr<TimingOutline>&
         child(size_t child, const std::string& label, const std::weak_ptr<TimingOutline>& thisPtr);
       GTSAM_EXPORT void tic();
@@ -268,6 +291,14 @@ inline void tictoc_finishedIteration_() {
 inline void tictoc_print_() {
   ::gtsam::internal::gTimingRoot->print(); }
 
+// print timing in CSV format
+inline void tictoc_printCsv_(bool displayHeader = false) {
+  if (displayHeader) {
+    ::gtsam::internal::gTimingRoot->printCsvHeader(true);
+  }
+  ::gtsam::internal::gTimingRoot->printCsv(true);
+}
+
 // print mean and standard deviation
 inline void tictoc_print2_() {
   ::gtsam::internal::gTimingRoot->print2(); }

gtsam/config.h.in

@@ -42,6 +42,9 @@
 // Whether to enable merging of equal leaf nodes in the Discrete Decision Tree.
 #cmakedefine GTSAM_DT_MERGING
 
+// Whether to enable timing in hybrid factor graph machinery
+#cmakedefine01 GTSAM_HYBRID_TIMING
+
 // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake)
 #cmakedefine GTSAM_USE_TBB
 

gtsam/discrete/AlgebraicDecisionTree.h

@@ -57,6 +57,9 @@ namespace gtsam {
 
     AlgebraicDecisionTree(double leaf = 1.0) : Base(leaf) {}
 
+    /// Constructor which accepts root pointer
+    AlgebraicDecisionTree(const typename Base::NodePtr root) : Base(root) {}
+
     // Explicitly non-explicit constructor
     AlgebraicDecisionTree(const Base& add) : Base(add) {}
 

gtsam/discrete/DiscreteConditional.cpp

@@ -77,6 +77,13 @@ DiscreteConditional::DiscreteConditional(const Signature& signature)
 /* ************************************************************************** */
 DiscreteConditional DiscreteConditional::operator*(
     const DiscreteConditional& other) const {
+  // If the root is a nullptr, we have a TableDistribution
+  // TODO(Varun) Revisit this hack after RSS2025 submission
+  if (!other.root_) {
+    DiscreteConditional dc(other.nrFrontals(), other.toDecisionTreeFactor());
+    return dc * (*this);
+  }
+
   // Take union of frontal keys
   std::set<Key> newFrontals;
   for (auto&& key : this->frontals()) newFrontals.insert(key);
@@ -479,6 +486,19 @@ double DiscreteConditional::evaluate(const HybridValues& x) const {
   return this->operator()(x.discrete());
 }
 
+/* ************************************************************************* */
+DiscreteFactor::shared_ptr DiscreteConditional::max(
+    const Ordering& keys) const {
+  return BaseFactor::max(keys);
+}
+
+/* ************************************************************************* */
+void DiscreteConditional::prune(size_t maxNrAssignments) {
+  // Get as DiscreteConditional so the probabilities are normalized
+  DiscreteConditional pruned(nrFrontals(), BaseFactor::prune(maxNrAssignments));
+  this->root_ = pruned.root_;
+}
+
 /* ************************************************************************* */
 double DiscreteConditional::negLogConstant() const { return 0.0; }
 

gtsam/discrete/DiscreteConditional.h

@@ -199,7 +199,7 @@ class GTSAM_EXPORT DiscreteConditional
    * @param parentsValues Known values of the parents
    * @return sample from conditional
    */
-  size_t sample(const DiscreteValues& parentsValues) const;
+  virtual size_t sample(const DiscreteValues& parentsValues) const;
 
   /// Single parent version.
   size_t sample(size_t parent_value) const;
@@ -214,6 +214,15 @@ class GTSAM_EXPORT DiscreteConditional
    */
   size_t argmax(const DiscreteValues& parentsValues = DiscreteValues()) const;
 
+  /**
+   * @brief Create new factor by maximizing over all
+   * values with the same separator.
+   *
+   * @param keys The keys to sum over.
+   * @return DiscreteFactor::shared_ptr
+   */
+  virtual DiscreteFactor::shared_ptr max(const Ordering& keys) const override;
+
   /// @}
   /// @name Advanced Interface
   /// @{
@@ -267,6 +276,9 @@ class GTSAM_EXPORT DiscreteConditional
    */
   double negLogConstant() const override;
 
+  /// Prune the conditional
+  virtual void prune(size_t maxNrAssignments);
+
   /// @}
 
  protected:

gtsam/discrete/DiscreteFactorGraph.cpp

@@ -118,17 +118,11 @@ namespace gtsam {
 //      }
 //  }
 
-  /**
-   * @brief Multiply all the `factors`.
-   *
-   * @param factors The factors to multiply as a DiscreteFactorGraph.
-   * @return DiscreteFactor::shared_ptr
-   */
-  static DiscreteFactor::shared_ptr DiscreteProduct(
-      const DiscreteFactorGraph& factors) {
+  /* ************************************************************************ */
+  DiscreteFactor::shared_ptr DiscreteFactorGraph::scaledProduct() const {
     // PRODUCT: multiply all factors
     gttic(product);
-    DiscreteFactor::shared_ptr product = factors.product();
+    DiscreteFactor::shared_ptr product = this->product();
     gttoc(product);
 
     // Max over all the potentials by pretending all keys are frontal:
@@ -145,7 +139,7 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateForMPE(const DiscreteFactorGraph& factors,
                   const Ordering& frontalKeys) {
-    DiscreteFactor::shared_ptr product = DiscreteProduct(factors);
+    DiscreteFactor::shared_ptr product = factors.scaledProduct();
 
     // max out frontals, this is the factor on the separator
     gttic(max);
@@ -223,7 +217,7 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   EliminateDiscrete(const DiscreteFactorGraph& factors,
                     const Ordering& frontalKeys) {
-    DiscreteFactor::shared_ptr product = DiscreteProduct(factors);
+    DiscreteFactor::shared_ptr product = factors.scaledProduct();
 
     // sum out frontals, this is the factor on the separator
     gttic(sum);

gtsam/discrete/DiscreteFactorGraph.h

@@ -150,6 +150,15 @@ class GTSAM_EXPORT DiscreteFactorGraph
   /** return product of all factors as a single factor */
   DiscreteFactor::shared_ptr product() const;
 
+  /**
+   * @brief Return product of all `factors` as a single factor,
+   * which is scaled by the max value to prevent underflow
+   *
+   * @param factors The factors to multiply as a DiscreteFactorGraph.
+   * @return DiscreteFactor::shared_ptr
+   */
+  DiscreteFactor::shared_ptr scaledProduct() const;
+
   /** 
    * Evaluates the factor graph given values, returns the joint probability of
    * the factor graph given specific instantiation of values