static void ForwardDerivatives()

in opensfm/src/geometry/camera_distortions_functions.h [709:757]

• 44 lines of code
• 2 McCabe index (conditional complexity)

  static void ForwardDerivatives(const T* point, const T* k, T* distorted,
                                 T* jacobian) {
    // dx, dy, dk1, dk2, dk3, dp1, dp2
    constexpr int stride = Stride<DERIV_PARAMS>();
    const auto& k1 = k[static_cast<int>(K1)];
    const auto& k2 = k[static_cast<int>(K2)];
    const auto& k3 = k[static_cast<int>(K3)];
    const auto& p1 = k[static_cast<int>(P1)];
    const auto& p2 = k[static_cast<int>(P2)];
    const auto& x = point[0];
    const auto& y = point[1];

    const T x2 = x * x;
    const T x4 = x2 * x2;
    const T y2 = y * y;
    const T y4 = y2 * y2;
    const T r2 = x2 + y2;
    const T r4 = r2 * r2;
    const T r6 = r4 * r2;

    jacobian[0] = T(5.0) * k2 * x4 + T(3.0) * k1 * x2 + T(6.0) * k3 * x2 * r4 +
                  T(6.0) * k2 * x2 * y2 + k3 * r6 + k2 * y4 + k1 * y2 + T(1.0) +
                  T(2.0) * p1 * y + T(6.0) * p2 * x;
    jacobian[1] =
        x * (T(2.0) * k1 * y + T(4.0) * k2 * y * r2 + T(6.0) * k3 * y * r4) +
        T(2.0) * p1 * x + T(2.0) * p2 * y;
    jacobian[stride + 1] = T(5.0) * k2 * y4 + T(3.0) * k1 * y2 +
                           T(6.0) * k3 * y2 * r4 + T(6.0) * k2 * x2 * y2 +
                           k3 * r6 + k2 * x4 + k1 * x2 + T(1.0) +
                           T(2.0) * p2 * x + T(6.0) * p1 * y;
    jacobian[stride] =
        y * (T(2.0) * k1 * x + T(4.0) * k2 * x * r2 + T(6.0) * k3 * x * r4) +
        T(2.0) * p2 * y + T(2.0) * p1 * x;

    if (DERIV_PARAMS) {
      jacobian[2] = x * r2;
      jacobian[3] = x * r2 * r2;
      jacobian[4] = x * r2 * r2 * r2;
      jacobian[5] = T(2.0) * x * y;
      jacobian[6] = T(3.0) * x2 + y2;
      jacobian[stride + 2] = y * r2;
      jacobian[stride + 3] = y * r2 * r2;
      jacobian[stride + 4] = y * r2 * r2 * r2;
      jacobian[stride + 5] = T(3.0) * y2 + x2;
      jacobian[stride + 6] = T(2.0) * x * y;
    }

    Forward(point, k, distorted);
  }