added flag_bump_up_near_zero_probs_.
							parent
							
								
									ca3956adc1
								
							
						
					
					
						commit
						f6402916d6
					
				|  | @ -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 { | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue