三.3D坐标系中XYZ转换为2D像素坐标UV

s-表示投影变换的任意比例，不是相机模型的一部分
cx, cy-表示图像的中心，单位像素
fx, fy-表示焦距，单位像素

// Intrisic camera parameters:UVC Webcam
    double f = 55;          // focal length in mm
    double sx = 22.3;       // 宽, sensor size
    double sy = 14.9;       // 高, sensor size
    double width = 640;     // u-y, image size
    double height = 480;    // v-x, image size

    double params_WEBCAM[] = {  width*f / sx,       // fx
                                height*f / sy,      // fy
                                width / 2,          // cx
                                height / 2 };       // cy

    PnPProblem pnp_detection(params_WEBCAM);        // instantiate PnPProblem class

    // Custom constructor given the intrisic camera parameters
    PnPProblem::PnPProblem(const double params[])
    {
        _A_matrix = cv::Mat::zeros(3, 3, CV_64FC1);   // intrinsic camera parameters
        _A_matrix.at<double>(0, 0) = params[0];       // [ fx 0 cx ]
        _A_matrix.at<double>(1, 1) = params[1];       // [ 0 fy cy ]
        _A_matrix.at<double>(0, 2) = params[2];       // [ 0  0  1 ]
        _A_matrix.at<double>(1, 2) = params[3];
        _A_matrix.at<double>(2, 2) = 1;
        _R_matrix = cv::Mat::zeros(3, 3, CV_64FC1);     // rotation matrix
        _t_matrix = cv::Mat::zeros(3, 1, CV_64FC1);     // translation matrix
        _P_matrix = cv::Mat::zeros(3, 4, CV_64FC1);     // rotation-translation matrix
    }

    // Backproject a 3D point to 2D using the estimated pose parameters
    cv::Point2f PnPProblem::backproject3DPoint(const cv::Point3f &point3d)
    {
        // 3D point vector [x y z]'

        cv::Mat point3d_vec = cv::Mat(4, 1, CV_64FC1);
        point3d_vec.at<double>(0) = point3d.x;         // 世界坐标x
        point3d_vec.at<double>(1) = point3d.y;         // 世界坐标y
        point3d_vec.at<double>(2) = point3d.z;         // 世界坐标z
        point3d_vec.at<double>(3) = 1;

        // 2D point vector [u v 1]'
        cv::Mat point2d_vec = cv::Mat(4, 1, CV_64FC1);
        point2d_vec = _A_matrix * _P_matrix * point3d_vec;  // 世界坐标到像素坐标矩阵运算

        // Normalization of [u v]'
        cv::Point2f point2d;
        point2d.x = point2d_vec.at<double>(0) / point2d_vec.at<double>(2);
        point2d.y = point2d_vec.at<double>(1) / point2d_vec.at<double>(2);

        return point2d;
    }