From f2a7864fb6465e7ba27fc06e9e7c9f2f005d7239 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 2 Jun 2020 23:52:38 -0400 Subject: [PATCH] Further optimization --- gtsam/linear/JacobianFactor.cpp | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index fcd517edd..8dfe46c49 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -558,13 +558,20 @@ void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const { Vector& dj = result.first->second; for (size_t k = 0; k < nj; ++k) { - Vector column_k = Ab_(pos).col(k); - if (model_) - model_->whitenInPlace(column_k); - if(!result.second) - dj(k) += dot(column_k, column_k); - else - dj(k) = dot(column_k, column_k); + Eigen::Ref column_k = Ab_(pos).col(k); + if (model_) { + Vector column_k_copy = column_k; + model_->whitenInPlace(column_k_copy); + if(!result.second) + dj(k) += dot(column_k_copy, column_k_copy); + else + dj(k) = dot(column_k_copy, column_k_copy); + } else { + if (!result.second) + dj(k) += dot(column_k, column_k); + else + dj(k) = dot(column_k, column_k); + } } } }