bool operator()

in IsometricPatternMatcher/HexGridCostFunction.h [32:51]

18 lines of code
1 McCabe index (conditional complexity)


  bool operator()(const Scalar* const paramTargetPose,
                  Scalar* residuals) const {
    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(transferSpacing_);
    return true;
  }