Cal3_S2Stereo Calibrate and Uncalibrate

gtsam/geometry/Cal3_S2.cpp

@@ -59,10 +59,11 @@ Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
   double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
 
   Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
-  if (Dcal)
+  if (Dcal) {
     *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
         -inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, -inv_fy * point.y(),
         0, 0, -inv_fy;
+  }
   if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
   return point;
 }

gtsam/geometry/Cal3_S2Stereo.cpp

@@ -42,6 +42,34 @@ bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
           std::fabs(b_ - other.baseline()) < tol);
 }
 
+/* ************************************************************************* */
+Point2 Cal3_S2Stereo::uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal,
+                            OptionalJacobian<2, 2> Dp) const {
+  const double x = p.x(), y = p.y();
+  if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, 0.0, y, 0.0, 0.0, 1.0, 0.0;
+  if (Dp) *Dp << fx_, s_, 0.0, fy_;
+  return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
+}
+
+/* ************************************************************************* */
+Point2 Cal3_S2Stereo::calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal,
+                          OptionalJacobian<2, 2> Dp) const {
+  const double u = p.x(), v = p.y();
+  double delta_u = u - u0_, delta_v = v - v0_;
+  double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
+  double inv_fy_delta_v = inv_fy * delta_v;
+  double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
+
+  Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
+  if (Dcal) {
+    *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
+        -inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, 0,
+        -inv_fy * point.y(), 0, 0, -inv_fy, 0;
+  }
+  if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
+  return point;
+}
+
 /* ************************************************************************* */
 
 }  // namespace gtsam

gtsam/geometry/Cal3_S2Stereo.h

@@ -55,6 +55,33 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
   Cal3_S2Stereo(double fov, int w, int h, double b)
       : Cal3_S2(fov, w, h), b_(b) {}
 
+  /**
+   * Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
+   * @param p point in intrinsic coordinates
+   * @param Dcal optional 2*6 Jacobian wrpt Cal3_S2Stereo parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in image coordinates
+   */
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
+                     OptionalJacobian<2, 2> Dp = boost::none) const;
+
+  /**
+   * Convert image coordinates uv to intrinsic coordinates xy
+   * @param p point in image coordinates
+   * @param Dcal optional 2*6 Jacobian wrpt Cal3_S2Stereo parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in intrinsic coordinates
+   */
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
+
+  /**
+   * Convert homogeneous image coordinates to intrinsic coordinates
+   * @param p point in image coordinates
+   * @return point in intrinsic coordinates
+   */
+  Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); }
+
   /// @}
   /// @name Testable
   /// @{

gtsam/geometry/tests/testCal3_S2Stereo.cpp

@@ -54,7 +54,39 @@ TEST(Cal3_S2Stereo, Calibrate) {
 TEST(Cal3_S2Stereo, CalibrateHomogeneous) {
   Vector3 intrinsic(2, 3, 1);
   Vector3 image(1320.3, 1740, 1);
-  CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image)));
+  CHECK(assert_equal(intrinsic, K.calibrate(image)));
+}
+
+/* ************************************************************************* */
+Point2 uncalibrate_(const Cal3_S2Stereo& k, const Point2& pt) {
+  return k.uncalibrate(pt);
+}
+
+TEST(Cal3_S2Stereo, Duncalibrate) {
+  Matrix26 Dcal;
+  Matrix22 Dp;
+  K.uncalibrate(p, Dcal, Dp);
+
+  Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
+  CHECK(assert_equal(numerical1, Dcal, 1e-8));
+  Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
+  CHECK(assert_equal(numerical2, Dp, 1e-9));
+}
+
+Point2 calibrate_(const Cal3_S2Stereo& K, const Point2& pt) {
+  return K.calibrate(pt);
+}
+/* ************************************************************************* */
+TEST(Cal3_S2Stereo, Dcalibrate) {
+  Matrix26 Dcal;
+  Matrix22 Dp;
+  Point2 expected = K.calibrate(p_uv, Dcal, Dp);
+  CHECK(assert_equal(expected, p_xy, 1e-8));
+
+  Matrix numerical1 = numericalDerivative21(calibrate_, K, p_uv);
+  CHECK(assert_equal(numerical1, Dcal, 1e-8));
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, p_uv);
+  CHECK(assert_equal(numerical2, Dp, 1e-8));
 }
 
 /* ************************************************************************* */