Update print method of OrientedPlane3Factor

borglab · Feb 15, 2021 · 0ad488c · 0ad488c
1 parent a62bdd4
commit 0ad488c
Showing 1 changed file with 8 additions and 4 deletions.
diff --git 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<Matrix&> 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: ");
 }