Projecting from one $2D$ plane onto another $2D$ plane

Question

I would like to project from one $2D$ plane onto another. Imagine that I have a picture taken with a camera that was looking onto a plane. Given camera's extrinsic and intrinsic parameters I want to know how the points in the picture map to the points on the pictured plane.

What I know so far is that this is normally achieved using a homography matrix. However, I want to confirm the particular formula for the described projection.

Let's assume our intrinsic camera matrix is the following: $$ I = \begin{pmatrix}f & 0 & 0 & 0\\ 0 & f & 0 & 0\\ 0 & 0 & 1 & 0\end{pmatrix} $$

The entrinsic matrix describes the position $(x_t, y_t, z_t)$ and rotation of the camera w.r.t. the world coordinates, for demonstration purposes let's assume it's only rotated around $x$ axis: $$ E = \begin{pmatrix}1 & 0 & 0 & x_t\\ 0 & cos\theta & -sin\theta & y_t \\ 0 & sin\theta & cos\theta & z_t \\ 0 & 0 & 0 & 1\end{pmatrix} $$

So if we want to find the projection of a $3D$ point in the world $(x_w, y_w, z_w)$ onto our camera plane, we can now use the final camera matrix to perform the projection transformation: $$ \begin{pmatrix}x_c \\ y_c \\ w \end{pmatrix} = I E \begin{pmatrix}x_w \\ y_w \\ z_w \\ 1\end{pmatrix} $$

And the position on the camera plane (image), currently assuming that image center is at $(0, 0)$, is given by: $x = x_c/w$ and $y = y_c/w$.

Now to get to the original problem and my question ($2D$ to $2D$ plane projection, rather than $3D$ to $2D$ projection) I would do something like the following. First, I only have the location on the image $(x_c, y_c)$ and I want to derive coordinates $(x_w, y_w)$ on a plane in the world. I can rewrite my equation like this: $$ H = IE $$ $$ H = \begin{pmatrix}h_{11} & h_{12} & h_{13} & h_{14}\\h_{21} & h_{22} & h_{23} & h_{24}\\h_{31} & h_{32} & h_{33} & h_{34}\end{pmatrix} $$ $$ \begin{pmatrix}x_c \\ y_c \\ w \end{pmatrix} = \begin{pmatrix}h_{11} & h_{12} & h_{13} & h_{14}\\h_{21} & h_{22} & h_{23} & h_{24}\\h_{31} & h_{32} & h_{33} & h_{34}\end{pmatrix} \begin{pmatrix}x_w \\ y_w \\ 0 \\ 1\end{pmatrix} $$ $$ \begin{pmatrix}x_c \\ y_c \\ w \end{pmatrix} = \begin{pmatrix}h_{11} & h_{12} & h_{14}\\h_{21} & h_{22} & h_{24}\\h_{31} & h_{32} & h_{34}\end{pmatrix} \begin{pmatrix}x_w \\ y_w \\ 1\end{pmatrix} $$ $$ H' = \begin{pmatrix}h_{11} & h_{12} & h_{14}\\h_{21} & h_{22} & h_{24}\\h_{31} & h_{32} & h_{34}\end{pmatrix} $$ $$ H'^{-1} = H'^T $$ $$ \begin{pmatrix}x_w \\ y_w \\ w\end{pmatrix} = H'^T \begin{pmatrix}x_c \\ y_c \\ 1 \end{pmatrix} $$

I would then use $x=x_w/w$ and $y=y_w/w$ as coordinates relative to the $2D$ plane in the world. Is that correct or at least going in the right direction?

Side note: this has been briefly touched upon, but without any good mathematical grounding in someone else's practical question https://stackoverflow.com/questions/20445147/transform-image-using-roll-pitch-yaw-angles-image-rectification and I'm interested in the mathematical foundation of a similar problem.

Answer 1

Here is the code in OpenCV C++. From 6ft away, I'm getting errors of less that 0.5 mm and 0.2 pixel.

typedef struct CameraCalibration {
    bool isCalibrated = false;
    Mat camMat;
    Mat distCoeffs;
    Mat rvec;
    Mat tvec;
    Mat invMat;
    double reprojectionError;
} CameraCalibration;

std::vector<cv::Point3f> Unproject(const std::vector<cv::Point2f>& points, const CameraCalibration &cameraCalibration)
{
    std::vector<cv::Point2f> points_undistorted;
    if (!points.empty()) {
        cv::undistortPoints(points, points_undistorted, cameraCalibration.camMat,
            cameraCalibration.distCoeffs, cv::noArray(), cameraCalibration.camMat);
    }

    std::vector<cv::Point3f> objectPoints;

    Mat m1 = Mat(3, 1, CV_64F);
    double c_x = cameraCalibration.camMat.at<double>(0, 2);
    double c_y = cameraCalibration.camMat.at<double>(1, 2);

    Mat m2;
    for (int count = 0; count < points_undistorted.size(); count++) {
        m1.at<double>(0, 0) = points_undistorted.at(count).x - c_x;
        m1.at<double>(1, 0) = points_undistorted.at(count).y - c_y;
        m1.at<double>(2, 0) = 1;
        m2 = cameraCalibration.invMat * m1;
        objectPoints.push_back(Point3f(m2.at<double>(0, 0) / m2.at<double>(2, 0),
            m2.at<double>(1, 0) / m2.at<double>(2, 0),
            0));
    }

    return objectPoints;
}

bool CalibrateCamera(const std::vector<Point3f> &vecObject, const Size &patternsize, Mat &image, CameraCalibration &cameraCalibration, vector<Point2f> *pcorners = nullptr, bool showImages = false)
{
    vector<Point2f> corners;

    cameraCalibration.isCalibrated = false;
    if (showImages) {
        imshow("image", image);
        waitKey(1);
    }

    bool patternfound = findChessboardCorners(image, patternsize, corners,
        CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE
        + CALIB_CB_FAST_CHECK);

    if (patternfound) {
        Mat grayImage;
        cvtColor(image, grayImage, COLOR_BGR2GRAY);
        cornerSubPix(grayImage, corners, Size(11, 11), Size(-1, -1),
            TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.001));
        drawChessboardCorners(image, patternsize, corners, patternfound);
        if (showImages) {
            imshow("image", image);
            waitKey(1);
        }
    }
    else
        return false;

    Size imageSize = Size(image.cols, image.rows);

    std::vector<std::vector<Point3f>> vecVecObject;
    std::vector<std::vector<Point2f>> vecVecImage;
    vecVecObject.push_back(vecObject);
    vecVecImage.push_back(corners);

    std::vector<Mat> rvecs;
    std::vector<Mat> tvecs;

    cameraCalibration.reprojectionError = calibrateCamera(vecVecObject, vecVecImage, imageSize, cameraCalibration.camMat, cameraCalibration.distCoeffs, rvecs, tvecs);

    cameraCalibration.rvec = rvecs[0];
    cameraCalibration.tvec = tvecs[0];

    double c_x = cameraCalibration.camMat.at<double>(0, 2);
    double c_y = cameraCalibration.camMat.at<double>(1, 2);

    Mat camMat2 = Mat::zeros(3, 4, CV_64F);
    camMat2.at<double>(0, 0) = cameraCalibration.camMat.at<double>(0, 0);
    camMat2.at<double>(1, 1) = cameraCalibration.camMat.at<double>(1, 1);
    camMat2.at<double>(2, 2) = cameraCalibration.camMat.at<double>(2, 2);

    cout << "camMat" << endl;
    cout << cameraCalibration.camMat << endl;
    cout << endl;
    cout << "camMat2" << endl;
    cout << camMat2 << endl;
    cout << endl;

    Mat rotMat;
    Rodrigues(cameraCalibration.rvec, rotMat);

    cout << "rotMat" << endl;
    cout << rotMat << endl;
    cout << endl;

    Mat eMat = Mat::zeros(4, 4, CV_64F);
    eMat.at<double>(0, 0) = rotMat.at<double>(0, 0);
    eMat.at<double>(1, 0) = rotMat.at<double>(1, 0);
    eMat.at<double>(2, 0) = rotMat.at<double>(2, 0);
    eMat.at<double>(0, 1) = rotMat.at<double>(0, 1);
    eMat.at<double>(1, 1) = rotMat.at<double>(1, 1);
    eMat.at<double>(2, 1) = rotMat.at<double>(2, 1);
    eMat.at<double>(0, 2) = rotMat.at<double>(0, 2);
    eMat.at<double>(1, 2) = rotMat.at<double>(1, 2);
    eMat.at<double>(2, 2) = rotMat.at<double>(2, 2);
    eMat.at<double>(0, 3) = cameraCalibration.tvec.at<double>(0, 0);
    eMat.at<double>(1, 3) = cameraCalibration.tvec.at<double>(1, 0);
    eMat.at<double>(2, 3) = cameraCalibration.tvec.at<double>(2, 0);
    eMat.at<double>(3, 3) = 1;

    Mat m0 = camMat2 * eMat;
    Mat m0_33 = Mat(3, 3, CV_64F);
    m0_33.at<double>(0, 0) = m0.at<double>(0, 0);
    m0_33.at<double>(1, 0) = m0.at<double>(1, 0);
    m0_33.at<double>(2, 0) = m0.at<double>(2, 0);
    m0_33.at<double>(0, 1) = m0.at<double>(0, 1);
    m0_33.at<double>(1, 1) = m0.at<double>(1, 1);
    m0_33.at<double>(2, 1) = m0.at<double>(2, 1);
    m0_33.at<double>(0, 2) = m0.at<double>(0, 3);
    m0_33.at<double>(1, 2) = m0.at<double>(1, 3);
    m0_33.at<double>(2, 2) = m0.at<double>(2, 3);
    cameraCalibration.invMat = m0_33.inv();

    cout << "m0_33" << endl;
    cout << m0_33 << endl;
    cout << endl;
    cout << "invMat" << endl;
    cout << cameraCalibration.invMat << endl;
    cout << endl;

    if (pcorners) {
        for (int count = 0; count < corners.size(); count++) {
            (*pcorners).push_back(corners.at(count));
        }
    }

    cameraCalibration.isCalibrated = true;
    return true;
}

int main(int argc, char* argv[])
{
    Mat image;
    
    image = imread("WIN_20210517_12_04_22_Pro.jpg"); //, IMREAD_GRAYSCALE
    Size patternsize(5, 5); //interior number of corners

    std::vector<Point3f> vecObject;
    vecObject.push_back(Point3f(x0,y0,0));
    ...
    vecObject.push_back(Point3f(x24,y24,0));

    CameraCalibration cameraCalibration;

    vector<Point2f> *pcorners = new vector<Point2f>;

    CalibrateCamera(vecObject, patternsize, image, cameraCalibration, pcorners, true);

    vector<Point2f> &corners = *pcorners;

    std::vector<Point2f> imagePoints;
    projectPoints(vecObject, cameraCalibration.rvec, cameraCalibration.tvec, cameraCalibration.camMat, cameraCalibration.distCoeffs, imagePoints);

    std::vector<cv::Point3f> objectPoints = Unproject(corners, cameraCalibration);

    float errY = 0;
    float errX = 0;
    float errU = 0;
    float errV = 0;
    for (int count = 0; count < vecObject.size(); count++) {
        cout << "GT(x,y,z) = " << vecObject.at(count).x << ", ";
        cout << vecObject.at(count).y << ", ";
        cout << vecObject.at(count).z << ", ";
        cout << "(x,y,z) = " << objectPoints.at(count).x << ", ";
        cout << objectPoints.at(count).y << ", ";
        cout << objectPoints.at(count).z << ", ";
        cout << "(u,v) = " << imagePoints.at(count).x << ", ";
        cout << imagePoints.at(count).y << ", ";
        cout << "GT(u,v) = " << corners.at(count).x << ", ";
        cout << corners.at(count).y << ", ";
        cout << endl;

        errX += abs(vecObject.at(count).x - objectPoints.at(count).x);
        errY += abs(vecObject.at(count).y - objectPoints.at(count).y);
        errU += abs(imagePoints.at(count).x - corners.at(count).x);
        errV += abs(imagePoints.at(count).y - corners.at(count).y);

    }
    cout << endl;
    errX = errX / vecObject.size();
    errY = errY / vecObject.size();
    cout << "errX = " << errX << endl;
    cout << "errY = " << errY << endl;
    errU = errU / vecObject.size();
    errV = errV / vecObject.size();
    cout << "errU = " << errU << endl;
    cout << "errV = " << errV << endl;

    if (pcorners)
        delete pcorners;

    return 0;
}