diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index f37d02312..8d3777d8c 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -216,7 +216,10 @@ struct ISAM2Clique : public BayesTreeCliqueBaseget_R().cols() + result.first->get_S().cols()) { // Compute gradient contribution const ConditionalType& conditional(*result.first); - gradientContribution_ << -(conditional.get_R() * conditional.permutation().transpose()).transpose() * conditional.get_d(), + // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons +// gradientContribution_ << -(conditional.get_R() * conditional.permutation().transpose()).transpose() * conditional.get_d(), +// -conditional.get_S().transpose() * conditional.get_d(); + gradientContribution_ << -(conditional.get_d().transpose() * conditional.get_R() * conditional.permutation().transpose()).transpose(), -conditional.get_S().transpose() * conditional.get_d(); }