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

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;
}
};