diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index c6eca2780..1fe86485b 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/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(); diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 140ab1bdf..b178b9b60 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/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 diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 87cb95d25..b00a2c499 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -88,8 +88,16 @@ public: /** h(x)-z */ Vector evaluateError(const Pose3& pose, const Point3& point, boost::optional H1, boost::optional 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