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;
/// evaluateError
Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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