'2D to 3D backprojection on MEI(CataCamera) with given Z in world by using Camodocal or VINS-Fusion

I use VINS-Fusion codes( or camodocal).
How can I do back-projection (2D image pixel point to world point(3D))?
I probably know how to do it on pinhole camera model after read this. But I don't know how to do it on MEI camera model.
I calibrated the camera and got the intrinsic( xi, k1, k2, p1, p2, gamma1, gamma2, u0, v0).
And used a plan(ground) in world with z = 0 to calibrate the extrinsic param. I got the extrinsic(rvec,tvec) by the function below

//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);
}

I try the below code to do backprojection but it seems not work. If i know z in world and how to get x ,y in world?

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);
}

I have referenced the following information
VINS-Fusion github
Single View Point Omnidirectional Camera Calibration from Planar Grids
Design and calibration of an omni-RGB+D camera



Sources

This article follows the attribution requirements of Stack Overflow and is licensed under CC BY-SA 3.0.

Source: Stack Overflow

Solution Source