12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added flag_bump_up_near_zero_probs_.

release/4.3a0
Vadim Indelman 2013-08-23 15:22:50 +00:00
parent ca3956adc1
commit f6402916d6
1 changed files with 36 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -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 {