30 using namespace std::placeholders;
32 using namespace gtsam;
35 static double fov = 60;
36 static size_t w = 640,
h = 480;
109 Pose3 body_P_sensor2(
110 Rot3::RzRyRx(0.0, 0.0, 0.0),
111 Point3(0.25, -0.10, 1.0));
195 Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0),
Point3(0, 0.2, 0.1));
230 Matrix H1Actual, H2Actual, H3Actual;
255 Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0),
Point3(0, 0.2, 0.1));
265 Matrix H1Actual, H2Actual, H3Actual;
293 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
296 bool throwCheirality =
true;
297 bool verboseCheirality =
true;
300 throwCheirality, verboseCheirality);
306 bool throwCheirality =
false;
307 bool verboseCheirality =
false;
310 throwCheirality, verboseCheirality);
313 Matrix H1Actual, H2Actual, H3Actual;
315 H2Actual, H3Actual));
318 Vector expectedError = Vector2::Constant(
337 Matrix H1Actual, H2Actual, H3Actual;
341 Matrix H1Expected = numericalDerivative31<Vector, Pose3, Pose3, Point3>(
343 std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
344 std::placeholders::_1, std::placeholders::_2,
345 std::placeholders::_3, {}, {},
349 Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
351 std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
352 std::placeholders::_1, std::placeholders::_2,
353 std::placeholders::_3, {}, {},
357 Matrix H3Expected = numericalDerivative33<Vector, Pose3, Pose3, Point3>(
359 std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor,
360 std::placeholders::_1, std::placeholders::_2,
361 std::placeholders::_3, {}, {},