Pinhole Radtan 4 example

pinhole-radtan4.hpp

template <typename Derived_>

class PinholeRadTan4CameraBase
    : public AbstractCamera<
          typename Eigen::internal::traits<Derived_>::Scalar> {
 public:
  DBATK_DEFINE_CAMERA_MODEL_BASE_CLASS_BOILERPLATE(PinholeRadTan4Camera)

  DBATK_DEFINE_CAMERA_MODEL_ACCESSOR(fx, 0);
  DBATK_DEFINE_CAMERA_MODEL_ACCESSOR(fy, 1);
  DBATK_DEFINE_CAMERA_MODEL_ACCESSOR(cx, 2);
  DBATK_DEFINE_CAMERA_MODEL_ACCESSOR(cy, 3);
  DBATK_DEFINE_CAMERA_MODEL_ACCESSOR(k1, 4);
  DBATK_DEFINE_CAMERA_MODEL_ACCESSOR(k2, 5);
  DBATK_DEFINE_CAMERA_MODEL_ACCESSOR(r1, 6);
  DBATK_DEFINE_CAMERA_MODEL_ACCESSOR(r2, 7);

  template <class PointScalar>
  inline bool project_inline(
      const Vec4Type<PointScalar>& p, Vec2Type<ResScalar<PointScalar>>& proj,
      Mat24Type<ResScalar<PointScalar>>* d_proj_d_p3d = nullptr) const {
    using ReturnScalar = ResScalar<PointScalar>;

    CHECK_GE(p.w(), 0.0)
        << "projection and jacobian (currently) don't consider w < 0";

    // validity check
    if (p.z() <= CameraModelConstants<PointScalar>::epsilon()) return false;

    const Scalar one{1.0};
    const Scalar two{2.0};
    const Scalar six{6.0};

    const PointScalar& x = p.x();
    const PointScalar& y = p.y();
    const PointScalar& z = p.z();

    const PointScalar mx = x / z;
    const PointScalar my = y / z;

    const PointScalar mx2 = mx * mx;
    const PointScalar my2 = my * my;
    const PointScalar mxy = mx * my;
    const PointScalar rho2 = mx2 + my2;
    const ReturnScalar rad_dist = k1() * rho2 + k2() * rho2 * rho2;

    const ReturnScalar x_dist =
        mx + mx * rad_dist + two * r1() * mxy + r2() * (rho2 + two * mx2);
    const ReturnScalar y_dist =
        my + my * rad_dist + two * r2() * mxy + r1() * (rho2 + two * my2);

    proj.x() = fx() * x_dist + cx();
    proj.y() = fy() * y_dist + cy();

    if (d_proj_d_p3d) {
      const ReturnScalar d_raddist_d_mx = two * mx * (k1() + two * k2() * rho2);
      const ReturnScalar d_raddist_d_my = two * my * (k1() + two * k2() * rho2);

      const ReturnScalar d_xdist_d_mx =
          one + rad_dist + mx * (d_raddist_d_mx + six * r2()) + two * r1() * my;
      const ReturnScalar d_ydist_d_my =
          one + rad_dist + my * (d_raddist_d_my + six * r1()) + two * r2() * mx;

      const ReturnScalar tmp = two * (r1() * mx + r2() * my);
      const ReturnScalar d_xdist_d_my = mx * d_raddist_d_my + tmp;
      const ReturnScalar d_ydist_d_mx = my * d_raddist_d_mx + tmp;

      (*d_proj_d_p3d)(0, 0) = fx() * d_xdist_d_mx / z;
      (*d_proj_d_p3d)(0, 1) = fx() * d_xdist_d_my / z;
      (*d_proj_d_p3d)(1, 0) = fy() * d_ydist_d_mx / z;
      (*d_proj_d_p3d)(1, 1) = fy() * d_ydist_d_my / z;
      (*d_proj_d_p3d)(0, 2) =
          -fx() * (d_xdist_d_mx * mx + d_xdist_d_my * my) / z;
      (*d_proj_d_p3d)(1, 2) =
          -fy() * (d_ydist_d_mx * mx + d_ydist_d_my * my) / z;
      (*d_proj_d_p3d)(0, 3) = ReturnScalar(0.0);
      (*d_proj_d_p3d)(1, 3) = ReturnScalar(0.0);
    }

    return true;
  }

  // helper to distort normalized (z=1) coordinates; used to invert the
  // distortion function in `unproject`
  template <class PointScalar>
  inline void distort(const Vec2Type<PointScalar>& p_undist,
                      Vec2Type<ResScalar<PointScalar>>& p_dist,
                      Eigen::Matrix<ResScalar<PointScalar>, 2, 2>*
                          d_dist_d_undist = nullptr) const {
    using ReturnScalar = ResScalar<PointScalar>;

    const Scalar one{1.0};
    const Scalar two{2.0};
    const Scalar six{6.0};

    const PointScalar mx = p_undist.x();
    const PointScalar my = p_undist.y();

    const PointScalar mx2 = mx * mx;
    const PointScalar my2 = my * my;
    const PointScalar mxy = mx * my;
    const PointScalar rho2 = mx2 + my2;
    const ReturnScalar rad_dist = k1() * rho2 + k2() * rho2 * rho2;

    p_dist.x() =
        mx + mx * rad_dist + two * r1() * mxy + r2() * (rho2 + two * mx2);
    p_dist.y() =
        my + my * rad_dist + two * r2() * mxy + r1() * (rho2 + two * my2);

    if (d_dist_d_undist) {
      const ReturnScalar d_raddist_d_mx = two * mx * (k1() + two * k2() * rho2);
      const ReturnScalar d_raddist_d_my = two * my * (k1() + two * k2() * rho2);

      const ReturnScalar d_xdist_d_mx =
          one + rad_dist + mx * (d_raddist_d_mx + six * r2()) + two * r1() * my;
      const ReturnScalar d_ydist_d_my =
          one + rad_dist + my * (d_raddist_d_my + six * r1()) + two * r2() * mx;

      const ReturnScalar tmp = two * (r1() * mx + r2() * my);
      const ReturnScalar d_xdist_d_my = mx * d_raddist_d_my + tmp;
      const ReturnScalar d_ydist_d_mx = my * d_raddist_d_mx + tmp;

      (*d_dist_d_undist)(0, 0) = d_xdist_d_mx;
      (*d_dist_d_undist)(0, 1) = d_xdist_d_my;
      (*d_dist_d_undist)(1, 0) = d_ydist_d_mx;
      (*d_dist_d_undist)(1, 1) = d_ydist_d_my;
    }
  }

  template <class PointScalar>
  inline bool unproject_inline(
      const Vec2Type<PointScalar>& proj, Vec4Type<ResScalar<PointScalar>>& p3d,
      Mat42Type<ResScalar<PointScalar>>* d_p3d_d_proj = nullptr) const {
    using ReturnScalar = ResScalar<PointScalar>;

    // use unqualified calls to enable ADL for ceres::Jet types
    using std::sqrt;

    const auto eps2 = CameraModelConstants<PointScalar>::epsilon() *
                      CameraModelConstants<PointScalar>::epsilon();
    const Scalar one{1.0};

    Vec2Type<ReturnScalar> p_dist;
    p_dist.x() = (proj.x() - cx()) / fx();
    p_dist.y() = (proj.y() - cy()) / fy();

    // invert distortion with Newton's method
    const int N = 15;
    Vec2Type<ReturnScalar> p_undist = p_dist;

    Vec2Type<ReturnScalar> f;
    Eigen::Matrix<ReturnScalar, 2, 2> J;
    for (int i = 0; i < N; ++i) {
      distort(p_undist, f, &J);
      const auto e = (f - p_dist).eval();
      p_undist -= J.inverse() * e;

      if (e.squaredNorm() < eps2) {
        break;
      }
    }

    // normalize to unit vector
    const auto mx = p_undist.x();
    const auto my = p_undist.y();

    const auto r2 = mx * mx + my * my;

    const auto norm = sqrt(one + r2);
    const auto norm_inv = one / norm;

    p3d[0] = mx * norm_inv;
    p3d[1] = my * norm_inv;
    p3d[2] = norm_inv;
    p3d[3] = ReturnScalar(0.0);

    if (d_p3d_d_proj) {
      // use inverse function thorem for jacobian of undistort function:
      // J(F^-1)(p) = J(F)(F^-1(p))^-1
      distort(p_undist, f, &J);
      Eigen::Matrix<ReturnScalar, 2, 2> Jundist = J.inverse();

      Jundist.template topRows<1>() /= fx();
      Jundist.template bottomRows<1>() /= fy();

      const auto d_norm_inv_d_r2 = -0.5 * norm_inv * norm_inv * norm_inv;
      Eigen::Matrix<ReturnScalar, 3, 2> Jnormalize;
      Jnormalize(0, 0) = (norm_inv + 2.0 * mx * mx * d_norm_inv_d_r2);
      Jnormalize(1, 0) = (2.0 * my * mx * d_norm_inv_d_r2);
      Jnormalize(2, 0) = 2.0 * mx * d_norm_inv_d_r2;

      Jnormalize(0, 1) = (2.0 * my * mx * d_norm_inv_d_r2);
      Jnormalize(1, 1) = (norm_inv + 2.0 * my * my * d_norm_inv_d_r2);
      Jnormalize(2, 1) = 2.0 * my * d_norm_inv_d_r2;

      d_p3d_d_proj->template topRows<3>() = Jnormalize * Jundist;
      d_p3d_d_proj->template bottomRows<1>().setZero();
    }

    // validity check: all x in R^2 can be unprojected
    return true;
  }

  void scale_to_pyramid_level(int level) {
    int scale_factor = 1 << level;
    width_ >>= level;
    height_ >>= level;
    fx() /= scale_factor;
    fy() /= scale_factor;
    cx() /= scale_factor;
    cy() /= scale_factor;
  }

  static Derived get_test_projection() {
    PlainParamType vec;
    vec << 458.654, 457.296, 367.215, 248.375, -0.28340811, 0.07395907,
        0.00019359, 1.76187114e-05;
    return {752, 480, vec};
  }
};