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

Added optional flag_bump_up_near_zero_probs.

release/4.3a0
Vadim Indelman 2013-08-29 22:43:09 +00:00
parent 86d21aaa05
commit b3d500f58c
2 changed files with 21 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam_unstable/gtsam_unstable.h
View File

@ -316,6 +316,10 @@ virtual class BetweenFactorEM : gtsam::NonlinearFactor {
const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier,
double prior_inlier, double prior_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 whitenedError(const gtsam::Values& x);
Vector unwhitenedError(const gtsam::Values& x); Vector unwhitenedError(const gtsam::Values& x);
Vector calcIndicatorProb(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, TransformBtwRobotsUnaryFactorEM(size_t key, const T& relativePose, size_t keyA, size_t keyB,
const gtsam::Values& valA, const gtsam::Values& valB, const gtsam::Values& valA, const gtsam::Values& valB,
const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, 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 whitenedError(const gtsam::Values& x);
Vector unwhitenedError(const gtsam::Values& x); Vector unwhitenedError(const gtsam::Values& x);

30
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -79,10 +79,12 @@ namespace gtsam {
TransformBtwRobotsUnaryFactorEM(Key key, const VALUE& measured, Key keyA, Key keyB, TransformBtwRobotsUnaryFactorEM(Key key, const VALUE& measured, Key keyA, Key keyB,
const gtsam::Values valA, const gtsam::Values valB, const gtsam::Values valA, const gtsam::Values valB,
const SharedGaussian& model_inlier, const SharedGaussian& model_outlier, 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), Base(cref_list_of<1>(key)), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB),
model_inlier_(model_inlier), model_outlier_(model_outlier), 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){ start_with_M_step_(false){
setValAValB(valA, valB); setValAValB(valA, valB);
@ -260,19 +262,19 @@ namespace gtsam {
p_inlier /= sumP; p_inlier /= sumP;
p_outlier /= sumP; p_outlier /= sumP;
// if (flag_bump_up_near_zero_probs_){ if (flag_bump_up_near_zero_probs_){
// Bump up near-zero probabilities (as in linerFlow.h) // Bump up near-zero probabilities (as in linerFlow.h)
double minP = 0.05; // == 0.1 / 2 indicator variables double minP = 0.05; // == 0.1 / 2 indicator variables
if (p_inlier < minP || p_outlier < minP){ if (p_inlier < minP || p_outlier < minP){
if (p_inlier < minP) if (p_inlier < minP)
p_inlier = minP; p_inlier = minP;
if (p_outlier < minP) if (p_outlier < minP)
p_outlier = minP; p_outlier = minP;
sumP = p_inlier + p_outlier; sumP = p_inlier + p_outlier;
p_inlier /= sumP; p_inlier /= sumP;
p_outlier /= sumP; p_outlier /= sumP;
}
} }
// }
return Vector_(2, p_inlier, p_outlier); return Vector_(2, p_inlier, p_outlier);
} }