diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h
index 279ffa6a5..8c8fa9310 100644
--- a/gtsam_unstable/slam/BetweenFactorEM.h
+++ b/gtsam_unstable/slam/BetweenFactorEM.h
@@ -52,6 +52,8 @@ namespace gtsam {
     double prior_inlier_;
     double prior_outlier_;
 
+    bool flag_bump_up_near_zero_probs_;
+
     /** concept check by type */
     GTSAM_CONCEPT_LIE_TYPE(T)
     GTSAM_CONCEPT_TESTABLE_TYPE(T)
@@ -70,7 +72,7 @@ namespace gtsam {
         const double prior_inlier, const double prior_outlier) :
           Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(measured),
           model_inlier_(model_inlier), model_outlier_(model_outlier),
-          prior_inlier_(prior_inlier), prior_outlier_(prior_outlier){
+          prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(false){
     }
 
     virtual ~BetweenFactorEM() {}
@@ -223,6 +225,8 @@ namespace gtsam {
     /* ************************************************************************* */
     gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const {
 
+      bool debug = false;
+
       Vector err =  unwhitenedError(x);
 
       // Calculate indicator probabilities (inlier and outlier)
@@ -235,20 +239,31 @@ namespace gtsam {
       double p_inlier  = prior_inlier_ * std::sqrt(invCov_inlier.norm()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) );
       double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.norm()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) );
 
+      if (debug){
+        std::cout<<"in calcIndicatorProb. err_unwh: "<<err[0]<<", "<<err[1]<<", "<<err[2]<<std::endl;
+        std::cout<<"in calcIndicatorProb. err_wh_inlier: "<<err_wh_inlier[0]<<", "<<err_wh_inlier[1]<<", "<<err_wh_inlier[2]<<std::endl;
+        std::cout<<"in calcIndicatorProb. err_wh_inlier.dot(err_wh_inlier): "<<err_wh_inlier.dot(err_wh_inlier)<<std::endl;
+        std::cout<<"in calcIndicatorProb. err_wh_outlier.dot(err_wh_outlier): "<<err_wh_outlier.dot(err_wh_outlier)<<std::endl;
+
+        std::cout<<"in calcIndicatorProb. p_inlier, p_outlier before normalization: "<<p_inlier<<", "<<p_outlier<<std::endl;
+      }
+
       double sumP = p_inlier + p_outlier;
       p_inlier  /= sumP;
       p_outlier /= sumP;
 
-      // 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);
@@ -257,8 +272,6 @@ namespace gtsam {
     /* ************************************************************************* */
     gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
 
-      bool debug = true;
-
       const T& p1 = x.at<T>(key1_);
       const T& p2 = x.at<T>(key2_);
 
@@ -269,6 +282,16 @@ namespace gtsam {
       return measured_.localCoordinates(hx);
     }
 
+    /* ************************************************************************* */
+    void set_flag_bump_up_near_zero_probs(bool flag) {
+      flag_bump_up_near_zero_probs_ = flag;
+    }
+
+    /* ************************************************************************* */
+    bool get_flag_bump_up_near_zero_probs() const {
+      return flag_bump_up_near_zero_probs_;
+    }
+
     /* ************************************************************************* */
     /** return the measured */
     const VALUE& measured() const {