diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 5c6646454..c34d4d8f3 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -39,7 +39,10 @@ namespace gtsam { const Point3 q = leftCamPose_.transform_to(point); - if ( q.z() <= 0 ) throw StereoCheiralityException(); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (q.z() <= 0) + throw StereoCheiralityException(); +#endif // get calibration const Cal3_S2Stereo& K = *K_; diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 497599a6a..f76d841bf 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -104,6 +104,20 @@ TEST( StereoCamera, Dproject) CHECK(assert_equal(expected2,actual2,1e-7)); } +/* ************************************************************************* */ +TEST( StereoCamera, projectCheirality) +{ + // create a Stereo camera + Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); + StereoCamera stereoCam(Pose3(), K); + + // point behind the camera + Point3 p(0, 0, -5); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + CHECK_EXCEPTION(stereoCam.project2(p), StereoCheiralityException); +#endif +} + /* ************************************************************************* */ TEST( StereoCamera, backproject_case1) {