bool operator()

in IsometricPatternMatcher/HexGridCostFunction.h [74:106]
29 lines of code
1 McCabe index (conditional complexity)

  bool operator()(const Scalar* const paramTargetPose,
                  const Scalar* const paramDistortion,
                  Scalar* residuals) const {
    Sophus::Vector<Scalar, KannalaBrandtK3Projection::kNumParams> intrinsics;
    intrinsics << Scalar(focalLength_), Scalar(focalLength_),
        Scalar(centerXY_(0)), Scalar(centerXY_(1)), paramDistortion[0],
        paramDistortion[1], paramDistortion[2], paramDistortion[3];

    Ray3<Scalar> rayInCamera = KannalaBrandtK3Projection::unproject(
        imageDots_.template cast<Scalar>(),
        intrinsics);  //.template cast<Scalar>();
    Ray3<Scalar> rayNeighbourInCamera = KannalaBrandtK3Projection::unproject(
        neighbourDots_.template cast<Scalar>(),
        intrinsics);  //.template cast<Scalar>();

    const Eigen::Map<const Sophus::SE3<Scalar>> T_camera_target(
        paramTargetPose);
    Ray3<Scalar> rayInTarget =
        T_camera_target.inverse() * rayInCamera.template cast<Scalar>();
    Ray3<Scalar> rayNeighbourInTarget =
        T_camera_target.inverse() *
        rayNeighbourInCamera.template cast<Scalar>();

    Sophus::Vector<Scalar, 3> ptTarget3d =
        rayInTarget.line().intersectionPoint(plane_.template cast<Scalar>());
    Sophus::Vector<Scalar, 3> ptNeighbourTarget3d =
        rayNeighbourInTarget.line().intersectionPoint(
            plane_.template cast<Scalar>());
    Sophus::Vector<Scalar, 3> delta = ptTarget3d - ptNeighbourTarget3d;

    residuals[0] = delta.squaredNorm() - Scalar(1.0);
    return true;
  }