First iteration of GTSAM formula