diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 417ba1354..59a267278 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -487,6 +487,15 @@ public: updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... } + /// Whiten the Jacobians computed by computeJacobians using noiseModel_ + void whitenJacobians(std::vector& F, Matrix& E, + Vector& b) const { + noiseModel_->WhitenSystem(E, b); + // TODO make WhitenInPlace work with any dense matrix type + BOOST_FOREACH(KeyMatrix2D& Fblock,F) + Fblock.second = noiseModel_->Whiten(Fblock.second); + } + /** * Return Jacobians as RegularImplicitSchurFactor with raw access */ @@ -497,11 +506,8 @@ public: Matrix E; Vector b; computeJacobians(F, E, b, cameras, point); - noiseModel_->WhitenSystem(E, b); + whitenJacobians(F, E, b); Matrix3 P = PointCov(E, lambda, diagonalDamping); - // TODO make WhitenInPlace work with any dense matrix type - BOOST_FOREACH(KeyMatrix2D& Fblock,F) - Fblock.second = noiseModel_->Whiten(Fblock.second); return boost::make_shared >(F, E, P, b); } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 4c0d1f4a2..51f892a6d 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -280,6 +280,7 @@ public: { std::vector Fblocks; f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + Base::whitenJacobians(Fblocks,E,b); Base::FillDiagonalF(Fblocks, F); // expensive ! }