From d827d3ebadd9d3f38b699028b2719c35e2211170 Mon Sep 17 00:00:00 2001
From: Glen Haggin <ghaggin@umich.edu>
Date: Wed, 22 Apr 2020 13:51:01 -0400
Subject: [PATCH] Added test cases for fisheye calibration verified in OpenCv

---
 gtsam/geometry/Cal3Fisheye.cpp            |  3 +-
 gtsam/geometry/tests/testCal3DFisheye.cpp | 85 ++++++++++++++++++++++-
 2 files changed, 86 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp
index 97931f588..c6b43004e 100644
--- a/gtsam/geometry/Cal3Fisheye.cpp
+++ b/gtsam/geometry/Cal3Fisheye.cpp
@@ -105,7 +105,8 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
   const double t = atan(r);
   const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
   const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
-  const double xd = td / r * xi, yd = td / r * yi;
+  const double td_o_r = r > 1e-8 ? td / r : 1;
+  const double xd = td_o_r * xi, yd = td_o_r * yi;
   Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_);
 
   Matrix2 DK;
diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp
index 50fae5a8b..9203b5438 100644
--- a/gtsam/geometry/tests/testCal3DFisheye.cpp
+++ b/gtsam/geometry/tests/testCal3DFisheye.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
+#include <gtsam/geometry/Point3.h>
 
 using namespace gtsam;
 
@@ -49,7 +50,63 @@ TEST(Cal3Fisheye, uncalibrate1) {
   CHECK(assert_equal(uv, uv_sol));
 }
 
-TEST(Cal3Fisheye, calibrate) {
+/* ************************************************************************* */
+/**
+ * Check that a point at (0,0) projects to the
+ * image center.
+ */
+TEST(Cal3Fisheye, uncalibrate2) {
+  Point2 pz(0, 0);
+  auto uv = K.uncalibrate(pz);
+  CHECK(assert_equal(uv, Point2(u0, v0)));
+}
+
+/* ************************************************************************* */
+/**
+ *  This test uses cv2::fisheye::projectPoints to test that uncalibrate
+ *  properly projects a point into the image plane.  One notable difference
+ *  between opencv and the Cal3Fisheye::uncalibrate function is the skew
+ *  parameter. The equivalence is alpha = s/fx.
+ *
+ *
+ * Python script to project points with fisheye model in OpenCv
+ * (script run with OpenCv version 4.2.0 and Numpy version 1.18.2)
+ */
+// clang-format off
+/*
+===========================================================
+
+import numpy as np
+import cv2
+
+objpts = np.float64([[23,27,31]]).reshape(1,-1,3)
+
+cameraMatrix = np.float64([
+    [250, 0, 320],
+    [0, 260, 240],
+    [0,0,1]
+])
+alpha = 0.1/250
+distCoeffs = np.float64([-0.013721808247486035, 0.020727425669427896,-0.012786476702685545, 0.0025242267320687625]) 
+
+rvec = np.float64([[0.,0.,0.]])
+tvec = np.float64([[0.,0.,0.]]);
+imagePoints, jacobian = cv2.fisheye.projectPoints(objpts, rvec, tvec, cameraMatrix, distCoeffs, alpha=alpha) 
+np.set_printoptions(precision=14) 
+print(imagePoints)
+===========================================================
+ * Script output: [[[457.82638130304935 408.18905848512986]]]
+ */
+// clang-format on
+TEST(Cal3Fisheye, uncalibrate3) {
+  Point3 p3(23, 27, 31);
+  Point2 xi(p3.x() / p3.z(), p3.y() / p3.z());
+  auto uv = K.uncalibrate(xi);
+  CHECK(assert_equal(uv, Point2(457.82638130304935, 408.18905848512986)));
+}
+
+/* ************************************************************************* */
+TEST(Cal3Fisheye, calibrate1) {
   Point2 pi;
   Point2 uv;
   Point2 pi_hat;
@@ -76,6 +133,32 @@ TEST(Cal3Fisheye, calibrate) {
   CHECK(traits<Point2>::Equals(pi, pi_hat, 1e-5));
 }
 
+/* ************************************************************************* */
+/**
+ * Check that calibrate returns (0,0) for the image center
+ */
+TEST(Cal3Fisheye, calibrate2) {
+  Point2 uv(u0, v0);
+  auto xi_hat = K.calibrate(uv);
+  CHECK(assert_equal(xi_hat, Point2(0, 0)))
+}
+
+/**
+ * Run calibrate on OpenCv test from uncalibrate3
+ *  (script shown above)
+ * 3d point: (23, 27, 31)
+ * 2d point in image plane: (457.82638130304935, 408.18905848512986)
+ */
+TEST(Cal3Fisheye, calibrate3) {
+  Point3 p3(23, 27, 31);
+  Point2 xi(p3.x() / p3.z(), p3.y() / p3.z());
+  Point2 uv(457.82638130304935, 408.18905848512986);
+  auto xi_hat = K.calibrate(uv);
+  CHECK(assert_equal(xi_hat, xi));
+}
+
+/* ************************************************************************* */
+// For numerical derivatives
 Point2 uncalibrate_(const Cal3Fisheye& k, const Point2& pt) {
   return k.uncalibrate(pt);
 }