Inlined derivatives and fixed one issue with Jacobians around image center
Frank dellaert
parent
6a23c476a1
commit
83d0b9d3ff
|
|
@ -24,7 +24,7 @@
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Fisheye::Cal3Fisheye(const Vector& v)
|
||||
Cal3Fisheye::Cal3Fisheye(const Vector9& v)
|
||||
: fx_(v[0]),
|
||||
fy_(v[1]),
|
||||
s_(v[2]),
|
||||
|
|
@ -50,76 +50,73 @@ Matrix3 Cal3Fisheye::K() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Matrix29 D2dcalibration(const double xd, const double yd,
|
||||
const double xi, const double yi,
|
||||
const double t3, const double t5,
|
||||
const double t7, const double t9, const double r,
|
||||
Matrix2& DK) {
|
||||
// order: fx, fy, s, u0, v0
|
||||
Matrix25 DR1;
|
||||
DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0;
|
||||
|
||||
// order: k1, k2, k3, k4
|
||||
Matrix24 DR2;
|
||||
DR2 << t3 * xi, t5 * xi, t7 * xi, t9 * xi, t3 * yi, t5 * yi, t7 * yi, t9 * yi;
|
||||
DR2 /= r;
|
||||
Matrix29 D;
|
||||
D << DR1, DK * DR2;
|
||||
return D;
|
||||
double Cal3Fisheye::Scaling(double r) {
|
||||
static constexpr double threshold = 1e-8;
|
||||
if (r > threshold || r < -threshold) {
|
||||
return atan(r) / r;
|
||||
} else {
|
||||
// Taylor expansion close to 0
|
||||
double r2 = r * r, r4 = r2 * r2;
|
||||
return 1.0 - r2 / 3 + r4 / 5;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Matrix2 D2dintrinsic(const double xi, const double yi, const double r,
|
||||
const double td, const double t, const double tt,
|
||||
const double t4, const double t6, const double t8,
|
||||
const double k1, const double k2, const double k3,
|
||||
const double k4, const Matrix2& DK) {
|
||||
const double dr_dxi = xi / sqrt(xi * xi + yi * yi);
|
||||
const double dr_dyi = yi / sqrt(xi * xi + yi * yi);
|
||||
const double dt_dr = 1 / (1 + r * r);
|
||||
const double dtd_dt =
|
||||
1 + 3 * k1 * tt + 5 * k2 * t4 + 7 * k3 * t6 + 9 * k4 * t8;
|
||||
const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
|
||||
const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
|
||||
|
||||
const double rinv = 1 / r;
|
||||
const double rrinv = 1 / (r * r);
|
||||
const double dxd_dxi =
|
||||
dtd_dxi * xi * rinv + td * rinv + td * xi * (-rrinv) * dr_dxi;
|
||||
const double dxd_dyi = dtd_dyi * xi * rinv - td * xi * rrinv * dr_dyi;
|
||||
const double dyd_dxi = dtd_dxi * yi * rinv - td * yi * rrinv * dr_dxi;
|
||||
const double dyd_dyi =
|
||||
dtd_dyi * yi * rinv + td * rinv + td * yi * (-rrinv) * dr_dyi;
|
||||
|
||||
Matrix2 DR;
|
||||
DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
|
||||
|
||||
return DK * DR;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
|
||||
OptionalJacobian<2, 2> H2) const {
|
||||
const double xi = p.x(), yi = p.y();
|
||||
const double r = sqrt(xi * xi + yi * yi);
|
||||
const double r2 = xi * xi + yi * yi, r = sqrt(r2);
|
||||
const double t = atan(r);
|
||||
const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
|
||||
const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
|
||||
const double td_o_r = r > 1e-8 ? td / r : 1;
|
||||
const double xd = td_o_r * xi, yd = td_o_r * yi;
|
||||
const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4;
|
||||
Vector5 K, T;
|
||||
K << 1, k1_, k2_, k3_, k4_;
|
||||
T << 1, t2, t4, t6, t8;
|
||||
const double scaling = Scaling(r);
|
||||
const double s = scaling * K.dot(T);
|
||||
const double xd = s * xi, yd = s * yi;
|
||||
Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_);
|
||||
|
||||
Matrix2 DK;
|
||||
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
|
||||
|
||||
// Derivative for calibration parameters (2 by 9)
|
||||
if (H1)
|
||||
*H1 = D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK);
|
||||
if (H1) {
|
||||
Matrix25 DR1;
|
||||
// order: fx, fy, s, u0, v0
|
||||
DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0;
|
||||
|
||||
// order: k1, k2, k3, k4
|
||||
Matrix24 DR2;
|
||||
auto T4 = T.tail<4>().transpose();
|
||||
DR2 << xi * T4, yi * T4;
|
||||
*H1 << DR1, DK * scaling * DR2;
|
||||
}
|
||||
|
||||
// Derivative for points in intrinsic coords (2 by 2)
|
||||
if (H2)
|
||||
*H2 =
|
||||
D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK);
|
||||
if (H2) {
|
||||
const double dtd_dt =
|
||||
1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8;
|
||||
const double dt_dr = 1 / (1 + r2);
|
||||
const double rinv = 1 / r;
|
||||
const double dr_dxi = xi * rinv;
|
||||
const double dr_dyi = yi * rinv;
|
||||
const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
|
||||
const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
|
||||
|
||||
const double td = t * K.dot(T);
|
||||
const double rrinv = 1 / r2;
|
||||
const double dxd_dxi =
|
||||
dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi;
|
||||
const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi;
|
||||
const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi;
|
||||
const double dyd_dyi =
|
||||
dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi;
|
||||
|
||||
Matrix2 DR;
|
||||
DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
|
||||
|
||||
*H2 = DK * DR;
|
||||
}
|
||||
|
||||
return uv;
|
||||
}
|
||||
|
|
@ -157,39 +154,10 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
|
|||
return pi;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix2 Cal3Fisheye::D2d_intrinsic(const Point2& p) const {
|
||||
const double xi = p.x(), yi = p.y();
|
||||
const double r = sqrt(xi * xi + yi * yi);
|
||||
const double t = atan(r);
|
||||
const double tt = t * t, t4 = tt * tt, t6 = t4 * tt, t8 = t4 * t4;
|
||||
const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
|
||||
|
||||
Matrix2 DK;
|
||||
DK << fx_, s_, 0.0, fy_;
|
||||
|
||||
return D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix29 Cal3Fisheye::D2d_calibration(const Point2& p) const {
|
||||
const double xi = p.x(), yi = p.y();
|
||||
const double r = sqrt(xi * xi + yi * yi);
|
||||
const double t = atan(r);
|
||||
const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
|
||||
const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
|
||||
const double xd = td / r * xi, yd = td / r * yi;
|
||||
|
||||
Matrix2 DK;
|
||||
DK << fx_, s_, 0.0, fy_;
|
||||
return D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Cal3Fisheye::print(const std::string& s_) const {
|
||||
gtsam::print((Matrix)K(), s_ + ".K");
|
||||
gtsam::print(Vector(k()), s_ + ".k");
|
||||
;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -20,6 +20,8 @@
|
|||
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
|
|
@ -43,7 +45,7 @@ namespace gtsam {
|
|||
* [u; v; 1] = K*[x_d; y_d; 1]
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3Fisheye {
|
||||
protected:
|
||||
private:
|
||||
double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point
|
||||
double k1_, k2_, k3_, k4_; // fisheye distortion coefficients
|
||||
|
||||
|
|
@ -78,7 +80,7 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
Cal3Fisheye(const Vector& v);
|
||||
explicit Cal3Fisheye(const Vector9& v);
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
|
|
@ -120,6 +122,9 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
/// Return all parameters as a vector
|
||||
Vector9 vector() const;
|
||||
|
||||
/// Helper function that calculates atan(r)/r
|
||||
static double Scaling(double r);
|
||||
|
||||
/**
|
||||
* @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image
|
||||
* coordinates [u; v]
|
||||
|
|
@ -136,13 +141,6 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
/// y_i]
|
||||
Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
|
||||
|
||||
/// Derivative of uncalibrate wrpt intrinsic coordinates [x_i; y_i]
|
||||
Matrix2 D2d_intrinsic(const Point2& p) const;
|
||||
|
||||
/// Derivative of uncalibrate wrpt the calibration parameters
|
||||
/// [fx, fy, s, u0, v0, k1, k2, k3, k4]
|
||||
Matrix29 D2d_calibration(const Point2& p) const;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
|
|
|||
Loading…
Reference in New Issue