From 8692c74765b71acf4aeb6d081f953756094e8c12 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Mon, 9 Mar 2015 22:54:21 +0100 Subject: [PATCH] Fixed bug in ExpressionFactor::linearize() when using robust noise models --- gtsam/nonlinear/ExpressionFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 4769e5048..4e0467a3b 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -101,8 +101,8 @@ public: Ab(size()).col(0) = -traits::Local(measurement_, value); // Whiten the corresponding system, Ab already contains RHS - Vector dummy(Dim); - noiseModel_->WhitenSystem(Ab.matrix(), dummy); + Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models + noiseModel_->WhitenSystem(Ab.matrix(), b); return factor; }