Frank Dellaert
GitHub
commit
23d97af76a
|
|
@ -24,7 +24,7 @@
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Fisheye::Cal3Fisheye(const Vector& v)
|
||||
Cal3Fisheye::Cal3Fisheye(const Vector9& v)
|
||||
: fx_(v[0]),
|
||||
fy_(v[1]),
|
||||
s_(v[2]),
|
||||
|
|
@ -50,76 +50,73 @@ Matrix3 Cal3Fisheye::K() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Matrix29 D2dcalibration(const double xd, const double yd,
|
||||
const double xi, const double yi,
|
||||
const double t3, const double t5,
|
||||
const double t7, const double t9, const double r,
|
||||
Matrix2& DK) {
|
||||
// order: fx, fy, s, u0, v0
|
||||
Matrix25 DR1;
|
||||
DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0;
|
||||
|
||||
// order: k1, k2, k3, k4
|
||||
Matrix24 DR2;
|
||||
DR2 << t3 * xi, t5 * xi, t7 * xi, t9 * xi, t3 * yi, t5 * yi, t7 * yi, t9 * yi;
|
||||
DR2 /= r;
|
||||
Matrix29 D;
|
||||
D << DR1, DK * DR2;
|
||||
return D;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Matrix2 D2dintrinsic(const double xi, const double yi, const double r,
|
||||
const double td, const double t, const double tt,
|
||||
const double t4, const double t6, const double t8,
|
||||
const double k1, const double k2, const double k3,
|
||||
const double k4, const Matrix2& DK) {
|
||||
const double dr_dxi = xi / sqrt(xi * xi + yi * yi);
|
||||
const double dr_dyi = yi / sqrt(xi * xi + yi * yi);
|
||||
const double dt_dr = 1 / (1 + r * r);
|
||||
const double dtd_dt =
|
||||
1 + 3 * k1 * tt + 5 * k2 * t4 + 7 * k3 * t6 + 9 * k4 * t8;
|
||||
const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
|
||||
const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
|
||||
|
||||
const double rinv = 1 / r;
|
||||
const double rrinv = 1 / (r * r);
|
||||
const double dxd_dxi =
|
||||
dtd_dxi * xi * rinv + td * rinv + td * xi * (-rrinv) * dr_dxi;
|
||||
const double dxd_dyi = dtd_dyi * xi * rinv - td * xi * rrinv * dr_dyi;
|
||||
const double dyd_dxi = dtd_dxi * yi * rinv - td * yi * rrinv * dr_dxi;
|
||||
const double dyd_dyi =
|
||||
dtd_dyi * yi * rinv + td * rinv + td * yi * (-rrinv) * dr_dyi;
|
||||
|
||||
Matrix2 DR;
|
||||
DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
|
||||
|
||||
return DK * DR;
|
||||
double Cal3Fisheye::Scaling(double r) {
|
||||
static constexpr double threshold = 1e-8;
|
||||
if (r > threshold || r < -threshold) {
|
||||
return atan(r) / r;
|
||||
} else {
|
||||
// Taylor expansion close to 0
|
||||
double r2 = r * r, r4 = r2 * r2;
|
||||
return 1.0 - r2 / 3 + r4 / 5;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
|
||||
OptionalJacobian<2, 2> H2) const {
|
||||
const double xi = p.x(), yi = p.y();
|
||||
const double r = sqrt(xi * xi + yi * yi);
|
||||
const double r2 = xi * xi + yi * yi, r = sqrt(r2);
|
||||
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 td_o_r = r > 1e-8 ? td / r : 1;
|
||||
const double xd = td_o_r * xi, yd = td_o_r * yi;
|
||||
const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4;
|
||||
Vector5 K, T;
|
||||
K << 1, k1_, k2_, k3_, k4_;
|
||||
T << 1, t2, t4, t6, t8;
|
||||
const double scaling = Scaling(r);
|
||||
const double s = scaling * K.dot(T);
|
||||
const double xd = s * xi, yd = s * yi;
|
||||
Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_);
|
||||
|
||||
Matrix2 DK;
|
||||
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
|
||||
|
||||
// Derivative for calibration parameters (2 by 9)
|
||||
if (H1)
|
||||
*H1 = D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK);
|
||||
if (H1) {
|
||||
Matrix25 DR1;
|
||||
// order: fx, fy, s, u0, v0
|
||||
DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0;
|
||||
|
||||
// order: k1, k2, k3, k4
|
||||
Matrix24 DR2;
|
||||
auto T4 = T.tail<4>().transpose();
|
||||
DR2 << xi * T4, yi * T4;
|
||||
*H1 << DR1, DK * scaling * DR2;
|
||||
}
|
||||
|
||||
// Derivative for points in intrinsic coords (2 by 2)
|
||||
if (H2)
|
||||
*H2 =
|
||||
D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK);
|
||||
if (H2) {
|
||||
const double dtd_dt =
|
||||
1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8;
|
||||
const double dt_dr = 1 / (1 + r2);
|
||||
const double rinv = 1 / r;
|
||||
const double dr_dxi = xi * rinv;
|
||||
const double dr_dyi = yi * rinv;
|
||||
const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
|
||||
const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
|
||||
|
||||
const double td = t * K.dot(T);
|
||||
const double rrinv = 1 / r2;
|
||||
const double dxd_dxi =
|
||||
dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi;
|
||||
const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi;
|
||||
const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi;
|
||||
const double dyd_dyi =
|
||||
dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi;
|
||||
|
||||
Matrix2 DR;
|
||||
DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
|
||||
|
||||
*H2 = DK * DR;
|
||||
}
|
||||
|
||||
return uv;
|
||||
}
|
||||
|
|
@ -157,39 +154,10 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
|
|||
return pi;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix2 Cal3Fisheye::D2d_intrinsic(const Point2& p) const {
|
||||
const double xi = p.x(), yi = p.y();
|
||||
const double r = sqrt(xi * xi + yi * yi);
|
||||
const double t = atan(r);
|
||||
const double tt = t * t, t4 = tt * tt, t6 = t4 * tt, t8 = t4 * t4;
|
||||
const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
|
||||
|
||||
Matrix2 DK;
|
||||
DK << fx_, s_, 0.0, fy_;
|
||||
|
||||
return D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix29 Cal3Fisheye::D2d_calibration(const Point2& p) const {
|
||||
const double xi = p.x(), yi = p.y();
|
||||
const double r = sqrt(xi * xi + yi * yi);
|
||||
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;
|
||||
|
||||
Matrix2 DK;
|
||||
DK << fx_, s_, 0.0, fy_;
|
||||
return D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Cal3Fisheye::print(const std::string& s_) const {
|
||||
gtsam::print((Matrix)K(), s_ + ".K");
|
||||
gtsam::print(Vector(k()), s_ + ".k");
|
||||
;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -20,6 +20,8 @@
|
|||
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
|
|
@ -43,7 +45,7 @@ namespace gtsam {
|
|||
* [u; v; 1] = K*[x_d; y_d; 1]
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3Fisheye {
|
||||
protected:
|
||||
private:
|
||||
double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point
|
||||
double k1_, k2_, k3_, k4_; // fisheye distortion coefficients
|
||||
|
||||
|
|
@ -78,7 +80,7 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
Cal3Fisheye(const Vector& v);
|
||||
explicit Cal3Fisheye(const Vector9& v);
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
|
|
@ -120,6 +122,9 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
/// Return all parameters as a vector
|
||||
Vector9 vector() const;
|
||||
|
||||
/// Helper function that calculates atan(r)/r
|
||||
static double Scaling(double r);
|
||||
|
||||
/**
|
||||
* @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image
|
||||
* coordinates [u; v]
|
||||
|
|
@ -136,13 +141,6 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
/// y_i]
|
||||
Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
|
||||
|
||||
/// Derivative of uncalibrate wrpt intrinsic coordinates [x_i; y_i]
|
||||
Matrix2 D2d_intrinsic(const Point2& p) const;
|
||||
|
||||
/// Derivative of uncalibrate wrpt the calibration parameters
|
||||
/// [fx, fy, s, u0, v0, k1, k2, k3, k4]
|
||||
Matrix29 D2d_calibration(const Point2& p) const;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -10,17 +10,18 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testCal3Fisheye.cpp
|
||||
* @file testCal3DFisheye.cpp
|
||||
* @brief Unit tests for fisheye calibration class
|
||||
* @author ghaggin
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/Cal3Fisheye.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
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,
|
||||
0.020727425669427896, -0.012786476702685545,
|
||||
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) {
|
||||
// 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 t = atan(r);
|
||||
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 = K.uncalibrate(p);
|
||||
Point2 uv = K.uncalibrate(kTestPoint2);
|
||||
CHECK(assert_equal(uv, uv_sol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* 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)));
|
||||
// For numerical derivatives
|
||||
Point2 f(const Cal3Fisheye& k, const Point2& pt) { return k.uncalibrate(pt); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Fisheye, Derivatives) {
|
||||
Matrix H1, H2;
|
||||
K.uncalibrate(kTestPoint2, H1, H2);
|
||||
CHECK(assert_equal(numericalDerivative21(f, K, kTestPoint2, 1e-7), H1, 1e-5));
|
||||
CHECK(assert_equal(numericalDerivative22(f, K, kTestPoint2, 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)
|
||||
*/
|
||||
// Check that a point at (0,0) projects to the image center.
|
||||
TEST(Cal3Fisheye, uncalibrate2) {
|
||||
Point2 pz(0, 0);
|
||||
Matrix H1, H2;
|
||||
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));
|
||||
// TODO(frank): the second jacobian is all NaN for the image center!
|
||||
// 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
|
||||
/*
|
||||
===========================================================
|
||||
|
|
@ -94,6 +120,7 @@ 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]]]
|
||||
*/
|
||||
|
|
@ -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) {
|
||||
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)
|
||||
*/
|
||||
/* ************************************************************************* */
|
||||
// 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());
|
||||
|
|
@ -157,47 +181,6 @@ TEST(Cal3Fisheye, calibrate3) {
|
|||
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() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue