diff --git a/examples/FisheyeExample.cpp b/examples/FisheyeExample.cpp new file mode 100644 index 000000000..b03c9a71d --- /dev/null +++ b/examples/FisheyeExample.cpp @@ -0,0 +1,129 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file FisheyeExample.cpp + * @brief A visualSLAM example for the structure-from-motion problem on a + * simulated dataset. This version uses a fisheye camera model and a GaussNewton + * solver to solve the graph in one batch + * @author ghaggin + * @Date Apr 9,2020 + */ + +/** + * A structure-from-motion example with landmarks + * - The landmarks form a 10 meter cube + * - The robot rotates around the landmarks, always facing towards the cube + */ + +// For loading the data +#include "SFMdata.h" + +// Camera observations of landmarks will be stored as Point2 (x, y). +#include + +// Each variable in the system (poses and landmarks) must be identified with a +// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols +// (X1, X2, L1). Here we will use Symbols +#include + +// Use GaussNewtonOptimizer to solve graph +#include +#include +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common +// factors have been provided with the library for solving robotics/SLAM/Bundle +// Adjustment problems. Here we will use Projection factors to model the +// camera's landmark observations. Also, we will initialize the robot at some +// location using a Prior factor. +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +int main(int argc, char *argv[]) +{ + // Define the camera calibration parameters + // Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + boost::shared_ptr K(new Cal3Fisheye( + 278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035, 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625)); + + // Define the camera observation noise model, 1 pixel stddev + auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); + + // Create the set of ground-truth landmarks + vector points = createPoints(); + + // Create the set of ground-truth poses + vector poses = createPoses(); + + // Create a Factor Graph and Values to hold the new data + NonlinearFactorGraph graph; + Values initialEstimate; + + // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw + static auto kPosePrior = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); + graph.emplace_shared>(Symbol('x', 0), poses[0], kPosePrior); + + // Add a prior on landmark l0 + static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1); + graph.emplace_shared>(Symbol('l', 0), points[0], kPointPrior); + + // Add initial guesses to all observed landmarks + // Intentionally initialize the variables off from the ground truth + static Point3 kDeltaPoint(-0.25, 0.20, 0.15); + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(Symbol('l', j), points[j] + kDeltaPoint); + + // Loop over the poses, adding the observations to the graph + for (size_t i = 0; i < poses.size(); ++i) + { + // Add factors for each landmark observation + for (size_t j = 0; j < points.size(); ++j) + { + PinholeCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + graph.emplace_shared>( + measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); + } + + // Add an initial guess for the current pose + // Intentionally initialize the variables off from the ground truth + static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose); + } + + GaussNewtonParams params; + params.setVerbosity("TERMINATION"); + params.maxIterations = 10000; + + std::cout << "Optimizing the factor graph" << std::endl; + GaussNewtonOptimizer optimizer(graph, initialEstimate, params); + Values result = optimizer.optimize(); + std::cout << "Optimization complete" << std::endl; + + std::cout << "initial error=" << graph.error(initialEstimate) << std::endl; + std::cout << "final error=" << graph.error(result) << std::endl; + + std::ofstream os("examples/vio_batch.dot"); + graph.saveGraph(os, result); + + return 0; +} +/* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp new file mode 100644 index 000000000..c6b43004e --- /dev/null +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -0,0 +1,217 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cal3Fisheye.cpp + * @date Apr 8, 2020 + * @author ghaggin + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +Cal3Fisheye::Cal3Fisheye(const Vector& v) + : fx_(v[0]), + fy_(v[1]), + s_(v[2]), + u0_(v[3]), + v0_(v[4]), + k1_(v[5]), + k2_(v[6]), + k3_(v[7]), + k4_(v[8]) {} + +/* ************************************************************************* */ +Vector9 Cal3Fisheye::vector() const { + Vector9 v; + v << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_; + return v; +} + +/* ************************************************************************* */ +Matrix3 Cal3Fisheye::K() const { + Matrix3 K; + K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; + return K; +} + +/* ************************************************************************* */ +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; +} + +/* ************************************************************************* */ +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 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; + 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); + + // 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); + + return uv; +} + +/* ************************************************************************* */ +Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const { + // initial gues just inverts the pinhole model + const double u = uv.x(), v = uv.y(); + const double yd = (v - v0_) / fy_; + const double xd = (u - s_ * yd - u0_) / fx_; + Point2 pi(xd, yd); + + // Perform newtons method, break when solution converges past tol, + // throw exception if max iterations are reached + const int maxIterations = 10; + int iteration; + for (iteration = 0; iteration < maxIterations; ++iteration) { + Matrix2 jac; + + // Calculate the current estimate (uv_hat) and the jacobian + const Point2 uv_hat = uncalibrate(pi, boost::none, jac); + + // Test convergence + if ((uv_hat - uv).norm() < tol) break; + + // Newton's method update step + pi = pi - jac.inverse() * (uv_hat - uv); + } + + if (iteration >= maxIterations) + throw std::runtime_error( + "Cal3Fisheye::calibrate fails to converge. need a better " + "initialization"); + + 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"); + ; +} + +/* ************************************************************************* */ +bool Cal3Fisheye::equals(const Cal3Fisheye& K, double tol) const { + if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || + std::abs(s_ - K.s_) > tol || std::abs(u0_ - K.u0_) > tol || + std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol || + std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol || + std::abs(k4_ - K.k4_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +Cal3Fisheye Cal3Fisheye::retract(const Vector& d) const { + return Cal3Fisheye(vector() + d); +} + +/* ************************************************************************* */ +Vector Cal3Fisheye::localCoordinates(const Cal3Fisheye& T2) const { + return T2.vector() - vector(); +} + +} // namespace gtsam +/* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h new file mode 100644 index 000000000..5fb196047 --- /dev/null +++ b/gtsam/geometry/Cal3Fisheye.h @@ -0,0 +1,211 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cal3Fisheye.h + * @brief Calibration of a fisheye camera + * @date Apr 8, 2020 + * @author ghaggin + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * @brief Calibration of a fisheye camera + * @addtogroup geometry + * \nosubgrouping + * + * Uses same distortionmodel as OpenCV, with + * https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html + * 3D point in camera frame + * p = (x, y, z) + * Intrinsic coordinates: + * [x_i;y_i] = [x/z; y/z] + * Distorted coordinates: + * r^2 = (x_i)^2 + (y_i)^2 + * th = atan(r) + * th_d = th(1 + k1*th^2 + k2*th^4 + k3*th^6 + k4*th^8) + * [x_d; y_d] = (th_d / r)*[x_i; y_i] + * Pixel coordinates: + * K = [fx s u0; 0 fy v0 ;0 0 1] + * [u; v; 1] = K*[x_d; y_d; 1] + */ +class GTSAM_EXPORT Cal3Fisheye { + protected: + double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point + double k1_, k2_, k3_, k4_; // fisheye distortion coefficients + + public: + enum { dimension = 9 }; + typedef boost::shared_ptr + shared_ptr; ///< shared pointer to fisheye calibration object + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3Fisheye() + : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {} + + Cal3Fisheye(const double fx, const double fy, const double s, const double u0, + const double v0, const double k1, const double k2, + const double k3, const double k4) + : fx_(fx), + fy_(fy), + s_(s), + u0_(u0), + v0_(v0), + k1_(k1), + k2_(k2), + k3_(k3), + k4_(k4) {} + + virtual ~Cal3Fisheye() {} + + /// @} + /// @name Advanced Constructors + /// @{ + + Cal3Fisheye(const Vector& v); + + /// @} + /// @name Standard Interface + /// @{ + + /// focal length x + inline double fx() const { return fx_; } + + /// focal length x + inline double fy() const { return fy_; } + + /// skew + inline double skew() const { return s_; } + + /// image center in x + inline double px() const { return u0_; } + + /// image center in y + inline double py() const { return v0_; } + + /// First distortion coefficient + inline double k1() const { return k1_; } + + /// Second distortion coefficient + inline double k2() const { return k2_; } + + /// First tangential distortion coefficient + inline double k3() const { return k3_; } + + /// Second tangential distortion coefficient + inline double k4() const { return k4_; } + + /// return calibration matrix + Matrix3 K() const; + + /// return distortion parameter vector + Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); } + + /// Return all parameters as a vector + Vector9 vector() const; + + /** + * @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image + * coordinates [u; v] + * @param p point in intrinsic coordinates + * @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters (fx, fy, ..., + * k4) + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi) + * @return point in (distorted) image coordinates + */ + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; + + /// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i, + /// 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 + /// @{ + + /// print with optional string + virtual void print(const std::string& s = "") const; + + /// assert equality up to a tolerance + bool equals(const Cal3Fisheye& K, double tol = 10e-9) const; + + /// @} + /// @name Manifold + /// @{ + + /// Given delta vector, update calibration + Cal3Fisheye retract(const Vector& d) const; + + /// Given a different calibration, calculate update to obtain it + Vector localCoordinates(const Cal3Fisheye& T2) const; + + /// Return dimensions of calibration manifold object + virtual size_t dim() const { return 9; } + + /// Return dimensions of calibration manifold object + static size_t Dim() { return 9; } + + /// @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(new Cal3Fisheye(*this)); + } + + /// @} + + private: + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(fx_); + ar& BOOST_SERIALIZATION_NVP(fy_); + ar& BOOST_SERIALIZATION_NVP(s_); + ar& BOOST_SERIALIZATION_NVP(u0_); + ar& BOOST_SERIALIZATION_NVP(v0_); + ar& BOOST_SERIALIZATION_NVP(k1_); + ar& BOOST_SERIALIZATION_NVP(k2_); + ar& BOOST_SERIALIZATION_NVP(k3_); + ar& BOOST_SERIALIZATION_NVP(k4_); + } + + /// @} +}; + +template <> +struct traits : public internal::Manifold {}; + +template <> +struct traits : public internal::Manifold {}; + +} // namespace gtsam diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp new file mode 100644 index 000000000..9203b5438 --- /dev/null +++ b/gtsam/geometry/tests/testCal3DFisheye.cpp @@ -0,0 +1,206 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testCal3Fisheye.cpp + * @brief Unit tests for fisheye calibration class + * @author ghaggin + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Cal3Fisheye) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3Fisheye) + +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); + +/* ************************************************************************* */ +TEST(Cal3Fisheye, uncalibrate1) { + // Calculate the solution + 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 + K.k1() * tt + K.k2() * t4 + K.k3() * t6 + K.k4() * t8); + Vector3 pd(td / r * xi, td / r * yi, 1); + Vector3 v = K.K() * pd; + + Point2 uv_sol(v[0] / v[2], v[1] / v[2]); + + Point2 uv = K.uncalibrate(p); + 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))); +} + +/* ************************************************************************* */ +/** + * 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; + + pi = Point2(0.5, 0.5); // point in intrinsic coordinates + uv = K.uncalibrate(pi); // map intrinsic coord to image plane (pi) + pi_hat = K.calibrate(uv); // map image coords (pi) back to intrinsic coords + CHECK(traits::Equals(pi, pi_hat, + 1e-5)); // check that the inv mapping works + + pi = Point2(-0.7, -1.2); + uv = K.uncalibrate(pi); + pi_hat = K.calibrate(uv); + CHECK(traits::Equals(pi, pi_hat, 1e-5)); + + pi = Point2(-3, 5); + uv = K.uncalibrate(pi); + pi_hat = K.calibrate(uv); + CHECK(traits::Equals(pi, pi_hat, 1e-5)); + + pi = Point2(7, -12); + uv = K.uncalibrate(pi); + pi_hat = K.calibrate(uv); + CHECK(traits::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); +} + +/* ************************************************************************* */ +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; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */