static Eigen::Matrix unproject()

in IsometricPatternMatcher/CameraModels.h [179:200]

• 18 lines of code
• 4 McCabe index (conditional complexity)

  static Eigen::Matrix<typename D::Scalar, 3, 1> unproject(
      const Eigen::MatrixBase<D>& uvPixel,
      const Eigen::MatrixBase<DP>& params) {
    EIGEN_STATIC_ASSERT(D::RowsAtCompileTime == 2 && D::ColsAtCompileTime == 1,
                        THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
    EIGEN_STATIC_ASSERT(
        DP::ColsAtCompileTime == 1 && (DP::RowsAtCompileTime == kNumParams ||
                                       DP::RowsAtCompileTime == Eigen::Dynamic),
        THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
    using T = typename D::Scalar;

    // Unprojection
    const T fu = params[0];
    const T fv = params[1];
    const T u0 = params[2];
    const T v0 = params[3];

    const T un = (uvPixel(0) - u0) / fu;
    const T vn = (uvPixel(1) - v0) / fv;

    return Eigen::Matrix<T, 3, 1>(un, vn, T(1.0));
  }