Cal3DS2_Base.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 Cal3DS2_Base::vector() const {
29  Vector9 v;
30  v << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_;
31  return v;
32 }
33 
34 /* ************************************************************************* */
35 std::ostream& operator<<(std::ostream& os, const Cal3DS2_Base& cal) {
36  os << (Cal3&)cal;
37  os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", p1: " << cal.p1()
38  << ", p2: " << cal.p2();
39  return os;
40 }
41 
42 /* ************************************************************************* */
43 void Cal3DS2_Base::print(const std::string& s) const {
44  gtsam::print((Matrix)K(), s + ".K");
45  gtsam::print(Vector(k()), s + ".k");
46 }
47 
48 /* ************************************************************************* */
49 bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
50  const Cal3* base = dynamic_cast<const Cal3*>(&K);
51  return Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
52  std::fabs(k2_ - K.k2_) < tol && std::fabs(p1_ - K.p1_) < tol &&
53  std::fabs(p2_ - K.p2_) < tol;
54 }
55 
56 /* ************************************************************************* */
57 static Matrix29 D2dcalibration(double x, double y, double xx, double yy,
58  double xy, double rr, double r4, double pnx,
59  double pny, const Matrix2& DK) {
60  Matrix25 DR1;
61  DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
62  Matrix24 DR2;
63  DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
64  y * rr, y * r4, rr + 2 * yy, 2 * xy;
65  Matrix29 D;
66  D << DR1, DK * DR2;
67  return D;
68 }
69 
70 /* ************************************************************************* */
71 static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1,
72  double k2, double p1, double p2,
73  const Matrix2& DK) {
74  const double drdx = 2. * x;
75  const double drdy = 2. * y;
76  const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
77  const double dgdy = k1 * drdy + k2 * 2. * rr * drdy;
78 
79  // Dx = 2*p1*xy + p2*(rr+2*xx);
80  // Dy = 2*p2*xy + p1*(rr+2*yy);
81  const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x);
82  const double dDxdy = 2. * p1 * x + p2 * drdy;
83  const double dDydx = 2. * p2 * y + p1 * drdx;
84  const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
85 
86  Matrix2 DR;
87  DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
88  y * dgdx + dDydx, g + y * dgdy + dDydy;
89 
90  return DK * DR;
91 }
92 
93 /* ************************************************************************* */
95  OptionalJacobian<2, 2> Dp) const {
96  // r² = x² + y²;
97  // g = (1 + k(1)*r² + k(2)*r⁴);
98  // dp = [2*k(3)*x*y + k(4)*(r² + 2*x²); 2*k(4)*x*y + k(3)*(r² + 2*y²)];
99  // pi(:,i) = g * pn(:,i) + dp;
100  const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
101  const double rr = xx + yy;
102  const double r4 = rr * rr;
103  const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
104 
105  // tangential component
106  const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
107  const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
108 
109  // Radial and tangential distortion applied
110  const double pnx = g * x + dx;
111  const double pny = g * y + dy;
112 
113  Matrix2 DK;
114  if (Dcal || Dp) {
115  DK << fx_, s_, 0.0, fy_;
116  }
117 
118  // Derivative for calibration
119  if (Dcal) {
120  *Dcal = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
121  }
122 
123  // Derivative for points
124  if (Dp) {
125  *Dp = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
126  }
127 
128  // Regular uncalibrate after distortion
129  return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
130 }
131 
132 /* ************************************************************************* */
134  OptionalJacobian<2, 2> Dp) const {
135  // Use the following fixed point iteration to invert the radial distortion.
136  // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
137 
138  const Point2 invKPi((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
139  (1 / fy_) * (pi.y() - v0_));
140 
141  // initialize by ignoring the distortion at all, might be problematic for
142  // pixels around boundary
143  Point2 pn = invKPi;
144 
145  // iterate until the uncalibrate is close to the actual pixel coordinate
146  const int maxIterations = 10;
147  int iteration;
148  for (iteration = 0; iteration < maxIterations; ++iteration) {
149  if (distance2(uncalibrate(pn), pi) <= tol_) break;
150  const double px = pn.x(), py = pn.y(), xy = px * py, xx = px * px,
151  yy = py * py;
152  const double rr = xx + yy;
153  const double g = (1 + k1_ * rr + k2_ * rr * rr);
154  const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
155  const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
156  pn = (invKPi - Point2(dx, dy)) / g;
157  }
158 
159  if (iteration >= maxIterations)
160  throw std::runtime_error(
161  "Cal3DS2::calibrate fails to converge. need a better initialization");
162 
163  calibrateJacobians<Cal3DS2_Base, dimension>(*this, pn, Dcal, Dp);
164 
165  return pn;
166 }
167 
168 /* ************************************************************************* */
169 Matrix2 Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
170  const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
171  const double rr = xx + yy;
172  const double r4 = rr * rr;
173  const double g = (1 + k1_ * rr + k2_ * r4);
174  Matrix2 DK;
175  DK << fx_, s_, 0.0, fy_;
176  return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
177 }
178 
179 /* ************************************************************************* */
180 Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const {
181  const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
182  const double rr = xx + yy;
183  const double r4 = rr * rr;
184  const double g = (1 + k1_ * rr + k2_ * r4);
185  const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
186  const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
187  const double pnx = g * x + dx;
188  const double pny = g * y + dy;
189  Matrix2 DK;
190  DK << fx_, s_, 0.0, fy_;
191  return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
192 }
193 }
194 /* ************************************************************************* */
gtsam::Cal3::fx_
double fx_
Definition: Cal3.h:71
gtsam::Cal3DS2_Base::equals
bool equals(const Cal3DS2_Base &K, double tol=1e-8) const
assert equality up to a tolerance
Definition: Cal3DS2_Base.cpp:49
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.cal
cal
Definition: test_ProjectionFactorRollingShutter.py:27
base
Annotation indicating that a class derives from another given type.
Definition: attr.h:64
D
MatrixXcd D
Definition: EigenSolver_EigenSolver_MatrixType.cpp:14
Vector.h
typedef and functions to augment Eigen's VectorXd
gtsam::Cal3DS2_Base::k1_
double k1_
Definition: Cal3DS2_Base.h:44
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
DR1
static double DR1
Definition: j0.c:159
xy
Matrix< float, 2, 1 > xy
Definition: LLT_solve.cpp:7
gtsam::distance2
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Definition: Point2.cpp:39
gtsam::Cal3DS2_Base
Calibration of a camera with radial distortion.
Definition: Cal3DS2_Base.h:42
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::D2dintrinsic
static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1, double k2, double p1, double p2, const Matrix2 &DK)
Definition: Cal3DS2_Base.cpp:71
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::Cal3::py
double py() const
image center in y
Definition: Cal3.h:154
gtsam::Cal3DS2_Base::p1_
double p1_
Definition: Cal3DS2_Base.h:45
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Point3.h
3D Point
gtsam::Cal3DS2_Base::print
void print(const std::string &s="") const override
print with optional string
Definition: Cal3DS2_Base.cpp:43
os
ofstream os("timeSchurFactors.csv")
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::Cal3::u0_
double u0_
Definition: Cal3.h:73
gtsam::Cal3DS2_Base::D2d_intrinsic
Matrix2 D2d_intrinsic(const Point2 &p) const
Derivative of uncalibrate wrpt intrinsic coordinates.
Definition: Cal3DS2_Base.cpp:169
Point2.h
2D Point
boost::multiprecision::fabs
Real fabs(const Real &a)
Definition: boostmultiprec.cpp:119
gtsam::Cal3DS2_Base::k
Vector4 k() const
return distortion parameter vector
Definition: Cal3DS2_Base.h:113
DR2
static double DR2
Definition: j0.c:162
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::Cal3DS2_Base::p2_
double p2_
tangential distortion
Definition: Cal3DS2_Base.h:45
gtsam::Cal3DS2_Base::vector
Vector9 vector() const
Return all parameters as a vector.
Definition: Cal3DS2_Base.cpp:28
k1
double k1(double x)
Definition: k1.c:133
gtsam::Cal3
Common base class for all calibration models.
Definition: Cal3.h:69
gtsam::Cal3DS2_Base::k2_
double k2_
radial 2nd-order and 4th-order
Definition: Cal3DS2_Base.h:44
gtsam::Cal3::fy_
double fy_
focal length
Definition: Cal3.h:71
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::Cal3::K
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
y
Scalar * y
Definition: level1_cplx_impl.h:124
gtsam::Cal3::equals
bool equals(const Cal3 &K, double tol=10e-9) const
Check if equal up to specified tolerance.
Definition: Cal3.cpp:59
gtsam
traits
Definition: SFMdata.h:40
K
#define K
Definition: igam.h:8
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::D2dcalibration
static Matrix29 D2dcalibration(double x, double y, double xx, double yy, double xy, double rr, double r4, double pnx, double pny, const Matrix2 &DK)
Definition: Cal3DS2_Base.cpp:57
gtsam::Cal3DS2_Base::calibrate
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Convert (distorted) image coordinates uv to intrinsic coordinates xy.
Definition: Cal3DS2_Base.cpp:133
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::tol
const G double tol
Definition: Group.h:79
pybind11
Definition: wrap/pybind11/pybind11/__init__.py:1
gtsam::Cal3DS2_Base::tol_
double tol_
tolerance value when calibrating
Definition: Cal3DS2_Base.h:46
gtsam::Cal3::px
double px() const
image center in x
Definition: Cal3.h:151
gtsam::Cal3::s_
double s_
skew
Definition: Cal3.h:72
Cal3DS2_Base.h
gtsam::Cal3DS2_Base::uncalibrate
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3DS2_Base.cpp:94
gtsam::Cal3DS2_Base::D2d_calibration
Matrix29 D2d_calibration(const Point2 &p) const
Derivative of uncalibrate wrpt the calibration parameters.
Definition: Cal3DS2_Base.cpp:180
gtsam::Cal3::v0_
double v0_
principal point
Definition: Cal3.h:73


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:03