12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix missing semicolons

release/4.3a0
roderick-koehle 2021-10-28 13:23:18 +02:00 committed by GitHub
parent c0219c1ad0
commit 0d01e4844f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -32,7 +32,7 @@ Vector9 Cal3Fisheye::vector() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Cal3Fisheye::Scaling(double r) { double Cal3Fisheye::Scaling(double r, double zi) {
static constexpr double threshold = 1e-8; static constexpr double threshold = 1e-8;
if (r > threshold || r < -threshold) { if (r > threshold || r < -threshold) {
return atan(r) / r; return atan(r) / r;
@ -48,12 +48,12 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
OptionalJacobian<2, 2> H2) const { OptionalJacobian<2, 2> H2) const {
const double xi = p.x(), yi = p.y(), zi = 1; const double xi = p.x(), yi = p.y(), zi = 1;
const double r2 = xi * xi + yi * yi, r = sqrt(r2); const double r2 = xi * xi + yi * yi, r = sqrt(r2);
const double t = atan(r); const double t = atan2(r, zi);
const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4; const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4;
Vector5 K, T; Vector5 K, T;
K << 1, k1_, k2_, k3_, k4_; K << 1, k1_, k2_, k3_, k4_;
T << 1, t2, t4, t6, t8; T << 1, t2, t4, t6, t8;
const double scaling = Scaling(r); const double scaling = Scaling(r, zi);
const double s = scaling * K.dot(T); const double s = scaling * K.dot(T);
const double xd = s * xi, yd = s * yi; const double xd = s * xi, yd = s * yi;
Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_); Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_);
@ -81,18 +81,18 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
} else { } else {
const double dtd_dt = const double dtd_dt =
1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8;
const double R2 = r2 + zi*zi const double R2 = r2 + zi*zi;
const double dt_dr = zi / R2; const double dt_dr = zi / R2;
const double rinv = 1 / r; const double rinv = 1 / r;
const double dr_dxi = xi * rinv; const double dr_dxi = xi * rinv;
const double dr_dyi = yi * rinv; const double dr_dyi = yi * rinv;
const double dtd_dr = dtd_dt * dt_dr const double dtd_dr = dtd_dt * dt_dr;
// const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; // const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
// const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; // const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
const double c2 = dr_dxi * dr_dxi const double c2 = dr_dxi * dr_dxi;
const double s2 = dr_dyi * dr_dyi const double s2 = dr_dyi * dr_dyi;
const double cs = dr_dxi * dr_dyi const double cs = dr_dxi * dr_dyi;
// Following refactoring is numerically stable, even for unnormalized radial // Following refactoring is numerically stable, even for unnormalized radial
// values by avoiding division with the square radius. // values by avoiding division with the square radius.