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

Slight re-factor

release/4.3a0
dellaert 2014-11-02 14:42:59 +01:00
parent d2f56b13ed
commit 17d352bab4
1 changed files with 4 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -128,13 +128,12 @@ public:
// Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, map); // <<< Reverse AD happens here !
Vector b(-measurement_.localCoordinates(value));
Ab(size()).col(0) = -measurement_.localCoordinates(value);
// Whiten the corresponding system
// Note the Ab.matrix() includes uninitialized RHS, but this beats taking it apart
noiseModel_->WhitenSystem(Ab.matrix(),b);
// Whiten the corresponding system, Ab already contains RHS
Vector dummy(Dim);
noiseModel_->WhitenSystem(Ab.matrix(),dummy);
Ab(size()).col(0) = b;
return factor;
}
};