diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 0f3fac53e..b1e265266 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -118,7 +118,7 @@ TEST( CalibratedCamera, Dproject_point_pose) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject_point_pose2) { - static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, 0.5)); + static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const CalibratedCamera camera(pose1); Matrix Dpose, Dpoint; camera.project(point1, Dpose, Dpoint); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 11c95d822..0e610d8d6 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -238,6 +238,20 @@ TEST( PinholeCamera, Dproject2) EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); } +/* ************************************************************************* */ +// Add a test with more arbitrary rotation +TEST( PinholeCamera, Dproject3) +{ + static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Camera camera(pose1); + Matrix Dpose, Dpoint; + camera.project2(point1, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(project4, camera, point1); + Matrix numerical_point = numericalDerivative22(project4, camera, point1); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + /* ************************************************************************* */ static double range0(const Camera& camera, const Point3& point) { return camera.range(point); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index f0c364955..411273c1f 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -172,7 +172,7 @@ TEST( PinholePose, Dproject2) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject3) { - static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, 0.5)); + static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const Camera camera(pose1); Matrix Dpose, Dpoint; camera.project2(point1, Dpose, Dpoint);