diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 918831a308..185fae73f7 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -14,7 +14,9 @@ namespace gtsam { //*************************************************************************** void OrientedPlane3Factor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << "OrientedPlane3Factor Factor on " << keyFormatter(key2()) << "\n"; + cout << s << (s == "" ? "" : "\n"); + cout << "OrientedPlane3Factor Factor (" << keyFormatter(key1()) << ", " + << keyFormatter(key2()) << ")\n"; measured_p_.print("Measured Plane"); this->noiseModel_->print(" noise model: "); } @@ -25,8 +27,9 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, boost::optional H2) const { Matrix36 predicted_H_pose; Matrix33 predicted_H_plane, error_H_predicted; - OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr, - H1 ? &predicted_H_pose : nullptr); + 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); @@ -44,7 +47,8 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose, //*************************************************************************** void OrientedPlane3DirectionPrior::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << "Prior Factor on " << keyFormatter(key()) << "\n"; + cout << s << (s == "" ? "" : "\n"); + cout << s << "Prior Factor on " << keyFormatter(key()) << "\n"; measured_p_.print("Measured Plane"); this->noiseModel_->print(" noise model: "); }