static Eigen::Matrix project()

in IsometricPatternMatcher/CameraModels.h [107:163]
49 lines of code
12 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 Eigen::Matrix<T, 2, 1> px =
        (Project(pointOptical).array() * ff.array()).matrix() + pp;

    if (d_point) {
      const T oneOverZ = T(1) / pointOptical(2);

      (*d_point)(0, 0) = ff(0) * oneOverZ;
      (*d_point)(0, 1) = 0.0;
      (*d_point)(0, 2) = -(*d_point)(0, 0) * pointOptical(0) * oneOverZ;
      (*d_point)(1, 0) = 0.0;
      (*d_point)(1, 1) = ff(1) * oneOverZ;
      (*d_point)(1, 2) = -(*d_point)(1, 1) * pointOptical(1) * oneOverZ;
    }
    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);
    }

    return px;
  }