fixed reproj error jacobians and added solid tests

gtsam/geometry/SphericalCamera.cpp

@@ -92,12 +92,24 @@ Unit3 SphericalCamera::project(const Point3& point,
 }
 
 /* ************************************************************************* */
-Vector2 SphericalCamera::reprojectionError(const Point3& point, const Unit3& measured,
+Vector2 SphericalCamera::reprojectionError(
-                                           OptionalJacobian<2, 6> Dpose,
+    const Point3& point, const Unit3& measured, OptionalJacobian<2, 6> Dpose,
     OptionalJacobian<2, 3> Dpoint) const {
-  // on-manifold version of: camera.project(point) - zi
+  // project point
-  std::cout << "SphericalCam:reprojectionError fix jacobians " << std::endl;
+  if (Dpose || Dpoint) {
-  return measured.localCoordinates( project2(point, 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));
+  }
 }
 
 /* ************************************************************************* */

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<EmptyCal>;
-//  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

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)