From 87a7c56c88edd4f5ea1ed41f616e7c2aeec4d535 Mon Sep 17 00:00:00 2001 From: Vadim Indelman Date: Mon, 12 Aug 2013 16:11:48 +0000 Subject: [PATCH] Switched to square root of covariance norm. --- gtsam_unstable/slam/BetweenFactorEM.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 5cfefca0d..9c3736387 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -232,8 +232,8 @@ namespace gtsam { Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R(); Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R(); - double p_inlier = prior_inlier_ * invCov_inlier.norm() * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) ); - double p_outlier = prior_outlier_ * invCov_outlier.norm() * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) ); + double p_inlier = prior_inlier_ * sqrt(invCov_inlier.norm()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) ); + double p_outlier = prior_outlier_ * sqrt(invCov_outlier.norm()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) ); double sumP = p_inlier + p_outlier; p_inlier /= sumP;