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

Added option to start with an M step.

release/4.3a0
Vadim Indelman 2013-08-28 16:56:57 +00:00
parent 0422b9cfef
commit df36ee643a
2 changed files with 16 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam_unstable/gtsam_unstable.h
View File

@ -334,6 +334,11 @@ virtual class TransformBtwRobotsUnaryFactorEM : 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);
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);
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);

13
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -61,6 +61,7 @@ namespace gtsam {
double prior_outlier_; double prior_outlier_;
bool flag_bump_up_near_zero_probs_; bool flag_bump_up_near_zero_probs_;
mutable bool start_with_M_step_;
/** concept check by type */ /** concept check by type */
GTSAM_CONCEPT_LIE_TYPE(T) GTSAM_CONCEPT_LIE_TYPE(T)
@ -78,10 +79,11 @@ 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 double prior_inlier, const double prior_outlier, 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_(false),
start_with_M_step_(false){
setValAValB(valA, valB); setValAValB(valA, valB);
@ -198,6 +200,13 @@ namespace gtsam {
double p_inlier = p_inlier_outlier[0]; double p_inlier = p_inlier_outlier[0];
double p_outlier = p_inlier_outlier[1]; double p_outlier = p_inlier_outlier[1];
if (start_with_M_step_){
start_with_M_step_ = false;
p_inlier = 0.5;
p_outlier = 0.5;
}
Vector err_wh_inlier = model_inlier_->whiten(err); Vector err_wh_inlier = model_inlier_->whiten(err);
Vector err_wh_outlier = model_outlier_->whiten(err); Vector err_wh_outlier = model_outlier_->whiten(err);