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

improve TransBtwRobotEM factor's preformance by removing redundant unwhitenedError calc

release/4.3a0
Jing Dong 2014-07-18 17:11:21 -04:00
parent e06c2cfacc
commit b5b1bbfba0
1 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -198,7 +198,7 @@ namespace gtsam {
Vector err = currA_T_currB_msr.localCoordinates(currA_T_currB_pred); Vector err = currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
// Calculate indicator probabilities (inlier and outlier) // 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_inlier = p_inlier_outlier[0];
double p_outlier = p_inlier_outlier[1]; double p_outlier = p_inlier_outlier[1];
@ -248,6 +248,12 @@ namespace gtsam {
Vector err = unwhitenedError(x); 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) // Calculate indicator probabilities (inlier and outlier)
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);