From 3a1653dd231d7983cd357761d4a785f6db5cd844 Mon Sep 17 00:00:00 2001
From: Binit Shah <bshah@gatech.edu>
Date: Sun, 17 May 2020 15:44:11 -0400
Subject: [PATCH 1/3] Include corrupting noise param to parse3DFactors

---
 gtsam/slam/dataset.cpp | 17 +++++++++++++++--
 gtsam/slam/dataset.h   |  3 ++-
 2 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 052bb3343..83636eb1d 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -541,10 +541,18 @@ std::map<Key, Pose3> parse3DPoses(const string& filename) {
 }
 
 /* ************************************************************************* */
-BetweenFactorPose3s parse3DFactors(const string& filename) {
+BetweenFactorPose3s parse3DFactors(const string& filename, 
+    const noiseModel::Diagonal::shared_ptr& corruptingNoise) {
   ifstream is(filename.c_str());
   if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
 
+  // If asked, create a sampler with random number generator
+  Sampler sampler;
+  if (corruptingNoise) {
+    sampler = Sampler(corruptingNoise);
+  }
+
+
   std::vector<BetweenFactor<Pose3>::shared_ptr> factors;
   while (!is.eof()) {
     char buf[LINESIZE];
@@ -585,8 +593,13 @@ BetweenFactorPose3s parse3DFactors(const string& filename) {
       mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonal
 
       SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
+      auto R12 = Rot3::Quaternion(qw, qx, qy, qz);
+      if (corruptingNoise) {
+        R12 = R12.retract(sampler.sample());
+      }
+
       factors.emplace_back(new BetweenFactor<Pose3>(
-          id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model));
+          id1, id2, Pose3(R12, {x, y, z}), model));
     }
   }
   return factors;
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index 3ab199bab..032799429 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -159,7 +159,8 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
 
 /// Parse edges in 3D TORO graph file into a set of BetweenFactors.
 using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>;
-GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename);
+GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename, 
+    const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr);
 
 /// Parse vertices in 3D TORO graph file into a map of Pose3s.
 GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);

From 537155dd05a0e21d095212084227438447514992 Mon Sep 17 00:00:00 2001
From: Binit Shah <binitpshah@gmail.com>
Date: Thu, 28 May 2020 04:36:35 -0400
Subject: [PATCH 2/3] Sampler initialized with noise argument

---
 gtsam/slam/dataset.cpp | 8 +-------
 1 file changed, 1 insertion(+), 7 deletions(-)

diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 83636eb1d..0ed958869 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -546,13 +546,6 @@ BetweenFactorPose3s parse3DFactors(const string& filename,
   ifstream is(filename.c_str());
   if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
 
-  // If asked, create a sampler with random number generator
-  Sampler sampler;
-  if (corruptingNoise) {
-    sampler = Sampler(corruptingNoise);
-  }
-
-
   std::vector<BetweenFactor<Pose3>::shared_ptr> factors;
   while (!is.eof()) {
     char buf[LINESIZE];
@@ -595,6 +588,7 @@ BetweenFactorPose3s parse3DFactors(const string& filename,
       SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
       auto R12 = Rot3::Quaternion(qw, qx, qy, qz);
       if (corruptingNoise) {
+        Sampler sampler(corruptingNoise);
         R12 = R12.retract(sampler.sample());
       }
 

From 5556db54200d19e453b87b7ba2eea1da106f3b78 Mon Sep 17 00:00:00 2001
From: Binit Shah <binitpshah@gmail.com>
Date: Sat, 30 May 2020 17:35:22 -0400
Subject: [PATCH 3/3] Using optional sampler outside loop

---
 gtsam/slam/dataset.cpp | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 0ed958869..66e8fc4c0 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -37,6 +37,7 @@
 #include <boost/assign/list_inserter.hpp>
 #include <boost/filesystem/operations.hpp>
 #include <boost/filesystem/path.hpp>
+#include <boost/optional.hpp>
 
 #include <cmath>
 #include <fstream>
@@ -546,6 +547,11 @@ BetweenFactorPose3s parse3DFactors(const string& filename,
   ifstream is(filename.c_str());
   if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
 
+  boost::optional<Sampler> sampler;
+  if (corruptingNoise) {
+    sampler = Sampler(corruptingNoise);
+  }
+
   std::vector<BetweenFactor<Pose3>::shared_ptr> factors;
   while (!is.eof()) {
     char buf[LINESIZE];
@@ -587,9 +593,8 @@ BetweenFactorPose3s parse3DFactors(const string& filename,
 
       SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
       auto R12 = Rot3::Quaternion(qw, qx, qy, qz);
-      if (corruptingNoise) {
-        Sampler sampler(corruptingNoise);
-        R12 = R12.retract(sampler.sample());
+      if (sampler) {
+        R12 = R12.retract(sampler->sample());
       }
 
       factors.emplace_back(new BetweenFactor<Pose3>(