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
gtsam::Cal3::fx_
double fx_
Definition: Cal3.h:71
base
Annotation indicating that a class derives from another given type.
Definition: attr.h:64
gtsam::Cal3Fisheye::k4_
double k4_
fisheye distortion coefficients
Definition: Cal3Fisheye.h:54
Vector.h
typedef and functions to augment Eigen's VectorXd
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
atan
const EIGEN_DEVICE_FUNC AtanReturnType atan() const
Definition: ArrayCwiseUnaryOps.h:283
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
DR1
static double DR1
Definition: j0.c:159
r2
static const double r2
Definition: testSmartRangeFactor.cpp:32
gtsam::Cal3Fisheye::calibrate
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3Fisheye.cpp:111
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::Cal3Fisheye::k2_
double k2_
fisheye distortion coefficients
Definition: Cal3Fisheye.h:53
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Point3.h
3D Point
os
ofstream os("timeSchurFactors.csv")
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::Cal3::u0_
double u0_
Definition: Cal3.h:73
gtsam::Cal3Fisheye::k
Vector4 k() const
return distortion parameter vector
Definition: Cal3Fisheye.h:108
fixture::cal
const Cal3_S2 cal(focalLength, focalLength, 0.0, principalPoint.x(), principalPoint.y())
Point2.h
2D Point
boost::multiprecision::fabs
Real fabs(const Real &a)
Definition: boostmultiprec.cpp:119
gtsam::Cal3Fisheye::equals
bool equals(const Cal3Fisheye &K, double tol=10e-9) const
assert equality up to a tolerance
Definition: Cal3Fisheye.cpp:171
DR2
static double DR2
Definition: j0.c:162
gtsam::Cal3Fisheye::k3_
double k3_
Definition: Cal3Fisheye.h:54
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
T4
static const Similarity3 T4(R, P, s)
gtsam::Cal3Fisheye::tol_
double tol_
tolerance value when calibrating
Definition: Cal3Fisheye.h:55
gtsam::Cal3
Common base class for all calibration models.
Definition: Cal3.h:69
gtsam::Cal3Fisheye
Calibration of a fisheye camera.
Definition: Cal3Fisheye.h:51
gtsam::Cal3Fisheye::Scaling
static double Scaling(double r)
Helper function that calculates atan(r)/r.
Definition: Cal3Fisheye.cpp:35
gtsam::Cal3::fy_
double fy_
focal length
Definition: Cal3.h:71
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
Eigen::Triplet< double >
gtsam::Cal3::K
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
tan
const EIGEN_DEVICE_FUNC TanReturnType tan() const
Definition: ArrayCwiseUnaryOps.h:269
atan2
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
Definition: AnnoyingScalar.h:110
gtsam::Cal3::equals
bool equals(const Cal3 &K, double tol=10e-9) const
Check if equal up to specified tolerance.
Definition: Cal3.cpp:59
so3::R2
SO3 R2
Definition: testShonanFactor.cpp:43
gtsam::Cal3Fisheye::print
void print(const std::string &s="") const override
print with optional string
Definition: Cal3Fisheye.cpp:165
gtsam
traits
Definition: SFMdata.h:40
gtsam::Cal3Fisheye::vector
Vector9 vector() const
Return all parameters as a vector.
Definition: Cal3Fisheye.cpp:28
K
#define K
Definition: igam.h:8
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
p
float * p
Definition: Tutorial_Map_using.cpp:9
c2
static double c2
Definition: airy.c:55
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::scale
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
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Cal3Fisheye::uncalibrate
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
tree::uv_hat
Expression< Point2 > uv_hat(uncalibrate< Cal3_S2 >, K, projection)
gtsam::Cal3::s_
double s_
skew
Definition: Cal3.h:72
align_3::t
Point2 t(10, 10)
gtsam::Cal3Fisheye::k1_
double k1_
Definition: Cal3Fisheye.h:53
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
gtsam::Cal3::v0_
double v0_
principal point
Definition: Cal3.h:73
Cal3Fisheye.h
Calibration of a fisheye camera.


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:01:56