bool Evaluate()

in opensfm/src/bundle/error/projection_errors.h [192:262]
61 lines of code
10 McCabe index (conditional complexity)

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

    Vec3d transformed;
    /* Error only */
    if (!jacobians) {
      geometry::PoseFunctor::Forward(point, rig_instance, &transformed[0]);
      geometry::PoseFunctor::Forward(&transformed[0], rig_camera,
                                     &transformed[0]);
      transformed.normalize();
    } /* Jacobian + Error */
    else {
      constexpr int PointSize = 3;
      constexpr int PoseSize = 6;

      double all_params[PoseSize + PoseSize];
      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];
      }

      constexpr int StrideFull = PoseSize + PoseSize + PointSize;
      double jacobian[Size * StrideFull];
      geometry::Dispatch<geometry::RigPoseNormalizedDerivatives>(
          geometry::ProjectionType::SPHERICAL, point, &all_params[0],
          transformed.data(), &jacobian[0]);

      // Unfold big jacobian stored as | point | pose | 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_camera) {
        for (int i = 0; i < Size; ++i) {
          jac_camera[i] = 0.;
        }
      }
      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);
      }
    }

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