From 17d352bab4cf5784d502c7865acdc96c40bcbc41 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 14:42:59 +0100 Subject: [PATCH] Slight re-factor --- gtsam_unstable/nonlinear/ExpressionFactor.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index f7b61120a..37a89af6b 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -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; } };