diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index bf6d20c45..067d30329 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -204,11 +204,15 @@ public: Gs, gs, 0.0); } + // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). std::vector Fblocks; Matrix E; Vector b; computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + // Whiten using noise model + Base::whitenJacobians(Fblocks, E, b); + // build augmented hessian SymmetricBlockMatrix augmentedHessian = // Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping);