Specify key involved when throwing cheirality exception

gtsam/geometry/CalibratedCamera.h

@@ -32,9 +32,17 @@ namespace gtsam {
 class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
     CheiralityException> {
 public:
-  CheiralityException() :
+  CheiralityException()
-      ThreadsafeException<CheiralityException>("Cheirality Exception") {
+    : CheiralityException(std::numeric_limits<Key>::max()) {}
-  }
+
+  CheiralityException(Key j)
+    : ThreadsafeException<CheiralityException>("CheiralityException"),
+      j_(j) {}
+
+  Key nearbyVariable() const {return j_;}
+
+private:
+  Key j_;
 };
 
 /**

gtsam/geometry/StereoCamera.h

@@ -25,9 +25,19 @@ namespace gtsam {
 
 class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
 public:
-  StereoCheiralityException() :
+  StereoCheiralityException()
-      std::runtime_error("Stereo Cheirality Exception") {
+    : StereoCheiralityException(std::numeric_limits<Key>::max()) {}
+
+  StereoCheiralityException(Key j)
+    : std::runtime_error("Stereo Cheirality Exception"),
+      j_(j) {}
+
+  Key nearbyVariable() const {
+    return j_;
   }
+
+private:
+  Key j_;
 };
 
 /**

gtsam/slam/ProjectionFactor.h

@@ -152,7 +152,7 @@ namespace gtsam {
           std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
               " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
         if (throwCheirality_)
-          throw e;
+          throw CheiralityException(this->key2());
       }
       return Vector2::Constant(2.0 * K_->fx());
     }

gtsam/slam/StereoFactor.h

@@ -144,7 +144,7 @@ public:
       std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
           " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
       if (throwCheirality_)
-        throw e;
+        throw StereoCheiralityException(this->key2());
     }
     return Vector3::Constant(2.0 * K_->fx());
   }