From 2af48952b2760c103d21459a28d5e8a5cfc25301 Mon Sep 17 00:00:00 2001
From: Toni Rosinol <tonirosinol@hotmail.com>
Date: Thu, 24 May 2018 18:05:57 -0400
Subject: [PATCH] Specify key involved when throwing cheirality exception

---
 gtsam/geometry/CalibratedCamera.h | 14 +++++++++++---
 gtsam/geometry/StereoCamera.h     | 14 ++++++++++++--
 gtsam/slam/ProjectionFactor.h     |  2 +-
 gtsam/slam/StereoFactor.h         |  2 +-
 4 files changed, 25 insertions(+), 7 deletions(-)

diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index f1fa509c1..9bd2f3f2e 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -32,9 +32,17 @@ namespace gtsam {
 class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
     CheiralityException> {
 public:
-  CheiralityException() :
-      ThreadsafeException<CheiralityException>("Cheirality Exception") {
-  }
+  CheiralityException()
+    : CheiralityException(std::numeric_limits<Key>::max()) {}
+
+  CheiralityException(Key j)
+    : ThreadsafeException<CheiralityException>("CheiralityException"),
+      j_(j) {}
+
+  Key nearbyVariable() const {return j_;}
+
+private:
+  Key j_;
 };
 
 /**
diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h
index ac32be7ae..0723aaec5 100644
--- a/gtsam/geometry/StereoCamera.h
+++ b/gtsam/geometry/StereoCamera.h
@@ -25,9 +25,19 @@ namespace gtsam {
 
 class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
 public:
-  StereoCheiralityException() :
-      std::runtime_error("Stereo Cheirality Exception") {
+  StereoCheiralityException()
+    : 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_;
 };
 
 /**
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index d13c28e11..aa04bf7d7 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/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());
     }
diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
index 59fc372cb..8828f5de7 100644
--- a/gtsam/slam/StereoFactor.h
+++ b/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());
   }