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