diff --git a/gtsam/geometry/SphericalCamera.cpp b/gtsam/geometry/SphericalCamera.cpp index 1ff74741e..bc4fb2015 100644 --- a/gtsam/geometry/SphericalCamera.cpp +++ b/gtsam/geometry/SphericalCamera.cpp @@ -92,12 +92,24 @@ Unit3 SphericalCamera::project(const Point3& point, } /* ************************************************************************* */ -Vector2 SphericalCamera::reprojectionError(const Point3& point, const Unit3& measured, - OptionalJacobian<2, 6> Dpose, - OptionalJacobian<2, 3> Dpoint) const { - // on-manifold version of: camera.project(point) - zi - std::cout << "SphericalCam:reprojectionError fix jacobians " << std::endl; - return measured.localCoordinates( project2(point, Dpose, Dpoint) ); +Vector2 SphericalCamera::reprojectionError( + const Point3& point, const Unit3& measured, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 3> Dpoint) const { + // project point + if (Dpose || Dpoint) { + Matrix26 H_project_pose; + Matrix23 H_project_point; + Matrix22 H_error; + Unit3 projected = project2(point, H_project_pose, H_project_point); + Vector2 error = measured.errorVector(projected, boost::none, H_error); + if (Dpose) + *Dpose = H_error * H_project_pose; + if (Dpoint) + *Dpoint = H_error * H_project_point; + return error; + } else { + return measured.errorVector(project2(point, Dpose, Dpoint)); + } } /* ************************************************************************* */ diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index d97ef9df1..01b749df4 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -39,17 +39,6 @@ class GTSAM_EXPORT EmptyCal { } }; -// -//class GTSAM_EXPORT EmptyCal { -// public: -// EmptyCal(){} -// virtual ~EmptyCal() = default; -// using shared_ptr = boost::shared_ptr; -// void print(const std::string& s) const { -// std::cout << "empty calibration: " << s << std::endl; -// } -//}; - /** * A spherical camera class that has a Pose3 and measures bearing vectors * @addtogroup geometry diff --git a/gtsam/geometry/tests/testSphericalCamera.cpp b/gtsam/geometry/tests/testSphericalCamera.cpp index cd1886412..0e5e3d9bf 100644 --- a/gtsam/geometry/tests/testSphericalCamera.cpp +++ b/gtsam/geometry/tests/testSphericalCamera.cpp @@ -109,6 +109,39 @@ TEST( SphericalCamera, Dproject) EXPECT(assert_equal(numerical_point, Dpoint, 1e-7)); } +/* ************************************************************************* */ +static Vector2 reprojectionError2(const Pose3& pose, const Point3& point, const Unit3& measured) { + return Camera(pose).reprojectionError(point,measured); +} + +/* ************************************************************************* */ +TEST( SphericalCamera, reprojectionError) { + Matrix Dpose, Dpoint; + Vector2 result = camera.reprojectionError(point1, bearing1, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative31(reprojectionError2, pose, + point1, bearing1); + Matrix numerical_point = numericalDerivative32(reprojectionError2, pose, + point1, bearing1); + EXPECT(assert_equal(Vector2(0.0, 0.0), result)); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(numerical_point, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +TEST( SphericalCamera, reprojectionError_noisy) { + Matrix Dpose, Dpoint; + Unit3 bearing_noisy = bearing1.retract(Vector2(0.01, 0.05)); + Vector2 result = camera.reprojectionError(point1, bearing_noisy, Dpose, + Dpoint); + Matrix numerical_pose = numericalDerivative31(reprojectionError2, pose, + point1, bearing_noisy); + Matrix numerical_point = numericalDerivative32(reprojectionError2, pose, + point1, bearing_noisy); + EXPECT(assert_equal(Vector2(-0.050282, 0.00833482), result, 1e-5)); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(numerical_point, Dpoint, 1e-7)); +} + /* ************************************************************************* */ // Add a test with more arbitrary rotation TEST( SphericalCamera, Dproject2)