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,
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);

10
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -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,7 +262,7 @@ namespace gtsam {
p_inlier /= 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)
double minP = 0.05; // == 0.1 / 2 indicator variables
if (p_inlier < minP || p_outlier < minP){
@ -272,7 +274,7 @@ namespace gtsam {
p_inlier /= sumP;
p_outlier /= sumP;
}
// }
}
return Vector_(2, p_inlier, p_outlier);
}