create a backproject2, with optional Jacobians

gtsam/geometry/StereoCamera.cpp

@@ -95,4 +95,27 @@ namespace gtsam {
     return world_point;
   }
 
+  /* ************************************************************************* */
+  Point3 StereoCamera::backproject2(const StereoPoint2& z, OptionalJacobian<3, 6> H1,
+                                    OptionalJacobian<3, 3> H2) {
+    const Cal3_S2Stereo& K = *K_;
+    const double fx = K.fx(), fy = K.fy(), cx = K.cx(), cy = K.cy(), b = K.baseline();
+
+    Vector measured = z.vector();
+    double Z = b * fx / (measured[0] - measured[1]);
+    double X = Z * (measured[0] - cx) / fx;
+    double Y = Z * (measured[2] - cy) / fy;
+
+    if(H1 || H2) {
+      if(H1) {
+        // do something here
+      }
+      if(H2) {
+
+      }
+    }
+
+    return leftCamPose_.transform_from(Point3(X, Y, Z));
+  }
+
 }

gtsam/geometry/StereoCamera.h

@@ -137,6 +137,14 @@ public:
   /// back-project a measurement
   Point3 backproject(const StereoPoint2& z) const;
 
+  /** Back-project the 2D point and compute optional derivatives
+   * @param H1 derivative with respect to pose
+   * @param H2 derivative with respect to point
+   */
+  Point3 backproject2(const StereoPoint2& z,
+                      OptionalJacobian<3, 6> H1 = boost::none,
+                      OptionalJacobian<3, 3> H2 = boost::none) const;
+
   /// @}
   /// @name Deprecated
   /// @{

gtsam/geometry/tests/testStereoCamera.cpp

@@ -105,7 +105,7 @@ TEST( StereoCamera, Dproject)
 }
 
 /* ************************************************************************* */
-TEST( StereoCamera, backproject)
+TEST( StereoCamera, backproject_case1)
 {
   Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
   StereoCamera stereoCam2(Pose3(), K2);
@@ -117,7 +117,7 @@ TEST( StereoCamera, backproject)
 }
 
 /* ************************************************************************* */
-TEST( StereoCamera, backproject2)
+TEST( StereoCamera, backproject_case2)
 {
   Rot3 R(0.589511291,  -0.804859792,  0.0683931805,
          -0.804435942,  -0.592650676,  -0.0405925523,
@@ -132,6 +132,42 @@ TEST( StereoCamera, backproject2)
   CHECK(assert_equal(z, actual, 1e-3));
 }
 
+static Point3 backproject3(const Pose3& pose, const StereoPoint2& point, const Cal3_S2Stereo& K) {
+  return StereoCamera(pose, boost::make_shared<Cal3_S2Stereo>(K)).backproject(point);
+}
+
+/* ************************************************************************* */
+TEST( StereoCamera, backproject2_case1)
+{
+  Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
+  StereoCamera stereoCam2(Pose3(), K2);
+
+  Point3 expected_point(1.2, 2.3, 4.5);
+  StereoPoint2 stereo_point = stereoCam2.project(expected_point);
+
+  Matrix actual_jacobian_1, actual_jacobian_2;
+  Point3 actual_point = stereoCam2.backproject2(stereo_point, actual_jacobian_1, actual_jacobian_2);
+  CHECK(assert_equal(expected_point, actual_point, 1e-8));
+
+  Matrix expected_jacobian = numericalDerivative32(backproject3, Pose3(), stereo_point, *K2);
+  CHECK(assert_equal(expected_jacobian, actual_jacobian_1, acutal_jacobian_2));
+}
+
+TEST( StereoCamera, backproject2_case2)
+{
+  Rot3 R(0.589511291,  -0.804859792,  0.0683931805,
+         -0.804435942,  -0.592650676,  -0.0405925523,
+         0.0732045588,  -0.0310882277,  -0.996832359);
+  Point3 t(53.5239823, 23.7866016, -4.42379876);
+  Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612));
+  StereoCamera camera(Pose3(R,t), K);
+  StereoPoint2 z(184.812, 129.068, 714.768);
+
+  Point3 l = camera.backproject2(z);
+  StereoPoint2 actual = camera.project(l);
+  CHECK(assert_equal(z, actual, 1e-3));
+}
+
 /* ************************************************************************* */
   int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */