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

unwhitenedError.

release/4.3a0
Vadim Indelman 2013-08-08 20:14:12 +00:00
parent 27cabb62f1
commit b0c96f9bf6
1 changed files with 17 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -223,15 +223,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const { gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const {
const T& p1 = x.at<T>(key1_); Vector err = unwhitenedError(x);
const T& p2 = x.at<T>(key2_);
Matrix H1, H2;
T hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
Vector err = measured_.localCoordinates(hx);
// 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);
@ -262,6 +254,22 @@ namespace gtsam {
return Vector_(2, p_inlier, p_outlier); return Vector_(2, p_inlier, p_outlier);
} }
/* ************************************************************************* */
gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
bool debug = true;
const T& p1 = x.at<T>(key1_);
const T& p2 = x.at<T>(key2_);
Matrix H1, H2;
T hx = p1.between(p2, H1, H2); // h(x)
return measured_.localCoordinates(hx);
}
/* ************************************************************************* */
/** return the measured */ /** return the measured */
const VALUE& measured() const { const VALUE& measured() const {
return measured_; return measured_;