void BundleAdjuster::ComputeCovariances()

in opensfm/src/bundle/src/bundle_adjuster.cc [1079:1147]
56 lines of code
14 McCabe index (conditional complexity)

void BundleAdjuster::ComputeCovariances(ceres::Problem *problem) {
  bool computed = false;

  if (last_run_summary_.termination_type != ceres::FAILURE) {
    ceres::Covariance::Options options;
    if (!ceres::StringToCovarianceAlgorithmType(covariance_algorithm_type_,
                                                &options.algorithm_type)) {
      throw std::runtime_error("Covariance algorithm type " +
                               covariance_algorithm_type_ + " doesn't exist.");
    }
    ceres::Covariance covariance(options);

    std::vector<std::pair<const double *, const double *>> covariance_blocks;
    for (auto &i : shots_) {
      covariance_blocks.push_back(
          std::make_pair(i.second.GetRigInstance()->GetValueData().data(),
                         i.second.GetRigInstance()->GetValueData().data()));
    }

    bool worked = covariance.Compute(covariance_blocks, problem);

    if (worked) {
      for (auto &i : shots_) {
        covariance_estimation_valid_ = true;

        MatXd covariance_matrix(6, 6);
        if (covariance.GetCovarianceBlock(
                i.second.GetRigInstance()->GetValueData().data(),
                i.second.GetRigInstance()->GetValueData().data(),
                covariance_matrix.data())) {
          i.second.GetRigInstance()->SetCovariance(covariance_matrix);
        }
      }
      computed = true;
    }
  }

  // TODO: It might be slow to check everything for NaNs
  //       So maybe we can find a better solution
  if (computed) {
    // Check for NaNs
    for (auto &i : shots_) {
      if (!i.second.GetRigInstance()->HasCovariance() ||
          !i.second.GetRigInstance()->GetCovariance().allFinite()) {
        covariance_estimation_valid_ = false;
        computed = false;
        break;
      }
      // stop after first Nan value
      if (!computed) break;
    }
  }

  // If covariance estimation failed, use a default value
  if (!computed) {
    covariance_estimation_valid_ = false;

    MatXd default_covariance_matrix = MatXd::Zero(6, 6);
    double default_rotation_variance = 1e-5;
    double default_translation_variance = 1e-2;
    default_covariance_matrix.diagonal().segment<3>(0).setConstant(
        default_rotation_variance);
    default_covariance_matrix.diagonal().segment<3>(3).setConstant(
        default_translation_variance);
    for (auto &i : shots_) {
      i.second.GetRigInstance()->SetCovariance(default_covariance_matrix);
    }
  }
}