diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index f0c70408e..a45da9789 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -61,6 +61,8 @@ Point2 CalibratedCamera::project(const Point3& point, const Rot3& R = pose.rotation(); const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); Point3 q = pose.transform_to(point); + if(q.z() <= 0) + throw CheiralityException(); if (D_intrinsic_pose || D_intrinsic_point) { double X = q.x(), Y = q.y(), Z = q.z(); diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 880bb9316..4538b3367 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -23,6 +23,11 @@ namespace gtsam { + class CheiralityException: public std::runtime_error { + public: + CheiralityException() : std::runtime_error("Cheirality Exception") {} + }; + /** * A Calibrated camera class [R|-R't], calibration K=I. * If calibration is known, it is more computationally efficient diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index ee5649f78..6940e367f 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -82,9 +82,18 @@ namespace gtsam { /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose, const Point3& point, boost::optional H1, boost::optional H2) const { - SimpleCamera camera(*K_, pose); - Point2 reprojectionError(camera.project(point, H1, H2) - z_); - return reprojectionError.vector(); + try { + SimpleCamera camera(*K_, pose); + Point2 reprojectionError(camera.project(point, H1, H2) - z_); + return reprojectionError.vector(); + } + catch( CheiralityException& e) { + if (H1) *H1 = zeros(2,6); + if (H2) *H2 = zeros(2,3); + cout << e.what() << ": Landmark "<< this->key2_.index() << + " moved behind camera " << this->key1_.index() << endl; + return zero(2); + } } /** return the measurement */