Go to the documentation of this file.
37 os <<
", k1: " <<
cal.k1() <<
", k2: " <<
cal.k2() <<
", p1: " <<
cal.p1()
38 <<
", p2: " <<
cal.p2();
58 double xy,
double rr,
double r4,
double pnx,
59 double pny,
const Matrix2& DK) {
61 DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
63 DR2 <<
x * rr,
x * r4, 2 *
xy, rr + 2 * xx,
64 y * rr,
y * r4, rr + 2 * yy, 2 *
xy;
72 double k2,
double p1,
double p2,
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;
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);
87 DR <<
g +
x * dgdx + dDxdx,
x * dgdy + dDxdy,
88 y * dgdx + dDydx,
g +
y * dgdy + dDydy;
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;
106 const double dx = 2. *
p1_ *
xy +
p2_ * (rr + 2. * xx);
107 const double dy = 2. *
p2_ *
xy +
p1_ * (rr + 2. * yy);
110 const double pnx =
g *
x + dx;
111 const double pny =
g *
y + dy;
139 (1 /
fy_) * (pi.y() -
v0_));
146 const int maxIterations = 10;
148 for (iteration = 0; iteration < maxIterations; ++iteration) {
150 const double px = pn.x(),
py = pn.y(),
xy =
px *
py, xx =
px *
px,
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;
159 if (iteration >= maxIterations)
160 throw std::runtime_error(
161 "Cal3DS2::calibrate fails to converge. need a better initialization");
163 calibrateJacobians<Cal3DS2_Base, dimension>(*
this, pn, Dcal, Dp);
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);
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;
bool equals(const Cal3DS2_Base &K, double tol=1e-8) const
assert equality up to a tolerance
Annotation indicating that a class derives from another given type.
typedef and functions to augment Eigen's VectorXd
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Calibration of a camera with radial distortion.
typedef and functions to augment Eigen's MatrixXd
static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1, double k2, double p1, double p2, const Matrix2 &DK)
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
double py() const
image center in y
void print(const std::string &s="") const override
print with optional string
ofstream os("timeSchurFactors.csv")
Matrix2 D2d_intrinsic(const Point2 &p) const
Derivative of uncalibrate wrpt intrinsic coordinates.
Vector4 k() const
return distortion parameter vector
void print(const Matrix &A, const string &s, ostream &stream)
double p2_
tangential distortion
Vector9 vector() const
Return all parameters as a vector.
Common base class for all calibration models.
double k2_
radial 2nd-order and 4th-order
virtual Matrix3 K() const
return calibration matrix K
void g(const string &key, int i)
bool equals(const Cal3 &K, double tol=10e-9) const
Check if equal up to specified tolerance.
static Matrix29 D2dcalibration(double x, double y, double xx, double yy, double xy, double rr, double r4, double pnx, double pny, const Matrix2 &DK)
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Convert (distorted) image coordinates uv to intrinsic coordinates xy.
Array< int, Dynamic, 1 > v
double tol_
tolerance value when calibrating
double px() const
image center in x
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Matrix29 D2d_calibration(const Point2 &p) const
Derivative of uncalibrate wrpt the calibration parameters.
double v0_
principal point
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:03