Re-ordered and cleaned up tests, added derivative tests for image center

release/4.3a0
Frank dellaert 2020-05-10 13:40:37 -04:00
parent 83d0b9d3ff
commit 4f07aeb859
1 changed files with 56 additions and 73 deletions

View File

@ -10,17 +10,18 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file testCal3Fisheye.cpp * @file testCal3DFisheye.cpp
* @brief Unit tests for fisheye calibration class * @brief Unit tests for fisheye calibration class
* @author ghaggin * @author ghaggin
*/ */
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Fisheye.h> #include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3Fisheye) GTSAM_CONCEPT_TESTABLE_INST(Cal3Fisheye)
@ -30,12 +31,27 @@ static const double fx = 250, fy = 260, s = 0.1, u0 = 320, v0 = 240;
static Cal3Fisheye K(fx, fy, s, u0, v0, -0.013721808247486035, static Cal3Fisheye K(fx, fy, s, u0, v0, -0.013721808247486035,
0.020727425669427896, -0.012786476702685545, 0.020727425669427896, -0.012786476702685545,
0.0025242267320687625); 0.0025242267320687625);
static Point2 p(2, 3); static Point2 kTestPoint2(2, 3);
/* ************************************************************************* */
TEST(Cal3Fisheye, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
/* ************************************************************************* */
TEST(Cal3Fisheye, retract) {
Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4,
K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8,
K.k4() + 9);
Vector d(9);
d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
Cal3Fisheye actual = K.retract(d);
CHECK(assert_equal(expected, actual, 1e-7));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Cal3Fisheye, uncalibrate1) { TEST(Cal3Fisheye, uncalibrate1) {
// Calculate the solution // Calculate the solution
const double xi = p.x(), yi = p.y(); const double xi = kTestPoint2.x(), yi = kTestPoint2.y();
const double r = sqrt(xi * xi + yi * yi); const double r = sqrt(xi * xi + yi * yi);
const double t = atan(r); const double t = atan(r);
const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4; const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
@ -46,32 +62,42 @@ TEST(Cal3Fisheye, uncalibrate1) {
Point2 uv_sol(v[0] / v[2], v[1] / v[2]); Point2 uv_sol(v[0] / v[2], v[1] / v[2]);
Point2 uv = K.uncalibrate(p); Point2 uv = K.uncalibrate(kTestPoint2);
CHECK(assert_equal(uv, uv_sol)); CHECK(assert_equal(uv, uv_sol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
/** // For numerical derivatives
* Check that a point at (0,0) projects to the Point2 f(const Cal3Fisheye& k, const Point2& pt) { return k.uncalibrate(pt); }
* image center.
*/ /* ************************************************************************* */
TEST(Cal3Fisheye, uncalibrate2) { TEST(Cal3Fisheye, Derivatives) {
Point2 pz(0, 0); Matrix H1, H2;
auto uv = K.uncalibrate(pz); K.uncalibrate(kTestPoint2, H1, H2);
CHECK(assert_equal(uv, Point2(u0, v0))); CHECK(assert_equal(numericalDerivative21(f, K, kTestPoint2, 1e-7), H1, 1e-5));
CHECK(assert_equal(numericalDerivative22(f, K, kTestPoint2, 1e-7), H2, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
/** // Check that a point at (0,0) projects to the image center.
* This test uses cv2::fisheye::projectPoints to test that uncalibrate TEST(Cal3Fisheye, uncalibrate2) {
* properly projects a point into the image plane. One notable difference Point2 pz(0, 0);
* between opencv and the Cal3Fisheye::uncalibrate function is the skew Matrix H1, H2;
* parameter. The equivalence is alpha = s/fx. auto uv = K.uncalibrate(pz, H1, H2);
* CHECK(assert_equal(uv, Point2(u0, v0)));
* CHECK(assert_equal(numericalDerivative21(f, K, pz, 1e-7), H1, 1e-5));
* Python script to project points with fisheye model in OpenCv // TODO(frank): the second jacobian is all NaN for the image center!
* (script run with OpenCv version 4.2.0 and Numpy version 1.18.2) // CHECK(assert_equal(numericalDerivative22(f, K, pz, 1e-7), H2, 1e-5));
*/ }
/* ************************************************************************* */
// 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 // clang-format off
/* /*
=========================================================== ===========================================================
@ -94,6 +120,7 @@ tvec = np.float64([[0.,0.,0.]]);
imagePoints, jacobian = cv2.fisheye.projectPoints(objpts, rvec, tvec, cameraMatrix, distCoeffs, alpha=alpha) imagePoints, jacobian = cv2.fisheye.projectPoints(objpts, rvec, tvec, cameraMatrix, distCoeffs, alpha=alpha)
np.set_printoptions(precision=14) np.set_printoptions(precision=14)
print(imagePoints) print(imagePoints)
=========================================================== ===========================================================
* Script output: [[[457.82638130304935 408.18905848512986]]] * Script output: [[[457.82638130304935 408.18905848512986]]]
*/ */
@ -134,21 +161,18 @@ TEST(Cal3Fisheye, calibrate1) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
/** // Check that calibrate returns (0,0) for the image center
* Check that calibrate returns (0,0) for the image center
*/
TEST(Cal3Fisheye, calibrate2) { TEST(Cal3Fisheye, calibrate2) {
Point2 uv(u0, v0); Point2 uv(u0, v0);
auto xi_hat = K.calibrate(uv); auto xi_hat = K.calibrate(uv);
CHECK(assert_equal(xi_hat, Point2(0, 0))) CHECK(assert_equal(xi_hat, Point2(0, 0)))
} }
/** /* ************************************************************************* */
* Run calibrate on OpenCv test from uncalibrate3 // Run calibrate on OpenCv test from uncalibrate3
* (script shown above) // (script shown above)
* 3d point: (23, 27, 31) // 3d point: (23, 27, 31)
* 2d point in image plane: (457.82638130304935, 408.18905848512986) // 2d point in image plane: (457.82638130304935, 408.18905848512986)
*/
TEST(Cal3Fisheye, calibrate3) { TEST(Cal3Fisheye, calibrate3) {
Point3 p3(23, 27, 31); Point3 p3(23, 27, 31);
Point2 xi(p3.x() / p3.z(), p3.y() / p3.z()); Point2 xi(p3.x() / p3.z(), p3.y() / p3.z());
@ -157,47 +181,6 @@ TEST(Cal3Fisheye, calibrate3) {
CHECK(assert_equal(xi_hat, xi)); CHECK(assert_equal(xi_hat, xi));
} }
/* ************************************************************************* */
// For numerical derivatives
Point2 uncalibrate_(const Cal3Fisheye& k, const Point2& pt) {
return k.uncalibrate(pt);
}
/* ************************************************************************* */
TEST(Cal3Fisheye, Duncalibrate1) {
Matrix computed;
K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical, computed, 1e-5));
Matrix separate = K.D2d_calibration(p);
CHECK(assert_equal(numerical, separate, 1e-5));
}
/* ************************************************************************* */
TEST(Cal3Fisheye, Duncalibrate2) {
Matrix computed;
K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical, computed, 1e-5));
Matrix separate = K.D2d_intrinsic(p);
CHECK(assert_equal(numerical, separate, 1e-5));
}
/* ************************************************************************* */
TEST(Cal3Fisheye, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
/* ************************************************************************* */
TEST(Cal3Fisheye, retract) {
Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4,
K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8,
K.k4() + 9);
Vector d(9);
d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
Cal3Fisheye actual = K.retract(d);
CHECK(assert_equal(expected, actual, 1e-7));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;