static void ForwardDerivatives()

in opensfm/src/geometry/camera_distortions_functions.h [348:414]
55 lines of code
2 McCabe index (conditional complexity)

  static void ForwardDerivatives(const T* point, const T* k, T* distorted,
                                 T* jacobian) {
    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& k4 = k[static_cast<int>(K4)];
    const auto& k5 = k[static_cast<int>(K5)];
    const auto& k6 = k[static_cast<int>(K6)];
    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 auto x2 = x * x;
    const auto y2 = y * y;
    const auto r2 = x2 + y2;
    const auto r2_2 = r2 * r2;
    const auto r2_3 = r2_2 * r2;
    const auto r2_4 = r2_3 * r2;
    const auto r2_5 = r2_4 * r2;

    // Compute tangential distortion separatedly
    const auto dx_dxt = T(2.0) * y * p1 + T(6.0) * p2 * x;
    const auto dx_dyt = T(2.0) * x * p1 + T(2.0) * p2 * y;
    const auto dy_dxt = dx_dyt;  // == dx_dyt
    const auto dy_dyt = T(2.0) * x * p2 + T(6.0) * p1 * y;
    // Computing the deriv. of the rad dist. p(x,y):
    // For x: d x * p(x,y)/ dx = p(x,y) + x * dp(x,y)/dy
    // and simplify dp(x,y)/dx = dp/dr * dr/dx
    const auto p = RadialDistortion(r2, k1, k2, k3, k4, k5, k6);
    const auto dr_dx = T(2.0) * x;  // dr/dx = (x^2 + y^2)' = 2x
    const auto dr_dy = T(2.0) * y;  // dr/dy = (x^2 + y^2)' = 2y
    const auto dp_dr = k1 + T(2.0) * k2 * r2 + T(3.0) * k3 * r2_2 +
                       T(4.0) * k4 * r2_3 + T(5.0) * k5 * r2_4 +
                       T(6.0) * k6 * r2_5;
    jacobian[0] = p + x * dp_dr * dr_dx + dx_dxt;
    jacobian[1] = x * dp_dr * dr_dy + dx_dyt;
    jacobian[stride + 0] = y * dp_dr * dr_dx + dy_dxt;
    jacobian[stride + 1] = p + y * dp_dr * dr_dy + dy_dyt;

    if (DERIV_PARAMS) {
      const auto r2_6 = r2_5 * r2;
      // K1 - K6
      jacobian[2] = x * r2;
      jacobian[3] = x * r2_2;
      jacobian[4] = x * r2_3;
      jacobian[5] = x * r2_4;
      jacobian[6] = x * r2_5;
      jacobian[7] = x * r2_6;
      // P1 - P2
      jacobian[8] = T(2.0) * x * y;
      jacobian[9] = T(3.0) * x2 + y2;
      // K1 - K6
      jacobian[stride + 2] = y * r2;
      jacobian[stride + 3] = y * r2_2;
      jacobian[stride + 4] = y * r2_3;
      jacobian[stride + 5] = y * r2_4;
      jacobian[stride + 6] = y * r2_5;
      jacobian[stride + 7] = y * r2_6;
      // P1 - P2
      jacobian[stride + 8] = T(3.0) * y2 + x2;
      jacobian[stride + 9] = jacobian[8];
    }

    Forward(point, k, distorted);
  }