diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 0b134c401..65036edfe 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -426,7 +426,7 @@ public: Matrix3 P = Base::PointCov(E, lambda); H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); + gs_vector.noalias() = - F.transpose() * (b - (E * (P * (E.transpose() * b)))); if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl;