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