added nice test on cheirality exception - done with projectionFactorRollingShutter

gtsam_unstable/slam/ProjectionFactorRollingShutter.h

@@ -18,7 +18,9 @@
 #pragma once
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <boost/optional.hpp>
 
 namespace gtsam {

gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp

@@ -290,6 +290,85 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) {
   CHECK(assert_equal(H3Expected, H3Actual, 1e-5));
 }
 
+/* ************************************************************************* */
+TEST( ProjectionFactorRollingShutter, cheirality ) {
+  // Create measurement by projecting 3D landmark behind camera
+  double t = 0.3;
+  Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0));
+  Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1));
+  Pose3 poseInterp = interpolate<Pose3>(pose1, pose2, t);
+  PinholeCamera<Cal3_S2> camera(poseInterp, *K);
+  Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera
+
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+  Point2 measured = Point2(0.0,0.0); // project would throw an exception
+  { // check that exception is thrown if we set throwCheirality = true
+    bool throwCheirality = true;
+    bool verboseCheirality = true;
+    ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality);
+    CHECK_EXCEPTION(factor.evaluateError(pose1, pose2, point),
+                    CheiralityException);
+  }
+  { // check that exception is NOT thrown if we set throwCheirality = false, and outputs are correct
+    bool throwCheirality = false; // default
+    bool verboseCheirality = false; // default
+    ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality);
+
+    // Use the factor to calculate the error
+    Matrix H1Actual, H2Actual, H3Actual;
+    Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual));
+
+    // The expected error is zero
+    Vector expectedError = Vector2::Constant(2.0 * K->fx()); // this is what we return when point is behind camera
+
+    // Verify we get the expected error
+    CHECK(assert_equal(expectedError, actualError, 1e-9));
+    CHECK(assert_equal(Matrix::Zero(2,6), H1Actual, 1e-5));
+    CHECK(assert_equal(Matrix::Zero(2,6), H2Actual, 1e-5));
+    CHECK(assert_equal(Matrix::Zero(2,3), H3Actual, 1e-5));
+  }
+#else
+  {
+    // everything is well defined, hence this matches the test "Jacobian" above:
+    Point2 measured = camera.project(point);
+
+    // create factor
+    ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K);
+
+    // Use the factor to calculate the Jacobians
+    Matrix H1Actual, H2Actual, H3Actual;
+    factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual);
+
+    // Expected Jacobians via numerical derivatives
+    Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>(
+        std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
+            std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
+                      std::placeholders::_1, std::placeholders::_2,
+                      std::placeholders::_3, boost::none, boost::none, boost::none)),
+                      pose1, pose2, point);
+
+    Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
+        std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
+            std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
+                      std::placeholders::_1, std::placeholders::_2,
+                      std::placeholders::_3, boost::none, boost::none, boost::none)),
+                      pose1, pose2, point);
+
+    Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
+        std::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
+            std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
+                      std::placeholders::_1, std::placeholders::_2,
+                      std::placeholders::_3, boost::none, boost::none, boost::none)),
+                      pose1, pose2, point);
+
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-5));
+    CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
+    CHECK(assert_equal(H3Expected, H3Actual, 1e-5));
+  }
+#endif
+}
+
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */