From 05840bc7e39e0177c295f69256a44139d9ac81ca Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 15 Jun 2019 16:36:08 -0400
Subject: [PATCH 01/10] prototyping weighted sampler

---
 gtsam/base/tests/testWeightedSampler.cpp | 142 +++++++++++++++++++++++
 1 file changed, 142 insertions(+)
 create mode 100644 gtsam/base/tests/testWeightedSampler.cpp

diff --git a/gtsam/base/tests/testWeightedSampler.cpp b/gtsam/base/tests/testWeightedSampler.cpp
new file mode 100644
index 000000000..865c33f9f
--- /dev/null
+++ b/gtsam/base/tests/testWeightedSampler.cpp
@@ -0,0 +1,142 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testWeightedSampler.cpp
+ * @brief   Unit test for WeightedSampler
+ * @author  Frank Dellaert
+ * @date    MAy 2019
+ **/
+
+#include <vector>
+
+namespace gtsam {
+template <class Engine>
+std::vector<size_t> sampleWithoutReplacement(Engine& rng, size_t s,
+                                             std::vector<double> weights);
+}
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/random/mersenne_twister.hpp>
+
+using namespace std;
+using namespace gtsam;
+
+TEST(WeightedSampler, sampleWithoutReplacement) {
+  vector<double> weights{1, 2, 3, 4, 3, 2, 1};
+  boost::mt19937 rng(42);
+  auto samples = sampleWithoutReplacement(rng, 5, weights);
+  EXPECT_LONGS_EQUAL(5, samples.size());
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+
+#include <queue>
+#include <stdexcept>
+#include <utility>
+#include <vector>
+
+namespace gtsam {
+/* ************************************************************************* */
+/* Implementation adapted from paper at
+ * https://www.ethz.ch/content/dam/ethz/special-interest/baug/ivt/ivt-dam/vpl/reports/1101-1200/ab1141.pdf
+ */
+template <class Engine>
+vector<size_t> sampleWithoutReplacement(Engine& rng, size_t s,
+                                        vector<double> weights) {
+  const size_t n = weights.size();
+  if (n < s) {
+    throw runtime_error("s must be smaller than weights.size()");
+  }
+
+  // Return empty array if s==0
+  vector<size_t> result(s);
+  if (s == 0) return result;
+
+  // Step 1: The first m items of V are inserted into reservoir
+  // Step 2: For each item v_i ∈ reservoir: Calculate a key k_i = u_i^(1/w),
+  // where u_i = random(0, 1)
+  // (Modification: Calculate and store -log k_i = e_i / w where e_i = exp(1),
+  //  reservoir is a priority queue that pops the *maximum* elements)
+  priority_queue<pair<double, size_t> > reservoir;
+
+  static const double kexp1 = exp(1.0);
+  for (auto iprob = weights.begin(); iprob != weights.begin() + s; ++iprob) {
+    double k_i = kexp1 / *iprob;
+    reservoir.push(make_pair(k_i, iprob - weights.begin() + 1));
+  }
+
+  // Step 4: Repeat Steps 5–10 until the population is exhausted
+  {
+    // Step 3: The threshold T_w is the minimum key of reservoir
+    // (Modification: This is now the logarithm)
+    // Step 10: The new threshold T w is the new minimum key of reservoir
+    const pair<double, size_t>& T_w = reservoir.top();
+
+    // Incrementing iprob is part of Step 7
+    for (auto iprob = weights.begin() + s; iprob != weights.end(); ++iprob) {
+      // Step 5: Let r = random(0, 1) and X_w = log(r) / log(T_w)
+      // (Modification: Use e = -exp(1) instead of log(r))
+      double X_w = kexp1 / T_w.first;
+
+      // Step 6: From the current item v_c skip items until item v_i, such that:
+      double w = 0.0;
+
+      // Step 7: w_c + w_{c+1} + ··· + w_{i−1} < X_w <= w_c + w_{c+1} + ··· +
+      // w_{i−1} + w_i
+      for (; iprob != weights.end(); ++iprob) {
+        w += *iprob;
+        if (X_w <= w) break;
+      }
+
+      // Step 7: No such item, terminate
+      if (iprob == weights.end()) break;
+
+      // Step 9: Let t_w = T_w^{w_i}, r_2 = random(t_w, 1) and v_i’s key: k_i =
+      // (r_2)^{1/w_i} (Mod: Let t_w = log(T_w) * {w_i}, e_2 =
+      // log(random(e^{t_w}, 1)) and v_i’s key: k_i = -e_2 / w_i)
+      double t_w = -T_w.first * *iprob;
+      boost::uniform_real<double> randomAngle(exp(t_w), 1.0);
+      double e_2 = log(randomAngle(rng));
+      double k_i = -e_2 / *iprob;
+
+      // Step 8: The item in reservoir with the minimum key is replaced by item
+      // v_i
+      reservoir.pop();
+      reservoir.push(make_pair(k_i, iprob - weights.begin() + 1));
+    }
+  }
+
+  for (auto iret = result.end(); iret != result.begin();) {
+    --iret;
+
+    if (reservoir.empty()) {
+      throw runtime_error(
+          "Reservoir empty before all elements have been filled");
+    }
+
+    *iret = reservoir.top().second;
+    reservoir.pop();
+  }
+
+  if (!reservoir.empty()) {
+    throw runtime_error(
+        "Reservoir not empty after all elements have been filled");
+  }
+
+  return result;
+}
+}  // namespace gtsam

From 1f598269f5a61e2dd00ffd07748850083a0f08f9 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 15 Jun 2019 17:50:17 -0400
Subject: [PATCH 02/10] Moved WeightedSampler into its own header

---
 gtsam/base/WeightedSampler.h             | 116 +++++++++++++++++++++++
 gtsam/base/tests/testWeightedSampler.cpp | 109 +--------------------
 2 files changed, 119 insertions(+), 106 deletions(-)
 create mode 100644 gtsam/base/WeightedSampler.h

diff --git a/gtsam/base/WeightedSampler.h b/gtsam/base/WeightedSampler.h
new file mode 100644
index 000000000..f4495ed68
--- /dev/null
+++ b/gtsam/base/WeightedSampler.h
@@ -0,0 +1,116 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    WeightedSampler.h
+ * @brief   Fast sampling without replacement.
+ * @author  Frank Dellaert
+ * @date    May 2019
+ **/
+
+#include <queue>
+#include <random>
+#include <stdexcept>
+#include <utility>
+#include <vector>
+
+namespace gtsam {
+/*
+ * Fast sampling without replacement.
+ */
+template <class Engine>
+std::vector<size_t> sampleWithoutReplacement(Engine& rng, size_t s,
+                                             std::vector<double> weights) {
+  // Implementation adapted from paper at
+  // https://www.ethz.ch/content/dam/ethz/special-interest/baug/ivt/ivt-dam/vpl/reports/1101-1200/ab1141.pdf
+  const size_t n = weights.size();
+  if (n < s) {
+    throw std::runtime_error("s must be smaller than weights.size()");
+  }
+
+  // Return empty array if s==0
+  std::vector<size_t> result(s);
+  if (s == 0) return result;
+
+  // Step 1: The first m items of V are inserted into reservoir
+  // Step 2: For each item v_i ∈ reservoir: Calculate a key k_i = u_i^(1/w),
+  // where u_i = random(0, 1)
+  // (Modification: Calculate and store -log k_i = e_i / w where e_i = exp(1),
+  //  reservoir is a priority queue that pops the *maximum* elements)
+  std::priority_queue<std::pair<double, size_t> > reservoir;
+
+  static const double kexp1 = exp(1.0);
+  for (auto iprob = weights.begin(); iprob != weights.begin() + s; ++iprob) {
+    double k_i = kexp1 / *iprob;
+    reservoir.push(std::make_pair(k_i, iprob - weights.begin() + 1));
+  }
+
+  // Step 4: Repeat Steps 5–10 until the population is exhausted
+  {
+    // Step 3: The threshold T_w is the minimum key of reservoir
+    // (Modification: This is now the logarithm)
+    // Step 10: The new threshold T w is the new minimum key of reservoir
+    const std::pair<double, size_t>& T_w = reservoir.top();
+
+    // Incrementing iprob is part of Step 7
+    for (auto iprob = weights.begin() + s; iprob != weights.end(); ++iprob) {
+      // Step 5: Let r = random(0, 1) and X_w = log(r) / log(T_w)
+      // (Modification: Use e = -exp(1) instead of log(r))
+      double X_w = kexp1 / T_w.first;
+
+      // Step 6: From the current item v_c skip items until item v_i, such that:
+      double w = 0.0;
+
+      // Step 7: w_c + w_{c+1} + ··· + w_{i−1} < X_w <= w_c + w_{c+1} + ··· +
+      // w_{i−1} + w_i
+      for (; iprob != weights.end(); ++iprob) {
+        w += *iprob;
+        if (X_w <= w) break;
+      }
+
+      // Step 7: No such item, terminate
+      if (iprob == weights.end()) break;
+
+      // Step 9: Let t_w = T_w^{w_i}, r_2 = random(t_w, 1) and v_i’s key: k_i =
+      // (r_2)^{1/w_i} (Mod: Let t_w = log(T_w) * {w_i}, e_2 =
+      // log(random(e^{t_w}, 1)) and v_i’s key: k_i = -e_2 / w_i)
+      double t_w = -T_w.first * *iprob;
+      std::uniform_real_distribution<double> randomAngle(exp(t_w), 1.0);
+      double e_2 = log(randomAngle(rng));
+      double k_i = -e_2 / *iprob;
+
+      // Step 8: The item in reservoir with the minimum key is replaced by item
+      // v_i
+      reservoir.pop();
+      reservoir.push(std::make_pair(k_i, iprob - weights.begin() + 1));
+    }
+  }
+
+  for (auto iret = result.end(); iret != result.begin();) {
+    --iret;
+
+    if (reservoir.empty()) {
+      throw std::runtime_error(
+          "Reservoir empty before all elements have been filled");
+    }
+
+    *iret = reservoir.top().second;
+    reservoir.pop();
+  }
+
+  if (!reservoir.empty()) {
+    throw std::runtime_error(
+        "Reservoir not empty after all elements have been filled");
+  }
+
+  return result;
+}
+}  // namespace gtsam
diff --git a/gtsam/base/tests/testWeightedSampler.cpp b/gtsam/base/tests/testWeightedSampler.cpp
index 865c33f9f..7eaaea545 100644
--- a/gtsam/base/tests/testWeightedSampler.cpp
+++ b/gtsam/base/tests/testWeightedSampler.cpp
@@ -16,24 +16,18 @@
  * @date    MAy 2019
  **/
 
-#include <vector>
-
-namespace gtsam {
-template <class Engine>
-std::vector<size_t> sampleWithoutReplacement(Engine& rng, size_t s,
-                                             std::vector<double> weights);
-}
+#include <gtsam/base/WeightedSampler.h>
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/random/mersenne_twister.hpp>
+#include <random>
 
 using namespace std;
 using namespace gtsam;
 
 TEST(WeightedSampler, sampleWithoutReplacement) {
   vector<double> weights{1, 2, 3, 4, 3, 2, 1};
-  boost::mt19937 rng(42);
+  mt19937 rng(42);
   auto samples = sampleWithoutReplacement(rng, 5, weights);
   EXPECT_LONGS_EQUAL(5, samples.size());
 }
@@ -43,100 +37,3 @@ int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }
-
-#include <queue>
-#include <stdexcept>
-#include <utility>
-#include <vector>
-
-namespace gtsam {
-/* ************************************************************************* */
-/* Implementation adapted from paper at
- * https://www.ethz.ch/content/dam/ethz/special-interest/baug/ivt/ivt-dam/vpl/reports/1101-1200/ab1141.pdf
- */
-template <class Engine>
-vector<size_t> sampleWithoutReplacement(Engine& rng, size_t s,
-                                        vector<double> weights) {
-  const size_t n = weights.size();
-  if (n < s) {
-    throw runtime_error("s must be smaller than weights.size()");
-  }
-
-  // Return empty array if s==0
-  vector<size_t> result(s);
-  if (s == 0) return result;
-
-  // Step 1: The first m items of V are inserted into reservoir
-  // Step 2: For each item v_i ∈ reservoir: Calculate a key k_i = u_i^(1/w),
-  // where u_i = random(0, 1)
-  // (Modification: Calculate and store -log k_i = e_i / w where e_i = exp(1),
-  //  reservoir is a priority queue that pops the *maximum* elements)
-  priority_queue<pair<double, size_t> > reservoir;
-
-  static const double kexp1 = exp(1.0);
-  for (auto iprob = weights.begin(); iprob != weights.begin() + s; ++iprob) {
-    double k_i = kexp1 / *iprob;
-    reservoir.push(make_pair(k_i, iprob - weights.begin() + 1));
-  }
-
-  // Step 4: Repeat Steps 5–10 until the population is exhausted
-  {
-    // Step 3: The threshold T_w is the minimum key of reservoir
-    // (Modification: This is now the logarithm)
-    // Step 10: The new threshold T w is the new minimum key of reservoir
-    const pair<double, size_t>& T_w = reservoir.top();
-
-    // Incrementing iprob is part of Step 7
-    for (auto iprob = weights.begin() + s; iprob != weights.end(); ++iprob) {
-      // Step 5: Let r = random(0, 1) and X_w = log(r) / log(T_w)
-      // (Modification: Use e = -exp(1) instead of log(r))
-      double X_w = kexp1 / T_w.first;
-
-      // Step 6: From the current item v_c skip items until item v_i, such that:
-      double w = 0.0;
-
-      // Step 7: w_c + w_{c+1} + ··· + w_{i−1} < X_w <= w_c + w_{c+1} + ··· +
-      // w_{i−1} + w_i
-      for (; iprob != weights.end(); ++iprob) {
-        w += *iprob;
-        if (X_w <= w) break;
-      }
-
-      // Step 7: No such item, terminate
-      if (iprob == weights.end()) break;
-
-      // Step 9: Let t_w = T_w^{w_i}, r_2 = random(t_w, 1) and v_i’s key: k_i =
-      // (r_2)^{1/w_i} (Mod: Let t_w = log(T_w) * {w_i}, e_2 =
-      // log(random(e^{t_w}, 1)) and v_i’s key: k_i = -e_2 / w_i)
-      double t_w = -T_w.first * *iprob;
-      boost::uniform_real<double> randomAngle(exp(t_w), 1.0);
-      double e_2 = log(randomAngle(rng));
-      double k_i = -e_2 / *iprob;
-
-      // Step 8: The item in reservoir with the minimum key is replaced by item
-      // v_i
-      reservoir.pop();
-      reservoir.push(make_pair(k_i, iprob - weights.begin() + 1));
-    }
-  }
-
-  for (auto iret = result.end(); iret != result.begin();) {
-    --iret;
-
-    if (reservoir.empty()) {
-      throw runtime_error(
-          "Reservoir empty before all elements have been filled");
-    }
-
-    *iret = reservoir.top().second;
-    reservoir.pop();
-  }
-
-  if (!reservoir.empty()) {
-    throw runtime_error(
-        "Reservoir not empty after all elements have been filled");
-  }
-
-  return result;
-}
-}  // namespace gtsam

From 8b201f07bbc4690997f16629fed71b4aefa1a7fb Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 15 Jun 2019 17:52:33 -0400
Subject: [PATCH 03/10] Random now uses std header <random>.

---
 gtsam/geometry/Rot3.cpp            |  7 ++++---
 gtsam/geometry/Rot3.h              | 11 +++++++++--
 gtsam/geometry/SO4.cpp             |  2 +-
 gtsam/geometry/SO4.h               |  2 --
 gtsam/geometry/SOn.h               | 13 +++++++------
 gtsam/geometry/Unit3.cpp           |  7 ++++---
 gtsam/geometry/Unit3.h             | 11 ++++++++---
 gtsam/geometry/tests/testSO4.cpp   |  3 ++-
 gtsam/geometry/tests/testSOn.cpp   |  5 ++---
 gtsam/geometry/tests/testUnit3.cpp |  9 +++++----
 10 files changed, 42 insertions(+), 28 deletions(-)

diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index c2e711a17..b1e8dd14b 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -21,8 +21,9 @@
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/SO3.h>
 #include <boost/math/constants/constants.hpp>
-#include <boost/random.hpp>
+
 #include <cmath>
+#include <random>
 
 using namespace std;
 
@@ -34,10 +35,10 @@ void Rot3::print(const std::string& s) const {
 }
 
 /* ************************************************************************* */
-Rot3 Rot3::Random(boost::mt19937& rng) {
+Rot3 Rot3::Random(std::mt19937& rng) {
   // TODO allow any engine without including all of boost :-(
   Unit3 axis = Unit3::Random(rng);
-  boost::uniform_real<double> randomAngle(-M_PI, M_PI);
+  uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
   double angle = randomAngle(rng);
   return AxisAngle(axis, angle);
 }
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index ab43b2d42..e4408c9f4 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -28,6 +28,8 @@
 #include <gtsam/base/concepts.h>
 #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
 
+#include <random>
+
 // You can override the default coordinate mode using this flag
 #ifndef ROT3_DEFAULT_COORDINATES_MODE
   #ifdef GTSAM_USE_QUATERNIONS
@@ -128,8 +130,13 @@ namespace gtsam {
     Rot3(const Quaternion& q);
     Rot3(double w, double x, double y, double z) : Rot3(Quaternion(w, x, y, z)) {}
 
-    /// Random, generates a random axis, then random angle \in [-p,pi]
-    static Rot3 Random(boost::mt19937 & rng);
+    /**
+     * Random, generates a random axis, then random angle \in [-p,pi]
+     * Example:
+     *   std::mt19937 engine(42);
+     *   Unit3 unit = Unit3::Random(engine);
+     */
+    static Rot3 Random(std::mt19937 & rng);
 
     /** Virtual destructor */
     virtual ~Rot3() {}
diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp
index 3e6ae485e..9b6e4217a 100644
--- a/gtsam/geometry/SO4.cpp
+++ b/gtsam/geometry/SO4.cpp
@@ -22,11 +22,11 @@
 #include <gtsam/geometry/Unit3.h>
 
 #include <Eigen/Eigenvalues>
-#include <boost/random.hpp>
 
 #include <algorithm>
 #include <cmath>
 #include <iostream>
+#include <random>
 #include <vector>
 
 using namespace std;
diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h
index e8e1bc017..5014414c2 100644
--- a/gtsam/geometry/SO4.h
+++ b/gtsam/geometry/SO4.h
@@ -27,8 +27,6 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/dllexport.h>
 
-#include <boost/random/mersenne_twister.hpp>
-
 #include <string>
 
 namespace gtsam {
diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h
index 945d94bdc..8b37f443a 100644
--- a/gtsam/geometry/SOn.h
+++ b/gtsam/geometry/SOn.h
@@ -23,8 +23,9 @@
 #include <gtsam/dllexport.h>
 
 #include <Eigen/Core>
-#include <boost/random.hpp>
 
+#include <iostream> // TODO(frank): how to avoid?
+#include <random>
 #include <string>
 #include <type_traits>
 #include <vector>
@@ -112,12 +113,12 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   /// currently only defined for SO3.
   static SO ChordalMean(const std::vector<SO>& rotations);
 
-  /// Random SO(n) element (no big claims about uniformity)
+  /// Random SO(n) element (no big claims about uniformity). SO(3) is specialized in SO3.cpp
   template <int N_ = N, typename = IsDynamic<N_>>
-  static SO Random(boost::mt19937& rng, size_t n = 0) {
+  static SO Random(std::mt19937& rng, size_t n = 0) {
     if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
-    // TODO(frank): This needs to be re-thought!
-    static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
+    // TODO(frank): this might need to be re-thought
+    static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
     const size_t d = SO::Dimension(n);
     Vector xi(d);
     for (size_t j = 0; j < d; j++) {
@@ -128,7 +129,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
 
   /// Random SO(N) element (no big claims about uniformity)
   template <int N_ = N, typename = IsFixed<N_>>
-  static SO Random(boost::mt19937& rng) {
+  static SO Random(std::mt19937& rng) {
     // By default, use dynamic implementation above. Specialized for SO(3).
     return SO(SO<Eigen::Dynamic>::Random(rng, N).matrix());
   }
diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp
index 533ee500e..0e1b09958 100755
--- a/gtsam/geometry/Unit3.cpp
+++ b/gtsam/geometry/Unit3.cpp
@@ -27,12 +27,13 @@
 #  pragma clang diagnostic push
 #  pragma clang diagnostic ignored "-Wunused-variable"
 #endif
+#define BOOST_ALLOW_DEPRECATED_HEADERS
 #include <boost/random/uniform_on_sphere.hpp>
+#include <boost/random/variate_generator.hpp>
 #ifdef __clang__
 #  pragma clang diagnostic pop
 #endif
 
-#include <boost/random/variate_generator.hpp>
 #include <iostream>
 #include <limits>
 #include <cmath>
@@ -54,12 +55,12 @@ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2, 3> H) {
 }
 
 /* ************************************************************************* */
-Unit3 Unit3::Random(boost::mt19937 & rng) {
+Unit3 Unit3::Random(std::mt19937& rng) {
   // TODO(dellaert): allow any engine without including all of boost :-(
   boost::uniform_on_sphere<double> randomDirection(3);
   // This variate_generator object is required for versions of boost somewhere
   // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
-  boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> > generator(
+  boost::variate_generator<std::mt19937&, boost::uniform_on_sphere<double> > generator(
       rng, randomDirection);
   const vector<double> d = generator();
   return Unit3(d[0], d[1], d[2]);
diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h
index 211698806..1c6945c4c 100644
--- a/gtsam/geometry/Unit3.h
+++ b/gtsam/geometry/Unit3.h
@@ -27,9 +27,9 @@
 #include <gtsam/dllexport.h>
 
 #include <boost/optional.hpp>
-#include <boost/random/mersenne_twister.hpp>
 #include <boost/serialization/nvp.hpp>
 
+#include <random>
 #include <string>
 
 #ifdef GTSAM_USE_TBB
@@ -97,8 +97,13 @@ public:
   GTSAM_EXPORT static Unit3 FromPoint3(const Point3& point, //
       OptionalJacobian<2, 3> H = boost::none);
 
-  /// Random direction, using boost::uniform_on_sphere
-  GTSAM_EXPORT static Unit3 Random(boost::mt19937 & rng);
+  /**
+   * Random direction, using boost::uniform_on_sphere
+   * Example:
+   *   std::mt19937 engine(42);
+   *   Unit3 unit = Unit3::Random(engine);
+   */
+  GTSAM_EXPORT static Unit3 Random(std::mt19937 & rng);
 
   /// @}
 
diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp
index 594da01f6..f771eea5f 100644
--- a/gtsam/geometry/tests/testSO4.cpp
+++ b/gtsam/geometry/tests/testSO4.cpp
@@ -25,6 +25,7 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <iostream>
+#include <random>
 
 using namespace std;
 using namespace gtsam;
@@ -57,7 +58,7 @@ SO4 Q3 = SO4::Expmap(v3);
 
 //******************************************************************************
 TEST(SO4, Random) {
-  boost::mt19937 rng(42);
+  std::mt19937 rng(42);
   auto Q = SO4::Random(rng);
   EXPECT_LONGS_EQUAL(4, Q.matrix().rows());
 }
diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp
index 4e5f12c0c..eb84a4d55 100644
--- a/gtsam/geometry/tests/testSOn.cpp
+++ b/gtsam/geometry/tests/testSOn.cpp
@@ -30,9 +30,8 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/random.hpp>
-
 #include <iostream>
+#include <random>
 #include <stdexcept>
 #include <type_traits>
 
@@ -88,7 +87,7 @@ TEST(SOn, Values) {
 
 //******************************************************************************
 TEST(SOn, Random) {
-  static boost::mt19937 rng(42);
+  static std::mt19937 rng(42);
   EXPECT_LONGS_EQUAL(3, SOn::Random(rng, 3).rows());
   EXPECT_LONGS_EQUAL(4, SOn::Random(rng, 4).rows());
   EXPECT_LONGS_EQUAL(5, SOn::Random(rng, 5).rows());
diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp
index 9d53a30a6..59b99e525 100644
--- a/gtsam/geometry/tests/testUnit3.cpp
+++ b/gtsam/geometry/tests/testUnit3.cpp
@@ -33,9 +33,10 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <boost/bind.hpp>
-#include <boost/random.hpp>
 #include <boost/assign/std/vector.hpp>
+
 #include <cmath>
+#include <random>
 
 using namespace boost::assign;
 using namespace gtsam;
@@ -339,7 +340,7 @@ TEST(Unit3, basis) {
 /// Check the basis derivatives of a bunch of random Unit3s.
 TEST(Unit3, basis_derivatives) {
   int num_tests = 100;
-  boost::mt19937 rng(42);
+  std::mt19937 rng(42);
   for (int i = 0; i < num_tests; i++) {
     Unit3 p = Unit3::Random(rng);
 
@@ -403,7 +404,7 @@ TEST(Unit3, retract_expmap) {
 
 //*******************************************************************************
 TEST(Unit3, Random) {
-  boost::mt19937 rng(42);
+  std::mt19937 rng(42);
   // Check that means are all zero at least
   Point3 expectedMean(0,0,0), actualMean(0,0,0);
   for (size_t i = 0; i < 100; i++)
@@ -415,7 +416,7 @@ TEST(Unit3, Random) {
 //*******************************************************************************
 // New test that uses Unit3::Random
 TEST(Unit3, localCoordinates_retract) {
-  boost::mt19937 rng(42);
+  std::mt19937 rng(42);
   size_t numIterations = 10000;
 
   for (size_t i = 0; i < numIterations; i++) {

From 7d86b073e6c19c231d7a34bcc07aff4c8fb44295 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 15 Jun 2019 17:52:57 -0400
Subject: [PATCH 04/10] Removed boost/random usage from linear and discrete
 directories

---
 gtsam/discrete/DiscreteConditional.cpp | 14 +++++---------
 gtsam/linear/NoiseModel.cpp            | 12 +++++-------
 gtsam/linear/Sampler.cpp               |  5 ++---
 gtsam/linear/Sampler.h                 |  4 ++--
 4 files changed, 14 insertions(+), 21 deletions(-)

diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index cf2997ce0..2ab3054a8 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -23,13 +23,11 @@
 #include <gtsam/base/debug.h>
 
 #include <boost/make_shared.hpp>
-#include <boost/random/mersenne_twister.hpp>
-#include <boost/random/uniform_real.hpp>
-#include <boost/random/variate_generator.hpp>
 
-#include <vector>
 #include <algorithm>
+#include <random>
 #include <stdexcept>
+#include <vector>
 
 using namespace std;
 
@@ -176,8 +174,7 @@ size_t DiscreteConditional::solve(const Values& parentsValues) const {
 /* ******************************************************************************** */
 size_t DiscreteConditional::sample(const Values& parentsValues) const {
 
-  using boost::uniform_real;
-  static boost::mt19937 gen(2); // random number generator
+  static mt19937 rng(2); // random number generator
 
   bool debug = ISDEBUG("DiscreteConditional::sample");
 
@@ -209,9 +206,8 @@ size_t DiscreteConditional::sample(const Values& parentsValues) const {
   }
 
   // inspired by http://www.boost.org/doc/libs/1_46_1/doc/html/boost_random/tutorial.html
-  uniform_real<> dist(0, cdf.back());
-  boost::variate_generator<boost::mt19937&, uniform_real<> > die(gen, dist);
-  size_t sampled = lower_bound(cdf.begin(), cdf.end(), die()) - cdf.begin();
+  uniform_real_distribution<double> dist(0, cdf.back());
+  size_t sampled = lower_bound(cdf.begin(), cdf.end(), dist(rng)) - cdf.begin();
   if (debug)
     cout << "-> " << sampled << endl;
 
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index 4e599d45f..a6ebea394 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -21,15 +21,13 @@
 
 #include <boost/format.hpp>
 #include <boost/make_shared.hpp>
-#include <boost/random/linear_congruential.hpp>
-#include <boost/random/normal_distribution.hpp>
-#include <boost/random/variate_generator.hpp>
 
-#include <limits>
-#include <iostream>
-#include <typeinfo>
-#include <stdexcept>
 #include <cmath>
+#include <iostream>
+#include <limits>
+#include <random>
+#include <stdexcept>
+#include <typeinfo>
 
 using namespace std;
 
diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp
index bfbc222ba..9f1527bf9 100644
--- a/gtsam/linear/Sampler.cpp
+++ b/gtsam/linear/Sampler.cpp
@@ -46,10 +46,9 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) {
     if (sigma == 0.0) {
       result(i) = 0.0;
     } else {
-      typedef boost::normal_distribution<double> Normal;
+      typedef std::normal_distribution<double> Normal;
       Normal dist(0.0, sigma);
-      boost::variate_generator<boost::mt19937_64&, Normal> norm(generator_, dist);
-      result(i) = norm();
+      result(i) = dist(generator_);
     }
   }
   return result;
diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h
index 6c84bfda2..93145c31a 100644
--- a/gtsam/linear/Sampler.h
+++ b/gtsam/linear/Sampler.h
@@ -20,7 +20,7 @@
 
 #include <gtsam/linear/NoiseModel.h>
 
-#include <boost/random.hpp>
+#include <random>
 
 namespace gtsam {
 
@@ -37,7 +37,7 @@ protected:
   noiseModel::Diagonal::shared_ptr model_;
 
   /** generator */
-  boost::mt19937_64 generator_;
+  std::mt19937_64 generator_;
 
 public:
   typedef boost::shared_ptr<Sampler> shared_ptr;

From f8af4a465d58d8a5adce2cca2e4bb399ddd51f1f Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 15 Jun 2019 18:42:54 -0400
Subject: [PATCH 05/10] Made into class

---
 gtsam/base/WeightedSampler.h             | 163 +++++++++++++----------
 gtsam/base/tests/testWeightedSampler.cpp |   5 +-
 2 files changed, 95 insertions(+), 73 deletions(-)

diff --git a/gtsam/base/WeightedSampler.h b/gtsam/base/WeightedSampler.h
index f4495ed68..916f37033 100644
--- a/gtsam/base/WeightedSampler.h
+++ b/gtsam/base/WeightedSampler.h
@@ -16,6 +16,9 @@
  * @date    May 2019
  **/
 
+#pragma once
+
+#include <cmath>
 #include <queue>
 #include <random>
 #include <stdexcept>
@@ -25,92 +28,110 @@
 namespace gtsam {
 /*
  * Fast sampling without replacement.
+ * Example usage:
+ *   std::mt19937 rng(42);
+ *   WeightedSampler<std::mt19937> sampler(&rng);
+ *   auto samples = sampler.sampleWithoutReplacement(5, weights);
  */
-template <class Engine>
-std::vector<size_t> sampleWithoutReplacement(Engine& rng, size_t s,
-                                             std::vector<double> weights) {
-  // Implementation adapted from paper at
-  // https://www.ethz.ch/content/dam/ethz/special-interest/baug/ivt/ivt-dam/vpl/reports/1101-1200/ab1141.pdf
-  const size_t n = weights.size();
-  if (n < s) {
-    throw std::runtime_error("s must be smaller than weights.size()");
-  }
+template <class Engine = std::mt19937>
+class WeightedSampler {
+ private:
+  Engine* engine_;  // random number generation engine
 
-  // Return empty array if s==0
-  std::vector<size_t> result(s);
-  if (s == 0) return result;
+ public:
+  /**
+   * Construct from random number generation engine
+   * We only store a pointer to it.
+   */
+  explicit WeightedSampler(Engine* engine) : engine_(engine) {}
 
-  // Step 1: The first m items of V are inserted into reservoir
-  // Step 2: For each item v_i ∈ reservoir: Calculate a key k_i = u_i^(1/w),
-  // where u_i = random(0, 1)
-  // (Modification: Calculate and store -log k_i = e_i / w where e_i = exp(1),
-  //  reservoir is a priority queue that pops the *maximum* elements)
-  std::priority_queue<std::pair<double, size_t> > reservoir;
+  std::vector<size_t> sampleWithoutReplacement(size_t numSamples,
+                                               std::vector<double> weights) {
+    // Implementation adapted from code accompanying paper at
+    // https://www.ethz.ch/content/dam/ethz/special-interest/baug/ivt/ivt-dam/vpl/reports/1101-1200/ab1141.pdf
+    const size_t n = weights.size();
+    if (n < numSamples) {
+      throw std::runtime_error(
+          "numSamples must be smaller than weights.size()");
+    }
 
-  static const double kexp1 = exp(1.0);
-  for (auto iprob = weights.begin(); iprob != weights.begin() + s; ++iprob) {
-    double k_i = kexp1 / *iprob;
-    reservoir.push(std::make_pair(k_i, iprob - weights.begin() + 1));
-  }
+    // Return empty array if numSamples==0
+    std::vector<size_t> result(numSamples);
+    if (numSamples == 0) return result;
 
-  // Step 4: Repeat Steps 5–10 until the population is exhausted
-  {
-    // Step 3: The threshold T_w is the minimum key of reservoir
-    // (Modification: This is now the logarithm)
-    // Step 10: The new threshold T w is the new minimum key of reservoir
-    const std::pair<double, size_t>& T_w = reservoir.top();
+    // Step 1: The first m items of V are inserted into reservoir
+    // Step 2: For each item v_i ∈ reservoir: Calculate a key k_i = u_i^(1/w),
+    // where u_i = random(0, 1)
+    // (Modification: Calculate and store -log k_i = e_i / w where e_i = exp(1),
+    //  reservoir is a priority queue that pops the *maximum* elements)
+    std::priority_queue<std::pair<double, size_t> > reservoir;
 
-    // Incrementing iprob is part of Step 7
-    for (auto iprob = weights.begin() + s; iprob != weights.end(); ++iprob) {
-      // Step 5: Let r = random(0, 1) and X_w = log(r) / log(T_w)
-      // (Modification: Use e = -exp(1) instead of log(r))
-      double X_w = kexp1 / T_w.first;
+    static const double kexp1 = std::exp(1.0);
+    for (auto it = weights.begin(); it != weights.begin() + numSamples; ++it) {
+      const double k_i = kexp1 / *it;
+      reservoir.push(std::make_pair(k_i, it - weights.begin() + 1));
+    }
 
-      // Step 6: From the current item v_c skip items until item v_i, such that:
-      double w = 0.0;
+    // Step 4: Repeat Steps 5–10 until the population is exhausted
+    {
+      // Step 3: The threshold T_w is the minimum key of reservoir
+      // (Modification: This is now the logarithm)
+      // Step 10: The new threshold T w is the new minimum key of reservoir
+      const std::pair<double, size_t>& T_w = reservoir.top();
 
-      // Step 7: w_c + w_{c+1} + ··· + w_{i−1} < X_w <= w_c + w_{c+1} + ··· +
-      // w_{i−1} + w_i
-      for (; iprob != weights.end(); ++iprob) {
-        w += *iprob;
-        if (X_w <= w) break;
+      // Incrementing it is part of Step 7
+      for (auto it = weights.begin() + numSamples; it != weights.end(); ++it) {
+        // Step 5: Let r = random(0, 1) and X_w = log(r) / log(T_w)
+        // (Modification: Use e = -exp(1) instead of log(r))
+        const double X_w = kexp1 / T_w.first;
+
+        // Step 6: From the current item v_c skip items until item v_i, such
+        // that:
+        double w = 0.0;
+
+        // Step 7: w_c + w_{c+1} + ··· + w_{i−1} < X_w <= w_c + w_{c+1} + ··· +
+        // w_{i−1} + w_i
+        for (; it != weights.end(); ++it) {
+          w += *it;
+          if (X_w <= w) break;
+        }
+
+        // Step 7: No such item, terminate
+        if (it == weights.end()) break;
+
+        // Step 9: Let t_w = T_w^{w_i}, r_2 = random(t_w, 1) and v_i’s key: k_i
+        // = (r_2)^{1/w_i} (Mod: Let t_w = log(T_w) * {w_i}, e_2 =
+        // log(random(e^{t_w}, 1)) and v_i’s key: k_i = -e_2 / w_i)
+        const double t_w = -T_w.first * *it;
+        std::uniform_real_distribution<double> randomAngle(std::exp(t_w), 1.0);
+        const double e_2 = std::log(randomAngle(*engine_));
+        const double k_i = -e_2 / *it;
+
+        // Step 8: The item in reservoir with the minimum key is replaced by
+        // item v_i
+        reservoir.pop();
+        reservoir.push(std::make_pair(k_i, it - weights.begin() + 1));
+      }
+    }
+
+    for (auto iret = result.end(); iret != result.begin();) {
+      --iret;
+
+      if (reservoir.empty()) {
+        throw std::runtime_error(
+            "Reservoir empty before all elements have been filled");
       }
 
-      // Step 7: No such item, terminate
-      if (iprob == weights.end()) break;
-
-      // Step 9: Let t_w = T_w^{w_i}, r_2 = random(t_w, 1) and v_i’s key: k_i =
-      // (r_2)^{1/w_i} (Mod: Let t_w = log(T_w) * {w_i}, e_2 =
-      // log(random(e^{t_w}, 1)) and v_i’s key: k_i = -e_2 / w_i)
-      double t_w = -T_w.first * *iprob;
-      std::uniform_real_distribution<double> randomAngle(exp(t_w), 1.0);
-      double e_2 = log(randomAngle(rng));
-      double k_i = -e_2 / *iprob;
-
-      // Step 8: The item in reservoir with the minimum key is replaced by item
-      // v_i
+      *iret = reservoir.top().second;
       reservoir.pop();
-      reservoir.push(std::make_pair(k_i, iprob - weights.begin() + 1));
     }
-  }
 
-  for (auto iret = result.end(); iret != result.begin();) {
-    --iret;
-
-    if (reservoir.empty()) {
+    if (!reservoir.empty()) {
       throw std::runtime_error(
-          "Reservoir empty before all elements have been filled");
+          "Reservoir not empty after all elements have been filled");
     }
 
-    *iret = reservoir.top().second;
-    reservoir.pop();
+    return result;
   }
-
-  if (!reservoir.empty()) {
-    throw std::runtime_error(
-        "Reservoir not empty after all elements have been filled");
-  }
-
-  return result;
-}
+};  // namespace gtsam
 }  // namespace gtsam
diff --git a/gtsam/base/tests/testWeightedSampler.cpp b/gtsam/base/tests/testWeightedSampler.cpp
index 7eaaea545..8ebcdfd2e 100644
--- a/gtsam/base/tests/testWeightedSampler.cpp
+++ b/gtsam/base/tests/testWeightedSampler.cpp
@@ -27,8 +27,9 @@ using namespace gtsam;
 
 TEST(WeightedSampler, sampleWithoutReplacement) {
   vector<double> weights{1, 2, 3, 4, 3, 2, 1};
-  mt19937 rng(42);
-  auto samples = sampleWithoutReplacement(rng, 5, weights);
+  std::mt19937 rng(42);
+  WeightedSampler<std::mt19937> sampler(&rng);
+  auto samples = sampler.sampleWithoutReplacement(5, weights);
   EXPECT_LONGS_EQUAL(5, samples.size());
 }
 

From 50e484a3c67d4977c2b9afa51fe0643bdc2666c7 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 15 Jun 2019 18:43:35 -0400
Subject: [PATCH 06/10] Now using new WeightedSampler class

---
 gtsam/linear/SubgraphBuilder.cpp | 91 +++++---------------------------
 1 file changed, 13 insertions(+), 78 deletions(-)

diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp
index a999b3a71..c6b3ca15f 100644
--- a/gtsam/linear/SubgraphBuilder.cpp
+++ b/gtsam/linear/SubgraphBuilder.cpp
@@ -17,6 +17,7 @@
 
 #include <gtsam/base/DSFVector.h>
 #include <gtsam/base/FastMap.h>
+#include <gtsam/base/WeightedSampler.h>
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/inference/VariableIndex.h>
 #include <gtsam/linear/Errors.h>
@@ -35,8 +36,11 @@
 #include <iostream>
 #include <numeric>  // accumulate
 #include <queue>
+#include <random>
 #include <set>
 #include <stdexcept>
+#include <string>
+#include <utility>
 #include <vector>
 
 using std::cout;
@@ -68,81 +72,11 @@ static vector<size_t> sort_idx(const Container &src) {
   return idx;
 }
 
-/*****************************************************************************/
-static vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
-  /* compute the sum of the weights */
-  const double sum = std::accumulate(weight.begin(), weight.end(), 0.0);
-  if (sum == 0.0) {
-    throw std::runtime_error("Weight vector has no non-zero weights");
-  }
-
-  /* make a normalized and accumulated version of the weight vector */
-  const size_t m = weight.size();
-  vector<double> cdf;
-  cdf.reserve(m);
-  for (size_t i = 0; i < m; ++i) {
-    cdf.push_back(weight[i] / sum);
-  }
-
-  vector<double> acc(m);
-  std::partial_sum(cdf.begin(), cdf.end(), acc.begin());
-
-  /* iid sample n times */
-  vector<size_t> samples;
-  samples.reserve(n);
-  const double denominator = (double)RAND_MAX;
-  for (size_t i = 0; i < n; ++i) {
-    const double value = rand() / denominator;
-    /* binary search the interval containing "value" */
-    const auto it = std::lower_bound(acc.begin(), acc.end(), value);
-    const size_t index = it - acc.begin();
-    samples.push_back(index);
-  }
-  return samples;
-}
-
-/*****************************************************************************/
-static vector<size_t> UniqueSampler(const vector<double> &weight,
-                                    const size_t n) {
-  const size_t m = weight.size();
-  if (n > m) throw std::invalid_argument("UniqueSampler: invalid input size");
-
-  vector<size_t> results;
-
-  size_t count = 0;
-  vector<bool> touched(m, false);
-  while (count < n) {
-    vector<size_t> localIndices;
-    localIndices.reserve(n - count);
-    vector<double> localWeights;
-    localWeights.reserve(n - count);
-
-    /* collect data */
-    for (size_t i = 0; i < m; ++i) {
-      if (!touched[i]) {
-        localIndices.push_back(i);
-        localWeights.push_back(weight[i]);
-      }
-    }
-
-    /* sampling and cache results */
-    vector<size_t> samples = iidSampler(localWeights, n - count);
-    for (const size_t &index : samples) {
-      if (touched[index] == false) {
-        touched[index] = true;
-        results.push_back(index);
-        if (++count >= n) break;
-      }
-    }
-  }
-  return results;
-}
-
 /****************************************************************************/
 Subgraph::Subgraph(const vector<size_t> &indices) {
   edges_.reserve(indices.size());
   for (const size_t &index : indices) {
-    const Edge e {index,1.0};
+    const Edge e{index, 1.0};
     edges_.push_back(e);
   }
 }
@@ -423,12 +357,13 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
 /****************************************************************/
 vector<size_t> SubgraphBuilder::sample(const vector<double> &weights,
                                        const size_t t) const {
-  return UniqueSampler(weights, t);
+  std::mt19937 rng(42);  // TODO(frank): allow us to use a different seed
+  WeightedSampler<std::mt19937> sampler(&rng);
+  return sampler.sampleWithoutReplacement(t, weights);
 }
 
 /****************************************************************/
-Subgraph SubgraphBuilder::operator()(
-    const GaussianFactorGraph &gfg) const {
+Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const {
   const auto &p = parameters_;
   const auto inverse_ordering = Ordering::Natural(gfg);
   const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
@@ -518,15 +453,15 @@ GaussianFactorGraph::shared_ptr buildFactorSubgraph(
   subgraphFactors->reserve(subgraph.size());
   for (const auto &e : subgraph) {
     const auto factor = gfg[e.index];
-    subgraphFactors->push_back(clone ? factor->clone(): factor);
+    subgraphFactors->push_back(clone ? factor->clone() : factor);
   }
   return subgraphFactors;
 }
 
 /**************************************************************************************************/
-std::pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
-splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph) {
-
+std::pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>  //
+splitFactorGraph(const GaussianFactorGraph &factorGraph,
+                 const Subgraph &subgraph) {
   // Get the subgraph by calling cheaper method
   auto subgraphFactors = buildFactorSubgraph(factorGraph, subgraph, false);
 

From afddf0084cbf216da2a51448b1d326d184cac588 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 15 Jun 2019 23:33:43 -0400
Subject: [PATCH 07/10] Inlined random direction generation

---
 gtsam/geometry/Unit3.cpp | 31 ++++++++++++-------------------
 1 file changed, 12 insertions(+), 19 deletions(-)

diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp
index 0e1b09958..3d46b18b8 100755
--- a/gtsam/geometry/Unit3.cpp
+++ b/gtsam/geometry/Unit3.cpp
@@ -23,17 +23,6 @@
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/config.h>  // for GTSAM_USE_TBB
 
-#ifdef __clang__
-#  pragma clang diagnostic push
-#  pragma clang diagnostic ignored "-Wunused-variable"
-#endif
-#define BOOST_ALLOW_DEPRECATED_HEADERS
-#include <boost/random/uniform_on_sphere.hpp>
-#include <boost/random/variate_generator.hpp>
-#ifdef __clang__
-#  pragma clang diagnostic pop
-#endif
-
 #include <iostream>
 #include <limits>
 #include <cmath>
@@ -56,14 +45,18 @@ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2, 3> H) {
 
 /* ************************************************************************* */
 Unit3 Unit3::Random(std::mt19937& rng) {
-  // TODO(dellaert): allow any engine without including all of boost :-(
-  boost::uniform_on_sphere<double> randomDirection(3);
-  // This variate_generator object is required for versions of boost somewhere
-  // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
-  boost::variate_generator<std::mt19937&, boost::uniform_on_sphere<double> > generator(
-      rng, randomDirection);
-  const vector<double> d = generator();
-  return Unit3(d[0], d[1], d[2]);
+  // http://mathworld.wolfram.com/SpherePointPicking.html
+  // Adapted from implementation in boost, but using std <random>
+  std::uniform_real_distribution<double> uniform(-1.0, 1.0);
+  double sqsum;
+  double x, y;
+  do {
+    x = uniform(rng);
+    y = uniform(rng);
+    sqsum = x * x + y * y;
+  } while (sqsum > 1);
+  const double mult = 2 * sqrt(1 - sqsum);
+  return Unit3(x * mult, y * mult, 2 * sqsum - 1);
 }
 
 /* ************************************************************************* */

From 8e81890f9b4cad3d89dcda2fec260cbf17605c7f Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 15 Jun 2019 23:45:43 -0400
Subject: [PATCH 08/10] eradicated last vestiges of boost/random in
 gtsam_unstable

---
 gtsam_unstable/geometry/SimPolygon2D.cpp      | 23 ++++++++-----------
 gtsam_unstable/geometry/SimPolygon2D.h        |  7 +++---
 .../geometry/tests/testSimPolygon2D.cpp       |  5 ++--
 .../slam/tests/testOccupancyGrid.cpp          | 11 ++++-----
 gtsam_unstable/timing/timeDSFvariants.cpp     | 11 +++++----
 5 files changed, 28 insertions(+), 29 deletions(-)

diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp
index eb732f2c5..2d4bd7a2d 100644
--- a/gtsam_unstable/geometry/SimPolygon2D.cpp
+++ b/gtsam_unstable/geometry/SimPolygon2D.cpp
@@ -4,10 +4,7 @@
  */
 
 #include <iostream>
-#include <boost/random/linear_congruential.hpp>
-#include <boost/random/uniform_real.hpp>
-#include <boost/random/normal_distribution.hpp>
-#include <boost/random/variate_generator.hpp>
+#include <random>
 
 #include <gtsam_unstable/geometry/SimPolygon2D.h>
 
@@ -16,11 +13,11 @@ namespace gtsam {
 using namespace std;
 
 const size_t max_it = 100000;
-boost::minstd_rand SimPolygon2D::rng(42u);
+std::minstd_rand SimPolygon2D::rng(42u);
 
 /* ************************************************************************* */
 void SimPolygon2D::seedGenerator(unsigned long seed) {
-  rng = boost::minstd_rand(seed);
+  rng = std::minstd_rand(seed);
 }
 
 /* ************************************************************************* */
@@ -225,23 +222,23 @@ SimPolygon2D SimPolygon2D::randomRectangle(
 
 /* ***************************************************************** */
 Point2 SimPolygon2D::randomPoint2(double s) {
-  boost::uniform_real<>  gen_t(-s/2.0, s/2.0);
+  std::uniform_real_distribution<> gen_t(-s/2.0, s/2.0);
   return Point2(gen_t(rng), gen_t(rng));
 }
 
 /* ***************************************************************** */
 Rot2 SimPolygon2D::randomAngle() {
-  boost::uniform_real<>  gen_r(-M_PI, M_PI); // modified range to avoid degenerate cases in triangles
+   // modified range to avoid degenerate cases in triangles:
+  std::uniform_real_distribution<> gen_r(-M_PI, M_PI);
   return Rot2::fromAngle(gen_r(rng));
 }
 
 /* ***************************************************************** */
 double SimPolygon2D::randomDistance(double mu, double sigma, double min_dist) {
-  boost::normal_distribution<double> norm_dist(mu, sigma);
-  boost::variate_generator<boost::minstd_rand&, boost::normal_distribution<double> > gen_d(rng, norm_dist);
+  std::normal_distribution<> norm_dist(mu, sigma);
   double d = -10.0;
   for (size_t i=0; i<max_it; ++i) {
-    d = std::abs(gen_d());
+    d = std::abs(norm_dist(rng));
     if (d > min_dist)
       return d;
   }
@@ -294,8 +291,8 @@ Point2 SimPolygon2D::randomBoundedPoint2(
     const Point2Vector& landmarks,
     const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist) {
 
-  boost::uniform_real<>  gen_x(0.0, UR_corner.x() - LL_corner.x());
-  boost::uniform_real<>  gen_y(0.0, UR_corner.y() - LL_corner.y());
+  std::uniform_real_distribution<> gen_x(0.0, UR_corner.x() - LL_corner.x());
+  std::uniform_real_distribution<> gen_y(0.0, UR_corner.y() - LL_corner.y());
 
   for (size_t i=0; i<max_it; ++i) {
     Point2 p = Point2(gen_x(rng), gen_y(rng)) + LL_corner;
diff --git a/gtsam_unstable/geometry/SimPolygon2D.h b/gtsam_unstable/geometry/SimPolygon2D.h
index 0a0e0da7f..1711abcf1 100644
--- a/gtsam_unstable/geometry/SimPolygon2D.h
+++ b/gtsam_unstable/geometry/SimPolygon2D.h
@@ -6,10 +6,11 @@
 
 #pragma once
 
-#include <map>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam_unstable/geometry/SimWall2D.h>
-#include <boost/random/linear_congruential.hpp>
+
+#include <map>
+#include <random>
 
 namespace gtsam {
 
@@ -19,7 +20,7 @@ namespace gtsam {
 class GTSAM_UNSTABLE_EXPORT SimPolygon2D {
 protected:
   Point2Vector landmarks_;
-  static boost::minstd_rand rng;
+  static std::minstd_rand rng;
 
 public:
 
diff --git a/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp b/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp
index 6528f3f91..5cdd6c100 100644
--- a/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp
+++ b/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp
@@ -1,12 +1,13 @@
 /**
- * @file testSimPolygon
+ * @file testSimPolygon2D.cpp
  * @author Alex Cunningham
  */
 
-#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam_unstable/geometry/SimPolygon2D.h>
 
+#include <CppUnitLite/TestHarness.h>
+
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam_unstable/slam/tests/testOccupancyGrid.cpp b/gtsam_unstable/slam/tests/testOccupancyGrid.cpp
index d92e5442e..1d28a7523 100644
--- a/gtsam_unstable/slam/tests/testOccupancyGrid.cpp
+++ b/gtsam_unstable/slam/tests/testOccupancyGrid.cpp
@@ -11,13 +11,12 @@
 
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/geometry/Pose2.h>
-#include <boost/random/mersenne_twister.hpp>
-//#include <boost/random/uniform_int_distribution.hpp>  // FIXME: does not exist in boost 1.46
-#include <boost/random/uniform_int.hpp> // Old header - should still exist
 
 #include <vector>
+#include <cmath.>
+#include <random>
+
 #include <stdlib.h>
-#include <math.h>
 
 using namespace std;
 using namespace gtsam;
@@ -229,8 +228,8 @@ public:
     Marginals marginals(size);
 
     // NOTE: using older interface for boost.random due to interface changes after boost 1.46
-    boost::mt19937 rng;
-    boost::uniform_int<Index> random_cell(0,size-1);
+    std::mt19937 rng;
+    std::uniform_int_distribution<> random_cell(0, size - 1);
 
     // run Metropolis for the requested number of operations
     // compute initial probability of occupancy grid, P(x_t)
diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp
index 26ed76d02..d8182ae19 100644
--- a/gtsam_unstable/timing/timeDSFvariants.cpp
+++ b/gtsam_unstable/timing/timeDSFvariants.cpp
@@ -20,13 +20,13 @@
 #include <gtsam_unstable/base/DSF.h>
 #include <gtsam/base/DSFMap.h>
 
-#include <boost/random.hpp>
 #include <boost/timer.hpp>
 #include <boost/format.hpp>
 #include <boost/assign/std/vector.hpp>
 
-#include <iostream>
 #include <fstream>
+#include <iostream>
+#include <random>
 #include <vector>
 #include <utility>
 
@@ -59,13 +59,14 @@ int main(int argc, char* argv[]) {
     cout << format("\nTesting with %1% images, %2% points, %3% matches\n")
             % (int)m % (int)N % (int)nm;
     cout << "Generating " << nm << " matches" << endl;
-    boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rn(
-        boost::mt19937(), boost::uniform_int<size_t>(0, N - 1));
+    std::mt19937 rng;
+    std::uniform_int_distribution<> rn(0, N - 1);
+ 
     typedef pair<size_t, size_t> Match;
     vector<Match> matches;
     matches.reserve(nm);
     for (size_t k = 0; k < nm; k++)
-      matches.push_back(Match(rn(), rn()));
+      matches.push_back(Match(rn(rng), rn(rng)));
 
     os << format("%1%,%2%,%3%,") % (int)m % (int)N % (int)nm;
 

From bf153b31ae7a2f0aee3526a1fb7fb2b9e2ba318b Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Fri, 28 Feb 2020 23:44:53 -0800
Subject: [PATCH 09/10] Fix test with new random behavior

---
 gtsam/discrete/tests/testDiscreteBayesNet.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp
index f8a1b6309..5ed662332 100644
--- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp
+++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp
@@ -97,8 +97,8 @@ TEST(DiscreteBayesNet, Asia)
   // now sample from it
   DiscreteFactor::Values expectedSample;
   SETDEBUG("DiscreteConditional::sample", false);
-  insert(expectedSample)(A.first, 1)(D.first, 1)(X.first, 0)(T.first, 0)(
-      S.first, 1)(E.first, 0)(L.first, 0)(B.first, 1);
+  insert(expectedSample)(A.first, 1)(D.first, 1)(X.first, 1)(T.first, 0)(
+      S.first, 1)(E.first, 1)(L.first, 1)(B.first, 0);
   DiscreteFactor::sharedValues actualSample = chordal2->sample();
   EXPECT(assert_equal(expectedSample, *actualSample));
 }

From d4135829124f96a18a52cc09b865785cf01a639b Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 29 Feb 2020 00:16:30 -0800
Subject: [PATCH 10/10] Return base 0 indices

---
 gtsam/base/WeightedSampler.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/base/WeightedSampler.h b/gtsam/base/WeightedSampler.h
index 916f37033..7c343b098 100644
--- a/gtsam/base/WeightedSampler.h
+++ b/gtsam/base/WeightedSampler.h
@@ -45,8 +45,8 @@ class WeightedSampler {
    */
   explicit WeightedSampler(Engine* engine) : engine_(engine) {}
 
-  std::vector<size_t> sampleWithoutReplacement(size_t numSamples,
-                                               std::vector<double> weights) {
+  std::vector<size_t> sampleWithoutReplacement(
+      size_t numSamples, const std::vector<double>& weights) {
     // Implementation adapted from code accompanying paper at
     // https://www.ethz.ch/content/dam/ethz/special-interest/baug/ivt/ivt-dam/vpl/reports/1101-1200/ab1141.pdf
     const size_t n = weights.size();
@@ -122,7 +122,7 @@ class WeightedSampler {
             "Reservoir empty before all elements have been filled");
       }
 
-      *iret = reservoir.top().second;
+      *iret = reservoir.top().second - 1;
       reservoir.pop();
     }