diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 1d06e01a7..fc7c939a8 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -18,7 +18,9 @@ #pragma once #include -#include +#include +#include +#include #include namespace gtsam { @@ -189,7 +191,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3(pose1, pose2, t); + PinholeCamera 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( + std::function( + 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( + std::function( + 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( + std::function( + 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); } /* ************************************************************************* */