static Eigen::Matrix project()

in IsometricPatternMatcher/CameraModels.h [242:380]
118 lines of code
15 McCabe index (conditional complexity)

  static Eigen::Matrix<typename D::Scalar, 2, 1> project(
      const Eigen::MatrixBase<D>& pointOptical,
      const Eigen::MatrixBase<DP>& params,
      Eigen::MatrixBase<DJ1>* d_point = nullptr,
      Eigen::MatrixBase<DJ2>* d_params = nullptr) {
    using T = typename D::Scalar;

    static_assert(D::RowsAtCompileTime == 3 && D::ColsAtCompileTime == 1,
                  "THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE");
    static_assert(
        DP::ColsAtCompileTime == 1 && (DP::RowsAtCompileTime == kNumParams ||
                                       DP::RowsAtCompileTime == Eigen::Dynamic),
        "THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE");
    static_assert((DJ1::ColsAtCompileTime == 3 ||
                   DJ1::ColsAtCompileTime == Eigen::Dynamic) &&
                      (DJ1::RowsAtCompileTime == 2 ||
                       DJ1::RowsAtCompileTime == Eigen::Dynamic),
                  "THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE");
    static_assert((DJ2::ColsAtCompileTime == kNumParams ||
                   DJ2::ColsAtCompileTime == Eigen::Dynamic) &&
                      (DJ2::RowsAtCompileTime == 2 ||
                       DJ2::RowsAtCompileTime == Eigen::Dynamic),
                  "THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE");

    SOPHUS_ENSURE(pointOptical.z() != T(0), "z(%) must not be zero.",
                  pointOptical.z());

    // Focal length and principal point
    const Eigen::Matrix<T, 2, 1> ff = params.template head<2>();
    const Eigen::Matrix<T, 2, 1> pp = params.template segment<2>(2);

    const T k0 = params[4];
    const T k1 = params[5];
    const T k2 = params[6];
    const T k3 = params[7];

    const T radiusSquared =
        pointOptical(0) * pointOptical(0) + pointOptical(1) * pointOptical(1);
    using std::atan2;
    using std::sqrt;

    if (radiusSquared > Sophus::Constants<T>::epsilon()) {
      const T radius = sqrt(radiusSquared);
      const T radiusInverse = T(1.0) / radius;
      const T theta = atan2(radius, pointOptical(2));
      const T theta2 = theta * theta;
      const T theta4 = theta2 * theta2;
      const T theta6 = theta4 * theta2;
      const T theta8 = theta4 * theta4;
      const T rDistorted = theta * (T(1.0) + k0 * theta2 + k1 * theta4 +
                                    k2 * theta6 + k3 * theta8);
      const T scaling = rDistorted * radiusInverse;

      if (d_point) {
        const T xSquared = pointOptical(0) * pointOptical(0);
        const T ySquared = pointOptical(1) * pointOptical(1);
        const T normSquared = pointOptical(2) * pointOptical(2) + radiusSquared;
        const T rDistortedDerivative =
            T(1.0) + T(3.0) * k0 * theta2 + T(5.0) * k1 * theta4 +
            T(7.0) * k2 * theta6 + T(9.0) * k3 * theta8;
        const T x13 =
            pointOptical(2) * rDistortedDerivative / normSquared - scaling;
        const T rDistortedDerivativeNormalized =
            rDistortedDerivative / normSquared;
        const T x20 = pointOptical(2) * rDistortedDerivative /
                      (normSquared)-radiusInverse * rDistorted;

        (*d_point)(0, 0) = xSquared / radiusSquared * x20 + scaling;
        (*d_point)(0, 1) =
            pointOptical(1) * x13 * pointOptical(0) / radiusSquared;
        (*d_point)(0, 2) = -pointOptical(0) * rDistortedDerivativeNormalized;
        (*d_point)(1, 0) = (*d_point)(0, 1);
        (*d_point)(1, 1) = ySquared / radiusSquared * x20 + scaling;
        (*d_point)(1, 2) = -pointOptical(1) * rDistortedDerivativeNormalized;

        // toDenseMatrix() is needed for CUDA to explicitly know the matrix
        // dimensions
        (*d_point) = ff.asDiagonal().toDenseMatrix() * (*d_point);
      }
      using std::pow;
      if (d_params) {
        const T xScaled = pointOptical[0] * params[0] * radiusInverse;
        const T yScaled = pointOptical[1] * params[1] * radiusInverse;

        const T theta3 = theta * theta2;
        const T theta5 = theta3 * theta2;
        const T theta7 = theta5 * theta2;
        const T theta9 = theta7 * theta2;

        (*d_params)(0, 0) = pointOptical[0] * scaling;
        (*d_params)(0, 1) = T(0.0);
        (*d_params)(0, 2) = T(1.0);
        (*d_params)(0, 3) = T(0.0);
        (*d_params)(0, 4) = xScaled * theta3;
        (*d_params)(0, 5) = xScaled * theta5;
        (*d_params)(0, 6) = xScaled * theta7;
        (*d_params)(0, 7) = xScaled * theta9;
        (*d_params)(1, 0) = T(0.0);
        (*d_params)(1, 1) = pointOptical[1] * scaling;
        (*d_params)(1, 2) = T(0.0);
        (*d_params)(1, 3) = T(1.0);
        (*d_params)(1, 4) = yScaled * theta3;
        (*d_params)(1, 5) = yScaled * theta5;
        (*d_params)(1, 6) = yScaled * theta7;
        (*d_params)(1, 7) = yScaled * theta9;
      }

      const Eigen::Matrix<T, 2, 1> px =
          scaling * ff.cwiseProduct(pointOptical.template head<2>()) + pp;

      return px;
    } else {
      // linearize r around radius=0
      if (d_point) {
        const T z2 = pointOptical(2) * pointOptical(2);
        // clang-format off
        (*d_point) << ff.x() / pointOptical(2), T(0.0), -ff.x() * pointOptical(0) / z2,
                      T(0.0), ff.y() / pointOptical(2), -ff.y() * pointOptical(1) / z2;
        // clang-format on
      }
      if (d_params) {
        (*d_params)(0, 0) = pointOptical(0) / pointOptical(2);
        (*d_params)(0, 1) = T(0.0);
        (*d_params)(0, 2) = T(1.0);
        (*d_params)(0, 3) = T(0.0);

        (*d_params)(1, 0) = T(0.0);
        (*d_params)(1, 1) = pointOptical(1) / pointOptical(2);
        (*d_params)(1, 2) = T(0.0);
        (*d_params)(1, 3) = T(1.0);
        (*d_params).template rightCols<4>().setZero();
      }
      const Eigen::Matrix<T, 2, 1> px =
          ff.cwiseProduct(pointOptical.template head<2>()) / pointOptical(2) +
          pp;

      return px;
    }
  }