From b5b1bbfba0d206dbd250e0d3467587cda6ab9f6b Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Fri, 18 Jul 2014 17:11:21 -0400 Subject: [PATCH] improve TransBtwRobotEM factor's preformance by removing redundant unwhitenedError calc --- gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index 7a837b1b5..48071f355 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -198,7 +198,7 @@ namespace gtsam { Vector err = currA_T_currB_msr.localCoordinates(currA_T_currB_pred); // Calculate indicator probabilities (inlier and outlier) - Vector p_inlier_outlier = calcIndicatorProb(x); + Vector p_inlier_outlier = calcIndicatorProb(x, err); double p_inlier = p_inlier_outlier[0]; double p_outlier = p_inlier_outlier[1]; @@ -248,6 +248,12 @@ namespace gtsam { Vector err = unwhitenedError(x); + return this->calcIndicatorProb(x, err); + } + + /* ************************************************************************* */ + gtsam::Vector calcIndicatorProb(const gtsam::Values& x, const gtsam::Vector& err) const { + // Calculate indicator probabilities (inlier and outlier) Vector err_wh_inlier = model_inlier_->whiten(err); Vector err_wh_outlier = model_outlier_->whiten(err);