From 4adca52daa1885b5bde0b14013ca3416c63f805a Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Mon, 12 Oct 2020 15:37:24 -0400 Subject: [PATCH] Simplified evaluateError --- gtsam/slam/OrientedPlane3Factor.h | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index e83464b1e..7abf5547b 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -45,16 +45,13 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /// evaluateError - Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const override { - OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, - H2); - Vector err(3); - err << predicted_plane.error(measured_p_); - return (err); + Vector evaluateError( + const Pose3& pose, const OrientedPlane3& plane, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + auto predicted_plane = OrientedPlane3::Transform(plane, pose, H1, H2); + return predicted_plane.error(measured_p_); } - ; }; // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior