From d64af0d626f7a44c59b65b5603cb3d7ce61365c2 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Sat, 13 Dec 2014 17:02:38 -0500 Subject: [PATCH] Modify gradientAtZero in RegularJacobianFactor --- gtsam/slam/RegularJacobianFactor.h | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index a9759cc26..e6e240389 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -143,19 +143,30 @@ public: /** Raw memory access version of gradientAtZero */ virtual void gradientAtZero(double* d) const { - // Get vector b not weighted - Vector b = getb(); - Vector b_sigma = model_ ? (model_->whiten(b)*model_->whiten(b)) : b; - // Just iterate over all A matrices + // Initialize d as a zero vector for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { DMap(d + D * j) = DVector::Zero(); + } + + // Get vector b not weighted + Vector b = getb(); + + // Whitening b + if (model_) { + b = model_->whiten(b); + b = model_->whiten(b); + } + + // Just iterate over all A matrices + for (size_t pos = 0; pos < size(); ++pos) { + Key j = keys_[pos]; DVector dj; // gradient -= A'*b/sigma^2 // Computing with each column for (size_t k = 0; k < D; ++k){ - Vector column_k = Ab_(j).col(k); - dj(k) = -1.0*dot(b_sigma,column_k); + Vector column_k = Ab_(pos).col(k); + dj(k) = -1.0*dot(b, column_k); } DMap(d + D * j) += dj; }