bool operator()

in opensfm/src/bundle/reconstruction_alignment.h [315:368]
43 lines of code
3 McCabe index (conditional complexity)

  bool operator()(const T *const reconstruction_a,
                  const T *const reconstruction_b, T *residuals) const {
    const double *const rotation_ai = shot_ai_ + RA_SHOT_RX;
    const double *const rotation_bi = shot_bi_ + RA_SHOT_RX;
    const double *const translation_ai = shot_ai_ + RA_SHOT_TX;
    const double *const translation_bi = shot_bi_ + RA_SHOT_TX;

    double pose_ai[3], pose_bi[3];
    const double rotation_ait[3] = {-rotation_ai[0], -rotation_ai[1],
                                    -rotation_ai[2]};
    ceres::AngleAxisRotatePoint(rotation_ait, translation_ai, pose_ai);
    const double rotation_bit[3] = {-rotation_bi[0], -rotation_bi[1],
                                    -rotation_bi[2]};
    ceres::AngleAxisRotatePoint(rotation_bit, translation_bi, pose_bi);
    for (int i = 0; i < 3; ++i) {
      pose_ai[i] = -pose_ai[i];
      pose_bi[i] = -pose_bi[i];
    }

    // optical center error in world coordinates :
    // Ta(-1)(shot_a center) - Tb(-1)(shot_b center)
    T world_pose_ai[3], world_pose_bi[3];
    transform_point(reconstruction_a, pose_ai, world_pose_ai);
    transform_point(reconstruction_b, pose_bi, world_pose_bi);

    // rotation error in world coordinates :
    // (Ra^t Rai^t)(^T)(Rb^t Rbi^t) = (Rai Ra)(Rb^t Rbi^t)
    const T *const Ra = reconstruction_a + RA_RECONSTRUCTION_RX;
    const T *const Rb = reconstruction_b + RA_RECONSTRUCTION_RX;
    const T Rbt[3] = {-Rb[0], -Rb[1], -Rb[2]};
    const T Rbit[3] = {T(-rotation_bi[0]), T(-rotation_bi[1]),
                       T(-rotation_bi[2])};
    const T Rai[3] = {T(rotation_ai[0]), T(rotation_ai[1]), T(rotation_ai[2])};

    T qRai[4], qRa[4], qRbt[4], qRbit[4], qRai_qRa[4], qRai_qRa_qRbt[4],
        qRai_qRa_qRbt_qRbit[4];
    ceres::AngleAxisToQuaternion(Rai, qRai);
    ceres::AngleAxisToQuaternion(Ra, qRa);
    ceres::AngleAxisToQuaternion(Rbt, qRbt);
    ceres::AngleAxisToQuaternion(Rbit, qRbit);
    ceres::QuaternionProduct(qRai, qRa, qRai_qRa);
    ceres::QuaternionProduct(qRai_qRa, qRbt, qRai_qRa_qRbt);
    ceres::QuaternionProduct(qRai_qRa_qRbt, qRbit, qRai_qRa_qRbt_qRbit);

    // final error
    ceres::QuaternionToAngleAxis(qRai_qRa_qRbt_qRbit, residuals);
    for (int i = 0; i < 3; ++i) {
      residuals[i] *= T(inv_std_rotation_);
      residuals[3 + i] =
          T(inv_std_center_) * (world_pose_ai[i] - world_pose_bi[i]);
    }

    return true;
  }