Обратное проецирование 2D в 3D на MEI(CataCamera) с заданным Z в мире с использованием Camodocal или VINS-Fusion

Question

Я использую коды ВИНС-Фьюжн (или камодокал).
Как я могу сделать обратную проекцию (пиксель 2D-изображения указывает на мировую точку (3D))?
Я, наверное, знаю, как это сделать на модели камеры-обскуры после прочтения этого . Но я не знаю, как это сделать на модели камеры MEI.
Я откалибровал камеру и получил внутренние (xi, k1, k2, p1, p2, gamma1, gamma2, u0, v0).
И использовал план (землю) в мире с z = 0 для калибровки внешнего параметра. Я получил внешний (rvec, tvec) с помощью функции ниже

      //Camera.cc line 122
void Camera::estimateExtrinsics(const std::vector<cv::Point3f>& objectPoints,
                       const std::vector<cv::Point2f>& imagePoints,
                       cv::Mat& rvec, cv::Mat& tvec) const
{
    std::vector<cv::Point2f> Ms(imagePoints.size());
    for (size_t i = 0; i < Ms.size(); ++i)
    {
        Eigen::Vector3d P;
        liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P);

        P /= P(2);

        Ms.at(i).x = P(0);
        Ms.at(i).y = P(1);
    }

    // assume unit focal length, zero principal point, and zero distortion
    cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec);
}

Я пытаюсь использовать приведенный ниже код для обратной проекции, но, похоже, он не работает. Если я знаю z в ​​мире и как получить x, y в мире?

      void
Camera::backProjectPoint(const cv::Point2f& imagePoint,
                     const Eigen::Matrix3d& R,// rotation matrix from rvec by cv::Rodrigues
                     const Eigen::Vector3d& T,// from tvec
                     cv::Point3f& objectPoint) const
{
    Eigen::Vector3d P;
    liftProjective(Eigen::Vector2d(imagePoint.x, imagePoint.y),P);
    P /= P(2);
    P = R.inverse() * (P - T);
    P /= P(2);
    objectPoint.x = P(0);
    objectPoint.y = P(1);
    objectPoint.z = P(2);
}

Я сослался на следующую информацию
VINS-Fusion github
Калибровка всенаправленной камеры с одной точкой обзора из планарных сеток
Проектирование и калибровка камеры всенаправленного RGB+D