From b3d500f58c15e106b4771bb37b32c29a4cf12ad4 Mon Sep 17 00:00:00 2001
From: Vadim Indelman <indelman@gmail.com>
Date: Thu, 29 Aug 2013 22:43:09 +0000
Subject: [PATCH] Added optional flag_bump_up_near_zero_probs.

---
 gtsam_unstable/gtsam_unstable.h               |  6 +++-
 .../slam/TransformBtwRobotsUnaryFactorEM.h    | 30 ++++++++++---------
 2 files changed, 21 insertions(+), 15 deletions(-)

diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h
index 07c4f36bc..bee6bb697 100644
--- a/gtsam_unstable/gtsam_unstable.h
+++ b/gtsam_unstable/gtsam_unstable.h
@@ -316,6 +316,10 @@ virtual class BetweenFactorEM : gtsam::NonlinearFactor {
       const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier,
       double prior_inlier, double prior_outlier);
 
+  BetweenFactorEM(size_t key1, size_t key2, const T& relativePose,
+        const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier,
+        double prior_inlier, double prior_outlier,  bool flag_bump_up_near_zero_probs);
+
   Vector whitenedError(const gtsam::Values& x);
   Vector unwhitenedError(const gtsam::Values& x);
   Vector calcIndicatorProb(const gtsam::Values& x);
@@ -337,7 +341,7 @@ virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor {
   TransformBtwRobotsUnaryFactorEM(size_t key, const T& relativePose, size_t keyA, size_t keyB,
         const gtsam::Values& valA, const gtsam::Values& valB,
         const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier,
-        double prior_inlier, double prior_outlier, bool start_with_M_step);
+        double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs, bool start_with_M_step);
 
   Vector whitenedError(const gtsam::Values& x);
   Vector unwhitenedError(const gtsam::Values& x);
diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
index b91e696b9..5ff8f66e7 100644
--- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
+++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
@@ -79,10 +79,12 @@ namespace gtsam {
     TransformBtwRobotsUnaryFactorEM(Key key, const VALUE& measured, Key keyA, Key keyB,
         const gtsam::Values valA, const gtsam::Values valB,
         const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
-        const double prior_inlier, const double prior_outlier, const bool start_with_M_step = false) :
+        const double prior_inlier, const double prior_outlier,
+        const bool flag_bump_up_near_zero_probs = false,
+        const bool start_with_M_step = false) :
           Base(cref_list_of<1>(key)), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB),
           model_inlier_(model_inlier), model_outlier_(model_outlier),
-          prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(false),
+          prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(flag_bump_up_near_zero_probs),
           start_with_M_step_(false){
 
       setValAValB(valA, valB);
@@ -260,19 +262,19 @@ namespace gtsam {
       p_inlier  /= sumP;
       p_outlier /= sumP;
 
-      //      if (flag_bump_up_near_zero_probs_){
-      // Bump up near-zero probabilities (as in linerFlow.h)
-      double minP = 0.05; // == 0.1 / 2 indicator variables
-      if (p_inlier < minP || p_outlier < minP){
-        if (p_inlier < minP)
-          p_inlier = minP;
-        if (p_outlier < minP)
-          p_outlier = minP;
-        sumP = p_inlier + p_outlier;
-        p_inlier  /= sumP;
-        p_outlier /= sumP;
+      if (flag_bump_up_near_zero_probs_){
+        // Bump up near-zero probabilities (as in linerFlow.h)
+        double minP = 0.05; // == 0.1 / 2 indicator variables
+        if (p_inlier < minP || p_outlier < minP){
+          if (p_inlier < minP)
+            p_inlier = minP;
+          if (p_outlier < minP)
+            p_outlier = minP;
+          sumP = p_inlier + p_outlier;
+          p_inlier  /= sumP;
+          p_outlier /= sumP;
+        }
       }
-      //      }
 
       return Vector_(2, p_inlier, p_outlier);
     }