From 5db6894b66d387c051ba593c43c3b013482349d0 Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Fri, 27 Nov 2020 18:25:38 -0500
Subject: [PATCH] finally I have a way to properly change the noise model!

---
 gtsam/nonlinear/NonlinearFactor.cpp |  8 ++++++++
 gtsam/nonlinear/NonlinearFactor.h   |  6 ++++++
 tests/smallExample.h                | 15 +++++++++++++++
 tests/testNonlinearFactor.cpp       | 20 ++++++++++++++++++++
 4 files changed, 49 insertions(+)

diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp
index 1cfcba274..8b8d2da6c 100644
--- a/gtsam/nonlinear/NonlinearFactor.cpp
+++ b/gtsam/nonlinear/NonlinearFactor.cpp
@@ -76,6 +76,14 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
               && noiseModel_->equals(*e->noiseModel_, tol)));
 }
 
+/* ************************************************************************* */
+NoiseModelFactor::shared_ptr NoiseModelFactor::cloneWithNewNoiseModel(
+    const SharedNoiseModel newNoise) const {
+  NoiseModelFactor::shared_ptr new_factor = boost::dynamic_pointer_cast<NoiseModelFactor>(clone());
+  new_factor->noiseModel_ = newNoise;
+  return new_factor;
+}
+
 /* ************************************************************************* */
 static void check(const SharedNoiseModel& noiseModel, size_t m) {
   if (noiseModel && m != noiseModel->dim())
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 80fbfbb11..00311fb87 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -244,6 +244,12 @@ public:
    */
   boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
 
+  /**
+   * Creates a shared_ptr clone of the
+   * factor with a new noise model
+   */
+  shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const;
+
  private:
   /** Serialization function */
   friend class boost::serialization::access;
diff --git a/tests/smallExample.h b/tests/smallExample.h
index 70cda1eb0..944899e70 100644
--- a/tests/smallExample.h
+++ b/tests/smallExample.h
@@ -342,10 +342,25 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> {
     return (h(x) - z_);
   }
 
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+        return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+            gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); }
 };
 
 }
 
+/* ************************************************************************* */
+inline NonlinearFactorGraph nonlinearFactorGraphWithGivenSigma(const double sigma) {
+  using symbol_shorthand::X;
+  using symbol_shorthand::L;
+  boost::shared_ptr<NonlinearFactorGraph> fg(new NonlinearFactorGraph);
+  Point2 z(1.0, 0.0);
+  boost::shared_ptr<smallOptimize::UnaryFactor> factor(
+      new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
+  fg->push_back(factor);
+  return *fg;
+}
+
 /* ************************************************************************* */
 inline boost::shared_ptr<const NonlinearFactorGraph> sharedReallyNonlinearFactorGraph() {
   using symbol_shorthand::X;
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index 662b071df..84bba850b 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -233,6 +233,26 @@ TEST( NonlinearFactor, linearize_constraint2 )
   CHECK(assert_equal((const GaussianFactor&)expected, *actual));
 }
 
+/* ************************************************************************* */
+TEST( NonlinearFactor, cloneWithNewNoiseModel )
+{
+  // create original factor
+  double sigma1 = 0.1;
+  NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma(sigma1);
+
+  // create expected
+  double sigma2 = 10;
+  NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma(sigma2);
+
+  // create actual
+  NonlinearFactorGraph actual;
+  SharedNoiseModel noise2 = noiseModel::Isotropic::Sigma(2,sigma2);
+  actual.push_back( boost::dynamic_pointer_cast<NoiseModelFactor>(nfg[0])->cloneWithNewNoiseModel(noise2) );
+
+  // check it's all good
+  CHECK(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 class TestFactor4 : public NoiseModelFactor4<double, double, double, double> {
 public: