From 1f43d73b34ff9863ec2e5919a299297175aab4ec Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Mon, 16 Dec 2019 15:52:24 -0800 Subject: [PATCH] make jacobian factor use noise model distance when available --- gtsam/linear/JacobianFactor.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 9be010ff5..2310d88f0 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -513,8 +513,10 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const { /* ************************************************************************* */ double JacobianFactor::error(const VectorValues& c) const { - Vector weighted = error_vector(c); - return 0.5 * weighted.dot(weighted); + Vector e = unweighted_error(c); + // Use the noise model distance function to get the correct error if available. + if (model_) return 0.5 * model_->distance(e); + return 0.5 * e.dot(e); } /* ************************************************************************* */