static void ForwardDerivatives()

in opensfm/src/geometry/transformations_functions.h [147:213]
48 lines of code
11 McCabe index (conditional complexity)

  static void ForwardDerivatives(const T* point, const T* rt, T* transformed,
                                 T* jacobian) {
    // dx, dy, dz, drx, dry, drz, dtz, dty, dtz
    if (DERIV_PARAMS) {
      using Dual = Eigen::AutoDiffScalar<Vec3d>;

      /* Get jacobian or R wrt. angle-axis using Dual */
      Dual r_diff[InSize];
      r_diff[0].value() = -rt[Rx];
      r_diff[0].derivatives() = -Vec3d::Unit(Rx);
      r_diff[1].value() = -rt[Ry];
      r_diff[1].derivatives() = -Vec3d::Unit(Ry);
      r_diff[2].value() = -rt[Rz];
      r_diff[2].derivatives() = -Vec3d::Unit(Rz);

      Eigen::Matrix<Dual, 3, 3, Eigen::RowMajor> rotation;
      AngleAxisToRotation(&r_diff[0], rotation.data());

      /* Storage is row-ordered : R00, R01, R02, R10, ... R22 */
      Eigen::Matrix<T, 9, 3, Eigen::RowMajor> rotation_angleaxis;
      for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
          for (int k = 0; k < 3; ++k) {
            rotation_angleaxis(i * 3 + j, k) = rotation(i, j).derivatives()(k);
          }
        }
      }

      /* R.(x-t) derivatives are pretty straightfoward : dR00, dR01, ... dR22,
       * dx, dy, dz, dtx, dty, dtz */
      const T xyz[] = {point[0] - rt[Tx], point[1] - rt[Ty], point[2] - rt[Tz]};
      Eigen::Matrix<T, 3, 15, Eigen::RowMajor> point_rotation =
          Eigen::Matrix<T, 3, 15, Eigen::RowMajor>::Zero();
      for (int i = 0; i < 3; ++i) {
        // dRij
        for (int j = 0; j < 3; ++j) {
          point_rotation(i, i * 3 + j) = xyz[j];
        }
        // dx, dy, dz
        for (int j = 0; j < 3; ++j) {
          point_rotation(i, 9 + j) = rotation(i, j).value();
        }
        // dtx, dty, dtz
        for (int j = 0; j < 3; ++j) {
          point_rotation(i, 12 + j) = -point_rotation(i, 9 + j);
        }
      }

      /* Compose d(R) / d(angle axis) with d(pose) / d(R | t | x) in order to
       * get d(pose) / d(angle axis | t | x) */

      ComposeDerivatives<T, true, 9, 3, 0, 3, 15, 6>(rotation_angleaxis,
                                                     point_rotation, jacobian);

      /* Re-orde from angle-axis | x | t to x | angle-axis | t */
      for (int i = 0; i < 3; ++i) {
        // Swap dai and dxi
        for (int j = 0; j < 3; ++j) {
          std::swap(jacobian[i * 9 + j], jacobian[i * 9 + 3 + j]);
        }
      }
    } else {
      double minus_r[] = {-rt[Rx], -rt[Ry], -rt[Rz]};
      AngleAxisToRotation(&minus_r[0], jacobian);
    }
    Forward(point, rt, transformed);
  }