added calibrate with jacobians for Cal3Bundler

gtsam/geometry/Cal3Bundler.cpp

@@ -121,6 +121,39 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
   return pn;
 }
 
+/* ************************************************************************* */
+Point2 Cal3Bundler::calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal,
+                              OptionalJacobian<2, 2> Dp) const {
+  Point2 pn = calibrate(p, 1e-5);
+
+  // Approximate the jacobians via a single iteration of g.
+  if (Dcal) {
+    const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_;
+    const double xx = x * x, yy = y * y;
+    const double rr = xx + yy;
+    const double g = (1 + k1_ * rr + k2_ * rr * rr);
+    const double inv_f = 1 / f_, inv_g = 1 / g;
+
+    *Dcal << -inv_f * x, -x * inv_g * rr, -x * inv_g * rr * rr, -inv_f * y,
+        -y * inv_g * rr, -y * inv_g * rr * rr;
+  }
+  if (Dp) {
+    const double u = p.x(), v = p.y(), x = (u - u0_) / f_, y = (v - v0_) / f_;
+    const double xx = x * x, yy = y * y;
+    const double rr = xx + yy;
+    const double g = (1 + k1_ * rr + k2_ * rr * rr);
+    const double inv_f = 1 / f_, inv_g = 1 / g;
+
+    const double dg_du = (2 * k1_ * x * inv_f) + (4 * k2_ * rr * x * inv_f);
+    const double dg_dv = (2 * k1_ * y * inv_f) + (4 * k2_ * rr * y * inv_f);
+
+    *Dp << (inv_f * inv_g) - (pn.x() * inv_g * dg_du), -pn.x() * inv_g * dg_dv,
+        -pn.y() * inv_g * dg_du, (inv_f * inv_g) - (pn.x() * inv_g * dg_dv);
+  }
+
+  return pn;
+}
+
 /* ************************************************************************* */
 Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const {
   Matrix2 Dp;

gtsam/geometry/Cal3Bundler.h

@@ -120,6 +120,16 @@ public:
   /// Convert a pixel coordinate to ideal coordinate
   Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
 
+  /**
+   * Convert image coordinates uv to intrinsic coordinates xy
+   * @param p point in image coordinates
+   * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in intrinsic coordinates
+   */
+  Point2 calibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
+                   OptionalJacobian<2, 2> Dp = boost::none) const;
+
   /// @deprecated might be removed in next release, use uncalibrate
   Matrix2 D2d_intrinsic(const Point2& p) const;
 

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -336,6 +336,15 @@ TEST( PinholeCamera, range3) {
   EXPECT(assert_equal(Hexpected2, D2, 1e-7));
 }
 
+/* ************************************************************************* */
+TEST( PinholeCamera, Cal3Bundler) {
+  Cal3Bundler calibration;
+  Pose3 wTc;
+  PinholeCamera<Cal3Bundler> camera(wTc, calibration);
+  Point2 p(50, 100);
+  camera.backproject(p, 10);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */