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

Update style and switch to errorVector()

release/4.3a0
David 2020-06-24 09:32:24 +10:00
parent 58a0f82cba
commit 795380d5d7
1 changed files with 13 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -23,14 +23,11 @@ void OrientedPlane3Factor::print(const string& s,
Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
const OrientedPlane3& plane, boost::optional<Matrix&> H1, const OrientedPlane3& plane, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {
Vector err(3);
if (H1 || H2) {
Matrix36 predicted_H_pose; Matrix36 predicted_H_pose;
Matrix33 predicted_H_plane, error_H_predicted; Matrix33 predicted_H_plane, error_H_predicted;
OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr,
OrientedPlane3 predicted_plane = plane.transform(pose, predicted_H_plane, predicted_H_pose); H1 ? &predicted_H_pose : nullptr);
err << predicted_plane.error(measured_p_, error_H_predicted); Vector3 err = predicted_plane.errorVector(measured_p_, (H1 || H2) ? &error_H_predicted : nullptr);
// Apply the chain rule to calculate the derivatives. // Apply the chain rule to calculate the derivatives.
if (H1) { if (H1) {
@ -39,11 +36,8 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
if (H2) { if (H2) {
*H2 = error_H_predicted * predicted_H_plane; *H2 = error_H_predicted * predicted_H_plane;
} }
} else {
OrientedPlane3 predicted_plane = plane.transform(pose); return err;
err << predicted_plane.error(measured_p_);
}
return (err);
} }
//*************************************************************************** //***************************************************************************