Go to the documentation of this file.
36 static constexpr
double threshold = 1
e-8;
37 if (r > threshold || r < -threshold) {
41 double r2 = r * r, r4 =
r2 *
r2;
42 return 1.0 -
r2 / 3 + r4 / 5;
49 const double xi =
p.x(), yi =
p.y(), zi = 1;
51 const double t =
atan2(r, zi);
52 const double t2 =
t *
t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4;
55 T << 1, t2, t4, t6, t8;
56 const double scaling =
Scaling(r);
57 const double s = scaling *
K.dot(
T);
58 const double xd =
s *
xi, yd =
s * yi;
62 if (H1 || H2) DK <<
fx_,
s_, 0.0,
fy_;
68 DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0;
72 auto T4 =
T.tail<4>().transpose();
74 *H1 <<
DR1, DK * scaling *
DR2;
83 1 + 3 *
k1_ * t2 + 5 *
k2_ * t4 + 7 *
k3_ * t6 + 9 *
k4_ * t8;
84 const double R2 =
r2 + zi*zi;
85 const double dt_dr = zi /
R2;
86 const double rinv = 1 / r;
87 const double dr_dxi =
xi * rinv;
88 const double dr_dyi = yi * rinv;
89 const double dtd_dr = dtd_dt * dt_dr;
91 const double c2 = dr_dxi * dr_dxi;
92 const double s2 = dr_dyi * dr_dyi;
93 const double cs = dr_dxi * dr_dyi;
95 const double dxd_dxi = dtd_dr *
c2 +
s * (1 -
c2);
96 const double dxd_dyi = (dtd_dr -
s) * cs;
97 const double dyd_dxi = dxd_dyi;
98 const double dyd_dyi = dtd_dr * s2 +
s * (1 - s2);
101 DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
116 const double u = uv.x(),
v = uv.y();
117 const double yd = (
v -
v0_) /
fy_;
118 const double xd = (u -
s_ * yd -
u0_) /
fx_;
119 const double theta =
sqrt(xd * xd + yd * yd);
126 const double scale = (theta > 0) ?
tan(theta) / theta : 1.0;
131 const int maxIterations = 10;
133 for (iteration = 0; iteration < maxIterations; ++iteration) {
143 pi = pi - jac.inverse() * (
uv_hat - uv);
146 if (iteration >= maxIterations)
147 throw std::runtime_error(
148 "Cal3Fisheye::calibrate fails to converge. need a better "
151 calibrateJacobians<Cal3Fisheye, dimension>(*
this, pi, Dcal, Dp);
159 os <<
", k1: " <<
cal.k1() <<
", k2: " <<
cal.k2() <<
", k3: " <<
cal.k3()
160 <<
", k4: " <<
cal.k4();
Annotation indicating that a class derives from another given type.
double k4_
fisheye distortion coefficients
typedef and functions to augment Eigen's VectorXd
Array< double, 1, 3 > e(1./3., 0.5, 2.)
const EIGEN_DEVICE_FUNC AtanReturnType atan() const
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
typedef and functions to augment Eigen's MatrixXd
double k2_
fisheye distortion coefficients
Eigen::Triplet< double > T
ofstream os("timeSchurFactors.csv")
Vector4 k() const
return distortion parameter vector
const Cal3_S2 cal(focalLength, focalLength, 0.0, principalPoint.x(), principalPoint.y())
bool equals(const Cal3Fisheye &K, double tol=10e-9) const
assert equality up to a tolerance
void print(const Matrix &A, const string &s, ostream &stream)
static const Similarity3 T4(R, P, s)
double tol_
tolerance value when calibrating
Common base class for all calibration models.
Calibration of a fisheye camera.
static double Scaling(double r)
Helper function that calculates atan(r)/r.
virtual Matrix3 K() const
return calibration matrix K
const EIGEN_DEVICE_FUNC TanReturnType tan() const
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
bool equals(const Cal3 &K, double tol=10e-9) const
Check if equal up to specified tolerance.
void print(const std::string &s="") const override
print with optional string
Vector9 vector() const
Return all parameters as a vector.
Array< int, Dynamic, 1 > v
static double scale(double x, double a, double b, double t1, double t2)
Scale x from [a, b] to [t1, t2].
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
convert intrinsic coordinates [x_i; y_i] to (distorted) image coordinates [u; v]
Expression< Point2 > uv_hat(uncalibrate< Cal3_S2 >, K, projection)
Jet< T, N > sqrt(const Jet< T, N > &f)
double v0_
principal point
Calibration of a fisheye camera.
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:01:56