From b7584ce362fd30fe498292f6c32754983a0cc34f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 5 Jan 2021 10:44:26 -0500
Subject: [PATCH] verbose printing of exceptions

---
 gtsam/slam/GeneralSFMFactor.h                 | 24 +++++++++++++------
 .../examples/SmartRangeExample_plaza1.cpp     |  3 ++-
 2 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h
index f848a56ca..3e292c892 100644
--- a/gtsam/slam/GeneralSFMFactor.h
+++ b/gtsam/slam/GeneralSFMFactor.h
@@ -70,6 +70,7 @@ class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
 protected:
 
   Point2 measured_; ///< the 2D measurement
+  bool verbose_; ///< Flag for print verbosity
 
 public:
 
@@ -86,12 +87,17 @@ public:
    * @param cameraKey is the index of the camera
    * @param landmarkKey is the index of the landmark
    */
-  GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) :
-  Base(model, cameraKey, landmarkKey), measured_(measured) {}
+  GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model,
+                   Key cameraKey, Key landmarkKey, bool verbose = false)
+      : Base(model, cameraKey, landmarkKey),
+        measured_(measured),
+        verbose_(verbose) {}
 
-  GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor
-  GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2
-  GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2
+  GeneralSFMFactor() : measured_(0.0, 0.0) {}  ///< default constructor
+  ///< constructor that takes a Point2
+  GeneralSFMFactor(const Point2& p) : measured_(p) {}
+  ///< constructor that takes doubles x,y to make a Point2
+  GeneralSFMFactor(double x, double y) : measured_(x, y) {}
 
   virtual ~GeneralSFMFactor() {} ///< destructor
 
@@ -127,7 +133,9 @@ public:
     catch( CheiralityException& e) {
       if (H1) *H1 = JacobianC::Zero();
       if (H2) *H2 = JacobianL::Zero();
-      // TODO warn if verbose output asked for
+      if (verbose_) {
+        std::cout << e.what() << std::endl;
+      }
       return Z_2x1;
     }
   }
@@ -149,7 +157,9 @@ public:
       H1.setZero();
       H2.setZero();
       b.setZero();
-      // TODO warn if verbose output asked for
+      if (verbose_) {
+        std::cout << e.what() << std::endl;
+      }
     }
 
     // Whiten the system if needed
diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
index 5fdc7a743..0976674de 100644
--- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
+++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
@@ -188,7 +188,8 @@ int main(int argc, char** argv) {
             smartFactors[j]->addRange(i, range);
             printf("adding range %g for %d",range,(int)j);
           } catch (const invalid_argument& e) {
-            printf("warning: omitting duplicate range %g for %d",range,(int)j);
+            printf("warning: omitting duplicate range %g for %d: %s", range,
+                   (int)j, e.what());
           }
           cout << endl;
         }