Merge pull request #279 from ghaggin/camera_mods

Fisheye Calibration
release/4.3a0
Frank Dellaert 2020-05-09 16:44:54 -04:00 committed by GitHub
commit 6a23c476a1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 763 additions and 0 deletions

129
examples/FisheyeExample.cpp Normal file
View File

@ -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 <gtsam/geometry/Point2.h>
// 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 <gtsam/inference/Symbol.h>
// Use GaussNewtonOptimizer to solve graph
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
// 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 <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <fstream>
#include <vector>
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<Cal3Fisheye> 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<Point3> points = createPoints();
// Create the set of ground-truth poses
vector<Pose3> 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<PriorFactor<Pose3>>(Symbol('x', 0), poses[0], kPosePrior);
// Add a prior on landmark l0
static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3>>(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<Point3>(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<Cal3Fisheye> camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3Fisheye>>(
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;
}
/* ************************************************************************* */

View File

@ -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 <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
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
/* ************************************************************************* */

View File

@ -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 <gtsam/geometry/Point2.h>
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<Cal3Fisheye>
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<Cal3Fisheye> clone() const {
return boost::shared_ptr<Cal3Fisheye>(new Cal3Fisheye(*this));
}
/// @}
private:
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template <class Archive>
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<Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
template <>
struct traits<const Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
} // namespace gtsam

View File

@ -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 <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Point3.h>
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<Point2>::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<Point2>::Equals(pi, pi_hat, 1e-5));
pi = Point2(-3, 5);
uv = K.uncalibrate(pi);
pi_hat = K.calibrate(uv);
CHECK(traits<Point2>::Equals(pi, pi_hat, 1e-5));
pi = Point2(7, -12);
uv = K.uncalibrate(pi);
pi_hat = K.calibrate(uv);
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);
}
/* ************************************************************************* */
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);
}
/* ************************************************************************* */