diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 0630392b7..f7d550390 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -447,7 +447,7 @@ namespace gtsam { size_t nj = Ab_(pos).cols(); Vector dj(nj); for (size_t k = 0; k < nj; ++k) { - Vector column_k = Ab_(pos).col(k); + Vector column_k = model_->whiten(Ab_(pos).col(k)); dj(k) = dot(column_k,column_k); } d.insert(j,dj);