12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Simplified evaluateError

release/4.3a0
Frank dellaert 2020-10-12 15:37:24 -04:00
parent 5df3eebd2e
commit 4adca52daa
1 changed files with 6 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/slam/OrientedPlane3Factor.h
View File

@ -45,16 +45,13 @@ public:
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/// evaluateError /// evaluateError
Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, Vector evaluateError(
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = const Pose3& pose, const OrientedPlane3& plane,
boost::none) const override { boost::optional<Matrix&> H1 = boost::none,
OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, boost::optional<Matrix&> H2 = boost::none) const override {
H2); auto predicted_plane = OrientedPlane3::Transform(plane, pose, H1, H2);
Vector err(3); return predicted_plane.error(measured_p_);
err << predicted_plane.error(measured_p_);
return (err);
} }
;
}; };
// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior