added Cheirality exception as in ProjectionFactor

gtsam/geometry/StereoCamera.cpp

@@ -39,6 +39,8 @@ namespace gtsam {
 		const Point3 q = leftCamPose_.transform_to(point);
 #endif
 
+		if ( q.z() <= 0 ) throw StereoCheiralityException();
+
 		// get calibration
 		const Cal3_S2Stereo& K = *K_;
 		const double fx = K.fx(), fy = K.fy(), b = K.baseline();

gtsam/geometry/StereoCamera.h

@@ -11,7 +11,7 @@
 
 /**
  * @file    StereoCamera.h
- * @brief   A Stereo Camera based on two Simple Cameras
+ * @brief   A Rectified Stereo Camera
  * @author  Chris Beall
  */
 
@@ -26,6 +26,12 @@
 
 namespace gtsam {
 
+class StereoCheiralityException: public std::runtime_error {
+public:
+  StereoCheiralityException() : std::runtime_error("Stereo Cheirality Exception") {}
+};
+
+
 /**
  * A stereo camera class, parameterize by left camera pose and stereo calibration
  * @addtogroup geometry

gtsam/slam/StereoFactor.h

@@ -88,8 +88,16 @@ public:
 	/** h(x)-z */
 	Vector evaluateError(const Pose3& pose, const Point3& point,
 			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-		StereoCamera stereoCam(pose, K_);
-		return (stereoCam.project(point, H1, H2) - measured_).vector();
+	  try {
+	    StereoCamera stereoCam(pose, K_);
+	    return (stereoCam.project(point, H1, H2) - measured_).vector();
+	  } catch(StereoCheiralityException& e) {
+      if (H1) *H1 = zeros(3,6);
+      if (H2) *H2 = zeros(3,3);
+      std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
+          " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
+	  }
+    return ones(3) * 2.0 * K_->fx();
 	}
 
 	/** return the measured */
@@ -102,7 +110,6 @@ public:
     return K_;
   }
 
-
 private:
 	/** Serialization function */
 	friend class boost::serialization::access;
@@ -115,5 +122,4 @@ private:
 	}
 };
 
-
-}
+} // \ namespace gtsam