根据单应矩阵H，实现2d点--＞3d点的映射（C++ OpenCV）

Eigen::Vector3d homographyMap(const std::vector<cv::Point3f>& vpts_3f, const std::vector<cv::Point2f>& vpts_2f, const cv::Point2f& Puv)
{
    cv::Mat mask;
    cv::Mat H_64 = cv::findHomography(vpts_3f, vpts_2f, mask);

    cv::Mat H_32 = cv::Mat(3, 3, CV_32F);
    H_64.convertTo(H_32, CV_32F);

    // 2d-ferture
    cv::Mat dst(3, 1, CV_32F);
    dst.at<float>(0, 0) = Puv.x;
    dst.at<float>(1, 0) = Puv.y;
    dst.at<float>(2, 0) = 1.;

    // 3d-feature
    cv::Mat src = H_32.inv() * dst;
    src /= src.at<float>(2, 0);
    src.at<float>(2, 0) = 0.;

    return Eigen::Vector3d(src.at<float>(0, 0), src.at<float>(1, 0), src.at<float>(2, 0));

}