bool Evaluate()

in opensfm/src/bundle/error/projection_errors.h [62:146]
71 lines of code
12 McCabe index (conditional complexity)

  bool Evaluate(double const* const* parameters, double* residuals,
                double** jacobians) const {
    const double* camera = parameters[0];
    const double* rig_instance = parameters[1];
    const double* rig_camera = parameters[2];
    const double* point = parameters[3];

    constexpr int PointSize = 3;
    double transformed[PointSize];
    double predicted[Size];

    /* Error only */
    if (!jacobians) {
      geometry::PoseFunctor::Forward(point, rig_instance, &transformed[0]);
      geometry::PoseFunctor::Forward(&transformed[0], rig_camera,
                                     &transformed[0]);
      geometry::Dispatch<geometry::ProjectFunction>(type_, transformed, camera,
                                                    predicted);
    } /* Jacobian + Error */
    else {
      constexpr int CameraSize = C;
      constexpr int PoseSize = 6;

      double all_params[PoseSize + PoseSize + CameraSize];
      for (int i = 0; i < PoseSize; ++i) {
        all_params[i] = rig_instance[i];
      }
      for (int i = 0; i < PoseSize; ++i) {
        all_params[PoseSize + i] = rig_camera[i];
      }
      for (int i = 0; i < CameraSize; ++i) {
        all_params[2 * PoseSize + i] = camera[i];
      }

      constexpr int StrideFull = PointSize + CameraSize + 2 * PoseSize;
      double jacobian[Size * StrideFull];
      geometry::Dispatch<geometry::ProjectRigPoseDerivatives>(
          type_, point, &all_params[0], &predicted[0], &jacobian[0]);

      // Unfold big jacobian stored as | point | jac_rig_instance |
      // jac_rig_camera | camera | per block We also take the opportunity to
      // apply the scale
      double *jac_camera = jacobians[0], *jac_rig_instance = jacobians[1],
             *jac_rig_camera = jacobians[2], *jac_point = jacobians[3];
      Eigen::Map<Eigen::Matrix<double, Size, StrideFull, Eigen::RowMajor>>
          map_jac_big(jacobian);

      if (jac_point) {
        Eigen::Map<Eigen::Matrix<double, Size, PointSize, Eigen::RowMajor>>
            map_jac_point(jac_point);
        map_jac_point =
            scale_ * map_jac_big.template block<Size, PointSize>(0, 0);
      }
      if (jac_rig_instance) {
        Eigen::Map<Eigen::Matrix<double, Size, PoseSize, Eigen::RowMajor>>
            map_jac_rig_instance(jac_rig_instance);
        map_jac_rig_instance =
            scale_ * map_jac_big.template block<Size, PoseSize>(0, PointSize);
      }
      if (jac_rig_camera) {
        Eigen::Map<Eigen::Matrix<double, Size, PoseSize, Eigen::RowMajor>>
            map_jac_rig_camera(jac_rig_camera);
        map_jac_rig_camera =
            scale_ *
            map_jac_big.template block<Size, PoseSize>(0, PointSize + PoseSize);
      }
      if (jac_camera) {
        // Eigen doesn't like vectors as 1-dim matrices (CameraSize == 1), using
        // simple assignments instead of Eigen's Map
        for (int i = 0; i < Size; ++i) {
          for (int j = 0; j < CameraSize; ++j) {
            jac_camera[i * CameraSize + j] =
                scale_ *
                jacobian[i * StrideFull + PointSize + 2 * PoseSize + j];
          }
        }
      }
    }

    // The error is the difference between the predicted and observed position
    for (int i = 0; i < Size; ++i) {
      residuals[i] = scale_ * (predicted[i] - observed_[i]);
    }
    return true;
  }