From 15850333b48ce2c1e05ea192ae29201200fb08dc Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 3 Jan 2022 17:28:10 -0500
Subject: [PATCH 1/6] Straight-up depth-first fold method

# Conflicts:
#	gtsam/discrete/tests/testDecisionTree.cpp
---
 gtsam/discrete/DecisionTree-inl.h         | 42 +++++++++++++++++
 gtsam/discrete/DecisionTree.h             | 16 +++++++
 gtsam/discrete/tests/testDecisionTree.cpp | 55 +++++++++++++++++++----
 3 files changed, 105 insertions(+), 8 deletions(-)

diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index 11ecbf183..5e17fa7d6 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -630,6 +630,48 @@ namespace gtsam {
     return LY::compose(functions.begin(), functions.end(), newLabel);
   }
 
+  /*********************************************************************************/
+  template <typename L, typename Y, typename X>
+  struct Fold {
+    std::function<X(const Y&, X)> f;
+
+    /// Construct from folding function
+    Fold(std::function<X(const Y&, X)> f) : f(f) {}
+
+    using NodePtr = typename DecisionTree<L, Y>::NodePtr;
+    using Choice = typename DecisionTree<L, Y>::Choice;
+    using Leaf = typename DecisionTree<L, Y>::Leaf;
+
+    /**
+     * @brief Do a depth-first fold on the tree rooted at node.
+     *
+     * @param node root of a (sub-) tree, or a leaf.
+     * @param x0 Initial accumulator value.
+     * @return X Final accumulator value.
+     */
+    X fold(const NodePtr& node, X x0) const {
+      if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node)) {
+        return f(leaf->constant(), x0);
+      } else if (auto choice =
+                     boost::dynamic_pointer_cast<const Choice>(node)) {
+        for (auto&& branch : choice->branches()) x0 = fold(branch, x0);
+        return x0;
+      } else {
+        throw std::invalid_argument("Fold: Invalid NodePtr");
+      }
+    }
+
+    // alias for fold:
+    X operator()(const NodePtr& node, X x0) const { return fold(node, x0); }
+  };
+
+  template <typename L, typename Y>
+  template <typename Func, typename X>
+  X DecisionTree<L, Y>::fold(Func f, X x0) const {
+    Fold<L, Y, X> fold(f);
+    return fold(root_, x0);
+  }
+
   /*********************************************************************************/
   template <typename L, typename Y>
   bool DecisionTree<L, Y>::equals(const DecisionTree& other,
diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h
index db8a12a20..4148c6c20 100644
--- a/gtsam/discrete/DecisionTree.h
+++ b/gtsam/discrete/DecisionTree.h
@@ -229,6 +229,22 @@ namespace gtsam {
     /** evaluate */
     const Y& operator()(const Assignment<L>& x) const;
 
+    /**
+     * @brief Fold a binary function over the tree, returning accumulator.
+     *
+     * @tparam X type for accumulator.
+     * @param f binary function: Y * X -> X returning an updated accumulator.
+     * @param x0 initial value for accumulator.
+     * @return X final value for accumulator.
+     * 
+     * @note X is always passed by value.
+     */
+    template <typename Func, typename X>
+    X fold(Func f, X x0) const;
+
+    /** Retrieve all labels. */
+    std::vector<L> labels() const { return std::vector<L>(); }
+
     /** apply Unary operation "op" to f */
     DecisionTree apply(const Unary& op) const;
 
diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp
index 5976ea2d4..64f46b129 100644
--- a/gtsam/discrete/tests/testDecisionTree.cpp
+++ b/gtsam/discrete/tests/testDecisionTree.cpp
@@ -123,8 +123,7 @@ struct Ring {
 
 /* ******************************************************************************** */
 // test DT
-TEST(DT, example)
-{
+TEST(DecisionTree, example) {
   // Create labels
   string A("A"), B("B"), C("C");
 
@@ -236,8 +235,7 @@ std::function<bool(const int&)> bool_of_int = [](const int& y) {
 };
 typedef DecisionTree<string, bool> StringBoolTree;
 
-TEST(DT, ConvertValuesOnly)
-{
+TEST(DecisionTree, ConvertValuesOnly) {
   // Create labels
   string A("A"), B("B");
 
@@ -260,8 +258,7 @@ enum Label {
 };
 typedef DecisionTree<Label, bool> LabelBoolTree;
 
-TEST(DT, ConvertBoth)
-{
+TEST(DecisionTree, ConvertBoth) {
   // Create labels
   string A("A"), B("B");
 
@@ -288,8 +285,7 @@ TEST(DT, ConvertBoth)
 
 /* ******************************************************************************** */
 // test Compose expansion
-TEST(DT, Compose)
-{
+TEST(DecisionTree, Compose) {
   // Create labels
   string A("A"), B("B"), C("C");
 
@@ -314,6 +310,49 @@ TEST(DT, Compose)
   DOT(f5);
 }
 
+/* ******************************************************************************** */
+// Check we can create a decision tree of containers.
+TEST(DecisionTree, Containers) {
+  using Container = std::vector<double>;
+  using StringContainerTree = DecisionTree<string, Container>;
+
+  // Check default constructor
+  StringContainerTree tree;
+
+  // Create small two-level tree
+  string A("A"), B("B"), C("C");
+  DT stringIntTree(B, DT(A, 0, 1), DT(A, 2, 3));
+
+  // Check conversion
+  std::function<Container(const int& i)> container_of_int = [](const int& i) {
+    Container c;
+    c.emplace_back(i);
+    return c;
+  };
+  StringContainerTree converted(stringIntTree, container_of_int);
+}
+
+/* ******************************************************************************** */
+// Test fold.
+TEST(DecisionTree, fold) {
+  // Create small two-level tree
+  string A("A"), B("B"), C("C");
+  DT tree(B, DT(A, 0, 1), DT(A, 2, 3));
+  auto add = [](const int& y, double x) { return y + x; };
+  double sum = tree.fold(add, 0.0);
+  EXPECT_DOUBLES_EQUAL(6.0, sum, 1e-9);
+}
+
+/* ******************************************************************************** */
+// Test retrieving all labels.
+TEST_DISABLED(DecisionTree, labels) {
+  // Create small two-level tree
+  string A("A"), B("B"), C("C");
+  DT tree(B, DT(A, 0, 1), DT(A, 2, 3));
+  auto labels = tree.labels();
+  EXPECT_LONGS_EQUAL(2, labels.size());
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From 5de3dc42bd3b52421f1165247243e1ee53b0771c Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 3 Jan 2022 22:43:45 -0500
Subject: [PATCH 2/6] visit and visitWith

---
 gtsam/discrete/DecisionTree-inl.h         | 104 +++++++++++++++-------
 gtsam/discrete/DecisionTree.h             |  35 +++++++-
 gtsam/discrete/tests/testDecisionTree.cpp |  26 +++++-
 3 files changed, 129 insertions(+), 36 deletions(-)

diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index 5e17fa7d6..a1ba0e4c1 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -631,48 +631,86 @@ namespace gtsam {
   }
 
   /*********************************************************************************/
-  template <typename L, typename Y, typename X>
-  struct Fold {
-    std::function<X(const Y&, X)> f;
+  // Functor performing depth-first visit without Assignment<L> argument.
+  template <typename L, typename Y>
+  struct Visit {
+    using F = std::function<void(const Y&)>;
+    Visit(F f) : f(f) {}  ///< Construct from folding function.
+    F f;                  ///< folding function object.
 
-    /// Construct from folding function
-    Fold(std::function<X(const Y&, X)> f) : f(f) {}
+    /// Do a depth-first visit on the tree rooted at node.
+    void operator()(const typename DecisionTree<L, Y>::NodePtr& node) const {
+      using Leaf = typename DecisionTree<L, Y>::Leaf;
+      if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node))
+        return f(leaf->constant());
 
-    using NodePtr = typename DecisionTree<L, Y>::NodePtr;
-    using Choice = typename DecisionTree<L, Y>::Choice;
-    using Leaf = typename DecisionTree<L, Y>::Leaf;
-
-    /**
-     * @brief Do a depth-first fold on the tree rooted at node.
-     *
-     * @param node root of a (sub-) tree, or a leaf.
-     * @param x0 Initial accumulator value.
-     * @return X Final accumulator value.
-     */
-    X fold(const NodePtr& node, X x0) const {
-      if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node)) {
-        return f(leaf->constant(), x0);
-      } else if (auto choice =
-                     boost::dynamic_pointer_cast<const Choice>(node)) {
-        for (auto&& branch : choice->branches()) x0 = fold(branch, x0);
-        return x0;
-      } else {
-        throw std::invalid_argument("Fold: Invalid NodePtr");
-      }
+      using Choice = typename DecisionTree<L, Y>::Choice;
+      auto choice = boost::dynamic_pointer_cast<const Choice>(node);
+      for (auto&& branch : choice->branches()) (*this)(branch);  // recurse!
     }
-
-    // alias for fold:
-    X operator()(const NodePtr& node, X x0) const { return fold(node, x0); }
   };
 
   template <typename L, typename Y>
-  template <typename Func, typename X>
-  X DecisionTree<L, Y>::fold(Func f, X x0) const {
-    Fold<L, Y, X> fold(f);
-    return fold(root_, x0);
+  template <typename Func>
+  void DecisionTree<L, Y>::visit(Func f) const {
+    Visit<L, Y> visit(f);
+    visit(root_);
   }
 
   /*********************************************************************************/
+  // Functor performing depth-first visit with Assignment<L> argument.
+  template <typename L, typename Y>
+  struct VisitWith {
+    using Choices = Assignment<L>;
+    using F = std::function<void(const Choices&, const Y&)>;
+    VisitWith(F f) : f(f) {}  ///< Construct from folding function.
+    Choices choices;          ///< Assignment, mutating through recursion.
+    F f;                      ///< folding function object.
+
+    /// Do a depth-first visit on the tree rooted at node.
+    void operator()(const typename DecisionTree<L, Y>::NodePtr& node) {
+      using Leaf = typename DecisionTree<L, Y>::Leaf;
+      if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node))
+        return f(choices, leaf->constant());
+
+      using Choice = typename DecisionTree<L, Y>::Choice;
+      auto choice = boost::dynamic_pointer_cast<const Choice>(node);
+      for (size_t i = 0; i < choice->nrChoices(); i++) {
+        choices[choice->label()] = i;    // Set assignment for label to i
+        (*this)(choice->branches()[i]);  // recurse!
+      }
+    }
+  };
+
+  template <typename L, typename Y>
+  template <typename Func>
+  void DecisionTree<L, Y>::visitWith(Func f) const {
+    VisitWith<L, Y> visit(f);
+    visit(root_);
+  }
+
+  /*********************************************************************************/
+  // fold is just done with a visit
+  template <typename L, typename Y>
+  template <typename Func, typename X>
+  X DecisionTree<L, Y>::fold(Func f, X x0) const {
+    visit([&](const Y& y) { x0 = f(y, x0); });
+    return x0;
+  }
+
+  /*********************************************************************************/
+  // labels is just done with a visit
+  template <typename L, typename Y>
+  std::set<L> DecisionTree<L, Y>::labels() const {
+    std::set<L> unique;
+    auto f = [&](const Assignment<L>& choices, const Y&) {
+      for (auto&& kv : choices) unique.insert(kv.first);
+    };
+    visitWith(f);
+    return unique;
+  }
+
+/*********************************************************************************/
   template <typename L, typename Y>
   bool DecisionTree<L, Y>::equals(const DecisionTree& other,
                                   const CompareFunc& compare) const {
diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h
index 4148c6c20..fbcb59665 100644
--- a/gtsam/discrete/DecisionTree.h
+++ b/gtsam/discrete/DecisionTree.h
@@ -28,6 +28,7 @@
 #include <map>
 #include <sstream>
 #include <vector>
+#include <set>
 
 namespace gtsam {
 
@@ -229,6 +230,32 @@ namespace gtsam {
     /** evaluate */
     const Y& operator()(const Assignment<L>& x) const;
 
+    /**
+     * @brief Visit all leaves in depth-first fashion.
+     * 
+     * @param f side-effect taking a value.
+     * 
+     * Example:
+     *   int sum = 0;
+     *   auto visitor = [&](int y) { sum += y; };
+     *   tree.visitWith(visitor);
+     */
+    template <typename Func>
+    void visit(Func f) const;
+
+    /**
+     * @brief Visit all leaves in depth-first fashion.
+     * 
+     * @param f side-effect taking an assignment and a value.
+     * 
+     * Example:
+     *   int sum = 0;
+     *   auto visitor = [&](const Assignment<L>& choices, int y) { sum += y; };
+     *   tree.visitWith(visitor);
+     */
+    template <typename Func>
+    void visitWith(Func f) const;
+
     /**
      * @brief Fold a binary function over the tree, returning accumulator.
      *
@@ -238,12 +265,16 @@ namespace gtsam {
      * @return X final value for accumulator.
      * 
      * @note X is always passed by value.
+     * 
+     * Example:
+     *   auto add = [](const double& y, double x) { return y + x; };
+     *   double sum = tree.fold(add, 0.0);
      */
     template <typename Func, typename X>
     X fold(Func f, X x0) const;
 
-    /** Retrieve all labels. */
-    std::vector<L> labels() const { return std::vector<L>(); }
+    /** Retrieve all unique labels as a set. */
+    std::set<L> labels() const;
 
     /** apply Unary operation "op" to f */
     DecisionTree apply(const Unary& op) const;
diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp
index 64f46b129..84b9ca5fa 100644
--- a/gtsam/discrete/tests/testDecisionTree.cpp
+++ b/gtsam/discrete/tests/testDecisionTree.cpp
@@ -332,6 +332,30 @@ TEST(DecisionTree, Containers) {
   StringContainerTree converted(stringIntTree, container_of_int);
 }
 
+/* ******************************************************************************** */
+// Test visit.
+TEST(DecisionTree, visit) {
+  // Create small two-level tree
+  string A("A"), B("B"), C("C");
+  DT tree(B, DT(A, 0, 1), DT(A, 2, 3));
+  double sum = 0.0;
+  auto visitor = [&](int y) { sum += y; };
+  tree.visit(visitor);
+  EXPECT_DOUBLES_EQUAL(6.0, sum, 1e-9);
+}
+
+/* ******************************************************************************** */
+// Test visit, with Choices argument.
+TEST(DecisionTree, visitWith) {
+  // Create small two-level tree
+  string A("A"), B("B"), C("C");
+  DT tree(B, DT(A, 0, 1), DT(A, 2, 3));
+  double sum = 0.0;
+  auto visitor = [&](const Assignment<string>& choices, int y) { sum += y; };
+  tree.visitWith(visitor);
+  EXPECT_DOUBLES_EQUAL(6.0, sum, 1e-9);
+}
+
 /* ******************************************************************************** */
 // Test fold.
 TEST(DecisionTree, fold) {
@@ -345,7 +369,7 @@ TEST(DecisionTree, fold) {
 
 /* ******************************************************************************** */
 // Test retrieving all labels.
-TEST_DISABLED(DecisionTree, labels) {
+TEST(DecisionTree, labels) {
   // Create small two-level tree
   string A("A"), B("B"), C("C");
   DT tree(B, DT(A, 0, 1), DT(A, 2, 3));

From b453152f3eeff2425755073a9fb9803d73bffc1c Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 3 Jan 2022 23:44:51 -0500
Subject: [PATCH 3/6] Use template parameter for functions, enables auto

# Conflicts:
#	gtsam/discrete/DecisionTree-inl.h
#	gtsam/discrete/DecisionTree.h
---
 gtsam/discrete/DecisionTree-inl.h         | 35 ++++++++---------------
 gtsam/discrete/DecisionTree.h             | 11 ++++---
 gtsam/discrete/tests/testDecisionTree.cpp |  8 ++----
 3 files changed, 20 insertions(+), 34 deletions(-)

diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index a1ba0e4c1..a0df966a0 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -82,13 +82,7 @@ namespace gtsam {
       return compare(this->constant_, other->constant_);
     }
 
-    /**
-     * @brief Print method.
-     * 
-     * @param s Prefix string.
-     * @param labelFormatter Functor to format the labels of type L.
-     * @param valueFormatter Functor to format the values of type Y.
-     */
+    /** print */
     void print(const std::string& s, const LabelFormatter& labelFormatter,
                const ValueFormatter& valueFormatter) const override {
       std::cout << s << " Leaf " << valueFormatter(constant_) << std::endl;
@@ -465,23 +459,20 @@ namespace gtsam {
 
   /*********************************************************************************/
   template <typename L, typename Y>
-  template <typename X>
+  template <typename X, typename Func>
   DecisionTree<L, Y>::DecisionTree(const DecisionTree<L, X>& other,
-                                   std::function<Y(const X&)> Y_of_X) {
+                                   Func Y_of_X) {
     // Define functor for identity mapping of node label.
-    auto L_of_L = [](const L& label) { return label; };
+	auto L_of_L = [](const L& label) { return label; };
     root_ = convertFrom<L, X>(other.root_, L_of_L, Y_of_X);
   }
 
   /*********************************************************************************/
   template <typename L, typename Y>
-  template <typename M, typename X>
+  template <typename M, typename X, typename Func>
   DecisionTree<L, Y>::DecisionTree(const DecisionTree<M, X>& other,
-                                   const std::map<M, L>& map,
-                                   std::function<Y(const X&)> Y_of_X) {
-    std::function<L(const M&)> L_of_M = [&map](const M& label) -> L {
-      return map.at(label);
-    };
+                                   const std::map<M, L>& map, Func Y_of_X) {
+    auto L_of_M = [&map](const M& label) -> L { return map.at(label); };
     root_ = convertFrom<M, X>(other.root_, L_of_M, Y_of_X);
   }
 
@@ -601,18 +592,16 @@ namespace gtsam {
       const typename DecisionTree<M, X>::NodePtr& f,
       std::function<L(const M&)> L_of_M,
       std::function<Y(const X&)> Y_of_X) const {
-    using MX = DecisionTree<M, X>;
-    using MXLeaf = typename MX::Leaf;
-    using MXChoice = typename MX::Choice;
-    using MXNodePtr = typename MX::NodePtr;
     using LY = DecisionTree<L, Y>;
 
     // ugliness below because apparently we can't have templated virtual functions
     // If leaf, apply unary conversion "op" and create a unique leaf
-    auto leaf = boost::dynamic_pointer_cast<const MXLeaf>(f);
-    if (leaf) return NodePtr(new Leaf(Y_of_X(leaf->constant())));
+    using MXLeaf = typename DecisionTree<M, X>::Leaf;
+    if (auto leaf = boost::dynamic_pointer_cast<const MXLeaf>(f))
+    	return NodePtr(new Leaf(Y_of_X(leaf->constant())));
 
     // Check if Choice
+    using MXChoice = typename DecisionTree<M, X>::Choice;
     auto choice = boost::dynamic_pointer_cast<const MXChoice>(f);
     if (!choice) throw std::invalid_argument(
         "DecisionTree::Convert: Invalid NodePtr");
@@ -623,7 +612,7 @@ namespace gtsam {
 
     // put together via Shannon expansion otherwise not sorted.
     std::vector<LY> functions;
-    for(const MXNodePtr& branch: choice->branches()) {
+    for(auto && branch: choice->branches()) {
       LY converted(convertFrom<M, X>(branch, L_of_M, Y_of_X));
       functions += converted;
     }
diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h
index fbcb59665..9692094e1 100644
--- a/gtsam/discrete/DecisionTree.h
+++ b/gtsam/discrete/DecisionTree.h
@@ -177,9 +177,8 @@ namespace gtsam {
      * @param other The DecisionTree to convert from.
      * @param Y_of_X Functor to convert from value type X to type Y.
      */
-    template <typename X>
-    DecisionTree(const DecisionTree<L, X>& other,
-                 std::function<Y(const X&)> Y_of_X);
+    template <typename X, typename Func>
+    DecisionTree(const DecisionTree<L, X>& other, Func Y_of_X);
 
     /**
      * @brief Convert from a different value type X to value type Y, also transate
@@ -191,9 +190,9 @@ namespace gtsam {
      * @param L_of_M Map from label type M to type L.
      * @param Y_of_X Functor to convert from type X to type Y.
      */
-    template <typename M, typename X>
-    DecisionTree(const DecisionTree<M, X>& other, const std::map<M, L>& L_of_M,
-                 std::function<Y(const X&)> Y_of_X);
+    template <typename M, typename X, typename Func>
+    DecisionTree(const DecisionTree<M, X>& other, const std::map<M, L>& map,
+                 Func Y_of_X);
 
     /// @}
     /// @name Testable
diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp
index 84b9ca5fa..2e6ec59f7 100644
--- a/gtsam/discrete/tests/testDecisionTree.cpp
+++ b/gtsam/discrete/tests/testDecisionTree.cpp
@@ -230,9 +230,7 @@ TEST(DecisionTree, example) {
 
 /* ******************************************************************************** */
 // test Conversion of values
-std::function<bool(const int&)> bool_of_int = [](const int& y) {
-  return y != 0;
-};
+bool bool_of_int(const int& y) { return y != 0; };
 typedef DecisionTree<string, bool> StringBoolTree;
 
 TEST(DecisionTree, ConvertValuesOnly) {
@@ -269,7 +267,7 @@ TEST(DecisionTree, ConvertBoth) {
   map<string, Label> ordering;
   ordering[A] = X;
   ordering[B] = Y;
-  LabelBoolTree f2(f1, ordering, bool_of_int);
+  LabelBoolTree f2(f1, ordering, &bool_of_int);
 
   // Check some values
   Assignment<Label> x00, x01, x10, x11;
@@ -324,7 +322,7 @@ TEST(DecisionTree, Containers) {
   DT stringIntTree(B, DT(A, 0, 1), DT(A, 2, 3));
 
   // Check conversion
-  std::function<Container(const int& i)> container_of_int = [](const int& i) {
+  auto container_of_int = [](const int& i) {
     Container c;
     c.emplace_back(i);
     return c;

From 517fe3963e4098c3b654e21ea0519af50b871c32 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 4 Jan 2022 10:40:56 -0500
Subject: [PATCH 4/6] Fix compile issues on Ubuntu/windows

---
 gtsam/discrete/DecisionTree-inl.h | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index a0df966a0..633cc1670 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -580,7 +580,7 @@ namespace gtsam {
     size_t split = size / nrChoices;
     for (size_t i = 0; i < nrChoices; i++, beginY += split) {
       NodePtr f = create<It, ValueIt>(labelC, end, beginY, beginY + split);
-      functions += DecisionTree(f);
+      functions.emplace_back(f);
     }
     return compose(functions.begin(), functions.end(), begin->first);
   }
@@ -613,8 +613,7 @@ namespace gtsam {
     // put together via Shannon expansion otherwise not sorted.
     std::vector<LY> functions;
     for(auto && branch: choice->branches()) {
-      LY converted(convertFrom<M, X>(branch, L_of_M, Y_of_X));
-      functions += converted;
+      functions.emplace_back(convertFrom<M, X>(branch, L_of_M, Y_of_X));
     }
     return LY::compose(functions.begin(), functions.end(), newLabel);
   }

From 40e3b0ff0d88c6cc9c9301a8d2858ac596128192 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 4 Jan 2022 10:41:16 -0500
Subject: [PATCH 5/6] Use make-shared where possible

---
 gtsam/discrete/DecisionTree-inl.h | 45 ++++++++++++++++---------------
 1 file changed, 23 insertions(+), 22 deletions(-)

diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index 633cc1670..3c74e57fd 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -28,6 +28,7 @@
 #include <boost/tuple/tuple.hpp>
 #include <boost/type_traits/has_dereference.hpp>
 #include <boost/unordered_set.hpp>
+#include <boost/make_shared.hpp>
 #include <cmath>
 #include <fstream>
 #include <list>
@@ -326,7 +327,7 @@ namespace gtsam {
 
     /** apply unary operator */
     NodePtr apply(const Unary& op) const override {
-      boost::shared_ptr<Choice> r(new Choice(label_, *this, op));
+      auto r = boost::make_shared<Choice>(label_, *this, op);
       return Unique(r);
     }
 
@@ -341,24 +342,24 @@ namespace gtsam {
 
     // If second argument of binary op is Leaf node, recurse on branches
     NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const override {
-      boost::shared_ptr<Choice> h(new Choice(label(), nrChoices()));
-      for(NodePtr branch: branches_)
-              h->push_back(fL.apply_f_op_g(*branch, op));
+      auto h = boost::make_shared<Choice>(label(), nrChoices());
+      for (auto&& branch : branches_)
+        h->push_back(fL.apply_f_op_g(*branch, op));
       return Unique(h);
     }
 
     // If second argument of binary op is Choice, call constructor
     NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const override {
-      boost::shared_ptr<Choice> h(new Choice(fC, *this, op));
+      auto h = boost::make_shared<Choice>(fC, *this, op);
       return Unique(h);
     }
 
     // If second argument of binary op is Leaf
     template<typename OP>
     NodePtr apply_fC_op_gL(const Leaf& gL, OP op) const {
-      boost::shared_ptr<Choice> h(new Choice(label(), nrChoices()));
-      for(const NodePtr& branch: branches_)
-              h->push_back(branch->apply_f_op_g(gL, op));
+      auto h = boost::make_shared<Choice>(label(), nrChoices());
+      for (auto&& branch : branches_)
+        h->push_back(branch->apply_f_op_g(gL, op));
       return Unique(h);
     }
 
@@ -368,9 +369,9 @@ namespace gtsam {
         return branches_[index]; // choose branch
 
       // second case, not label of interest, just recurse
-      boost::shared_ptr<Choice> r(new Choice(label_, branches_.size()));
-      for(const NodePtr& branch: branches_)
-              r->push_back(branch->choose(label, index));
+      auto r = boost::make_shared<Choice>(label_, branches_.size());
+      for (auto&& branch : branches_)
+        r->push_back(branch->choose(label, index));
       return Unique(r);
     }
 
@@ -395,10 +396,9 @@ namespace gtsam {
   }
 
   /*********************************************************************************/
-  template<typename L, typename Y>
-  DecisionTree<L, Y>::DecisionTree(//
-      const L& label, const Y& y1, const Y& y2)  {
-    boost::shared_ptr<Choice> a(new Choice(label, 2));
+  template <typename L, typename Y>
+  DecisionTree<L, Y>::DecisionTree(const L& label, const Y& y1, const Y& y2) {
+    auto a = boost::make_shared<Choice>(label, 2);
     NodePtr l1(new Leaf(y1)), l2(new Leaf(y2));
     a->push_back(l1);
     a->push_back(l2);
@@ -406,12 +406,12 @@ namespace gtsam {
   }
 
   /*********************************************************************************/
-  template<typename L, typename Y>
-  DecisionTree<L, Y>::DecisionTree(//
-      const LabelC& labelC, const Y& y1, const Y& y2)  {
+  template <typename L, typename Y>
+  DecisionTree<L, Y>::DecisionTree(const LabelC& labelC, const Y& y1,
+                                   const Y& y2) {
     if (labelC.second != 2) throw std::invalid_argument(
         "DecisionTree: binary constructor called with non-binary label");
-    boost::shared_ptr<Choice> a(new Choice(labelC.first, 2));
+    auto a = boost::make_shared<Choice>(labelC.first, 2);
     NodePtr l1(new Leaf(y1)), l2(new Leaf(y2));
     a->push_back(l1);
     a->push_back(l2);
@@ -502,13 +502,14 @@ namespace gtsam {
 
     // if label is already in correct order, just put together a choice on label
     if (!nrChoices || !highestLabel || label > *highestLabel) {
-      boost::shared_ptr<Choice> choiceOnLabel(new Choice(label, end - begin));
+      auto choiceOnLabel = boost::make_shared<Choice>(label, end - begin);
       for (Iterator it = begin; it != end; it++)
         choiceOnLabel->push_back(it->root_);
       return Choice::Unique(choiceOnLabel);
     } else {
       // Set up a new choice on the highest label
-      boost::shared_ptr<Choice> choiceOnHighestLabel(new Choice(*highestLabel, nrChoices));
+      auto choiceOnHighestLabel =
+          boost::make_shared<Choice>(*highestLabel, nrChoices);
       // now, for all possible values of highestLabel
       for (size_t index = 0; index < nrChoices; index++) {
         // make a new set of functions for composing by iterating over the given
@@ -567,7 +568,7 @@ namespace gtsam {
         std::cout << boost::format("DecisionTree::create: expected %d values but got %d instead") % nrChoices % size << std::endl;
         throw std::invalid_argument("DecisionTree::create invalid argument");
       }
-      boost::shared_ptr<Choice> choice(new Choice(begin->first, endY - beginY));
+      auto choice = boost::make_shared<Choice>(begin->first, endY - beginY);
       for (ValueIt y = beginY; y != endY; y++)
         choice->push_back(NodePtr(new Leaf(*y)));
       return Choice::Unique(choice);

From 7f87a4c72267e2ae2b263112c2f2dca01f40ca57 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 4 Jan 2022 10:44:31 -0500
Subject: [PATCH 6/6] Avoid internal warnings with
 GTSAM_ALLOW_DEPRECATED_SINCE_V42 ON

---
 gtsam/navigation/ImuBias.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h
index 7557c4732..9346a4a77 100644
--- a/gtsam/navigation/ImuBias.h
+++ b/gtsam/navigation/ImuBias.h
@@ -142,10 +142,10 @@ public:
     return q - (*this);
   }
   Vector6 GTSAM_DEPRECATED localCoordinates(const ConstantBias& q) {
-    return between(q).vector();
+    return (q - (*this)).vector();
   }
   ConstantBias GTSAM_DEPRECATED retract(const Vector6& v) {
-    return compose(ConstantBias(v));
+    return (*this) + ConstantBias(v);
   }
   static Vector6 GTSAM_DEPRECATED Logmap(const ConstantBias& p) {
     return p.vector();