Cal3Fisheye.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #include <gtsam/base/Matrix.h>
20 #include <gtsam/base/Vector.h>
22 #include <gtsam/geometry/Point2.h>
23 #include <gtsam/geometry/Point3.h>
24 
25 namespace gtsam {
26 
27 /* ************************************************************************* */
28 Vector9 Cal3Fisheye::vector() const {
29  Vector9 v;
30  v << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_;
31  return v;
32 }
33 
34 /* ************************************************************************* */
35 double Cal3Fisheye::Scaling(double r) {
36  static constexpr double threshold = 1e-8;
37  if (r > threshold || r < -threshold) {
38  return atan(r) / r;
39  } else {
40  // Taylor expansion close to 0
41  double r2 = r * r, r4 = r2 * r2;
42  return 1.0 - r2 / 3 + r4 / 5;
43  }
44 }
45 
46 /* ************************************************************************* */
48  OptionalJacobian<2, 2> H2) const {
49  const double xi = p.x(), yi = p.y(), zi = 1;
50  const double r2 = xi * xi + yi * yi, r = sqrt(r2);
51  const double t = atan2(r, zi);
52  const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4;
53  Vector5 K, T;
54  K << 1, k1_, k2_, k3_, k4_;
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;
59  Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_);
60 
61  Matrix2 DK;
62  if (H1 || H2) DK << fx_, s_, 0.0, fy_;
63 
64  // Derivative for calibration parameters (2 by 9)
65  if (H1) {
66  Matrix25 DR1;
67  // order: fx, fy, s, u0, v0
68  DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0;
69 
70  // order: k1, k2, k3, k4
71  Matrix24 DR2;
72  auto T4 = T.tail<4>().transpose();
73  DR2 << xi * T4, yi * T4;
74  *H1 << DR1, DK * scaling * DR2;
75  }
76 
77  // Derivative for points in intrinsic coords (2 by 2)
78  if (H2) {
79  if (r2==0) {
80  *H2 = DK;
81  } else {
82  const double dtd_dt =
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;
90 
91  const double c2 = dr_dxi * dr_dxi;
92  const double s2 = dr_dyi * dr_dyi;
93  const double cs = dr_dxi * dr_dyi;
94 
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);
99 
100  Matrix2 DR;
101  DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
102 
103  *H2 = DK * DR;
104  }
105  }
106 
107  return uv;
108 }
109 
110 /* ************************************************************************* */
112  OptionalJacobian<2, 2> Dp) const {
113  // Apply inverse camera matrix to map the pixel coordinate (u, v)
114  // of the equidistant fisheye image to angular coordinate space (xd, yd)
115  // with radius theta given in radians.
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);
120 
121  // Provide initial guess for the Gauss-Newton search.
122  // The angular coordinates given by (xd, yd) are mapped back to
123  // the focal plane of the perspective undistorted projection pi.
124  // See Cal3Unified.calibrate() using the same pattern for the
125  // undistortion of omnidirectional fisheye projection.
126  const double scale = (theta > 0) ? tan(theta) / theta : 1.0;
127  Point2 pi(scale * xd, scale * yd);
128 
129  // Perform newtons method, break when solution converges past tol_,
130  // throw exception if max iterations are reached
131  const int maxIterations = 10;
132  int iteration;
133  for (iteration = 0; iteration < maxIterations; ++iteration) {
134  Matrix2 jac;
135 
136  // Calculate the current estimate (uv_hat) and the jacobian
137  const Point2 uv_hat = uncalibrate(pi, {}, jac);
138 
139  // Test convergence
140  if ((uv_hat - uv).norm() < tol_) break;
141 
142  // Newton's method update step
143  pi = pi - jac.inverse() * (uv_hat - uv);
144  }
145 
146  if (iteration >= maxIterations)
147  throw std::runtime_error(
148  "Cal3Fisheye::calibrate fails to converge. need a better "
149  "initialization");
150 
151  calibrateJacobians<Cal3Fisheye, dimension>(*this, pi, Dcal, Dp);
152 
153  return pi;
154 }
155 
156 /* ************************************************************************* */
157 std::ostream& operator<<(std::ostream& os, const Cal3Fisheye& cal) {
158  os << (Cal3&)cal;
159  os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", k3: " << cal.k3()
160  << ", k4: " << cal.k4();
161  return os;
162 }
163 
164 /* ************************************************************************* */
165 void Cal3Fisheye::print(const std::string& s) const {
166  gtsam::print((Matrix)K(), s + ".K");
167  gtsam::print(Vector(k()), s + ".k");
168 }
169 
170 /* ************************************************************************* */
171 bool Cal3Fisheye::equals(const Cal3Fisheye& K, double tol) const {
172  const Cal3* base = dynamic_cast<const Cal3*>(&K);
173  return Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
174  std::fabs(k2_ - K.k2_) < tol && std::fabs(k3_ - K.k3_) < tol &&
175  std::fabs(k4_ - K.k4_) < tol;
176 }
177 
178 /* ************************************************************************* */
179 
180 } // \ namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
static double Scaling(double r)
Helper function that calculates atan(r)/r.
Definition: Cal3Fisheye.cpp:35
double k4_
fisheye distortion coefficients
Definition: Cal3Fisheye.h:54
void print(const std::string &s="") const override
print with optional string
double k2_
fisheye distortion coefficients
Definition: Cal3Fisheye.h:53
double tol_
tolerance value when calibrating
Definition: Cal3Fisheye.h:55
Vector2 Point2
Definition: Point2.h:32
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Cal3Fisheye &cal)
Output stream operator.
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
double u0_
Definition: Cal3.h:73
Real fabs(const Real &a)
double k3() const
First tangential distortion coefficient.
Definition: Cal3Fisheye.h:102
static const Similarity3 T4(R, P, s)
Common base class for all calibration models.
Definition: Cal3.h:69
Vector9 vector() const
Return all parameters as a vector.
Definition: Cal3Fisheye.cpp:28
Vector4 k() const
return distortion parameter vector
Definition: Cal3Fisheye.h:108
EIGEN_DEVICE_FUNC const AtanReturnType atan() const
Eigen::VectorXd Vector
Definition: Vector.h:38
double fy_
focal length
Definition: Cal3.h:71
bool equals(const Cal3 &K, double tol=10e-9) const
Check if equal up to specified tolerance.
Definition: Cal3.cpp:59
Array< int, Dynamic, 1 > v
Eigen::Triplet< double > T
double s_
skew
Definition: Cal3.h:72
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
double k2() const
Second distortion coefficient.
Definition: Cal3Fisheye.h:99
static const double r2
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]
Definition: Cal3Fisheye.cpp:47
Expression< Point2 > uv_hat(uncalibrate< Cal3_S2 >, K, projection)
double v0_
principal point
Definition: Cal3.h:73
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
Vector xi
Definition: testPose2.cpp:148
bool equals(const Cal3Fisheye &K, double tol=10e-9) const
assert equality up to a tolerance
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
double k4() const
Second tangential distortion coefficient.
Definition: Cal3Fisheye.h:105
ofstream os("timeSchurFactors.csv")
double k1() const
First distortion coefficient.
Definition: Cal3Fisheye.h:96
static double scale(double x, double a, double b, double t1, double t2)
Scale x from [a, b] to [t1, t2].
Definition: Chebyshev.cpp:35
EIGEN_DEVICE_FUNC const TanReturnType tan() const
3D Point
float * p
Calibration of a fisheye camera.
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Annotation indicating that a class derives from another given type.
Definition: attr.h:61
const G double tol
Definition: Group.h:86
double fx_
Definition: Cal3.h:71
2D Point
Calibration of a fisheye camera.
Definition: Cal3Fisheye.h:51
Point2 t(10, 10)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:00