Sloving PnP（DLT）using only the C++ standard library

Languages: |English | 中文|

1. Introduction

• Purpose：To enable the use of PnP in scenarios where it is not possible to rely on third-party libraries such as OpenCV and Eigen, mainly for the reproduction of the interface cv::solvePnP.(flag=cv::SOLVEPNP_ITERATIVE).

• Overall content: The implementation of PnP relies heavily on the

  1). Singular value decomposition (SVD) algorithm,

  2). the Cholesky decomposition algorithm,

  3). and the Levenberg-Marquardt (LM) algorithm.

  It is generally the same as in the OpenCV interface, but distortion and plane transforms are not considered for now.

• Results: Tested with randomly generated 2D data using custom 3D world coordinate points, the pose computation results are consistent with the cv::solvePnP interface. But on the rotation matrix R and translation vector t, there is a discrepancy with OpenCV at 8 decimal places, and the reprojection error is higher than OpenCV. This phenomenon should be related to the internal optimization of OpenCV.

2. Quickly Start

There is an example for using custom PnP.

#include "../include/PnP.h"

std::vector<Matrix::Point3D> pWorld = {
    {-24.02275467, -61.03339767, 6.6065011},
    // ... other 3D points ...
};

std::vector<Matrix::Point2D> pImage = {
    {287.70471191, 115.31045532},
    // ... other 2D points ...
};

// intrinsics
Matrix K(std::vector<Matrix::Point3D>{{640, 0, 320}, {0, 640, 240}, {0, 0, 1}});

// result
Matrix R_dlt, t_dlt;

// reproject error
double err_dlt;

// start
PnP solver(pImage, pWorld, K);
solver.solveDLT(R_dlt, t_dlt);

// convert to euler angles
Matrix::EulerAngles eulerAnglesMine = Matrix::radiansToDegrees(Matrix::rotationMatrixToEulerAngles(R_dlt));
err_dlt = (Matrix::reProjectPoints(R_dlt, Matrix(t_dlt), K, pWorld) - Matrix(pImage)).norm(1);

std::cout << "=== Custom ==="
            << "\n Rotation: \n"
            << R_dlt
            << "\n Translation: \n"
            << t_dlt
            << "\n Pose: \n"
            << eulerAnglesMine
            << "\n Error: \n"
            << err_dlt
            << std::endl;

3. Unit Test

Functions testSVD() and testLM() are provided in src/test.cpp for separate testing of the SVD and LM modules, which can be easily compared with the OpenCV interface. The overall process testing is done using testAll().

3.1 SVD

#include <opencv2/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/imgproc/detail/distortion_model.hpp>
#include <opencv2/calib3d/calib3d_c.h>

#include "../include/PnP.h"

Matrix A(3, 3);
Matrix U(A.rows, A.rows), S(A.rows, A.cols), Vt(A.cols, A.cols);
A[0] = {-0.204046952762572398, 0.266198297979582499, 0.118685355971054646};
A[1] = {-0.135800315684291656, -0.215025536429863412, 0.248807514366522736};
A[2] = {0.257887590425267599, 0.097392890562252157, 0.224925551666915108};

A.svd(U, S, Vt);
std::cout << "matrix U: \n" 
            << U
            << "\n matrix Sigma: \n" 
            << S
            << "\n matrix Vt: \n"
            << Vt
            << std::endl;

Matrix reconstructed = U * S * Vt;
Matrix uUt = U * U.transpose();
Matrix vVt = Vt.transpose() * Vt;

double sum = 0;
for (int i = 0; i < A.rows; i++)
{
    for (int j = 0; j < A.cols; j++)
    {
        sum += (reconstructed[i][j] - A[i][j]) * (reconstructed[i][j] - A[i][j]);
    }
}
std::cout << "reconstruct: \n"
            << reconstructed << std::endl;
std::cout << "reconstructed error: " << sqrt(sum) << std::endl;
std::cout << "U orthogonality error: " << (uUt - Matrix::identity(3)).norm() << std::endl;
std::cout << "V orthogonality error: " << (vVt - Matrix::identity(3)).norm() << std::endl;

// ==========================OpenCV===========================================
cv::Mat _U, _S, _Vt, _ATA = matrixToCVMat(A.transpose() * A);
cv::SVD::compute(_ATA, _S, _U, _Vt, cv::SVD::FULL_UV);
std::cout << "\n CV SVD U \n"
            << _U
            << "\n CV SVD S \n"
            << _S
            << "\n CV SVD Vt \n"
            << _Vt
            << "\n reconstruct: \n"
            << _U * cv::Mat::diag(_S) * _Vt << std::endl;

3.2 LM

In order to exclude the influence of other factors, the initial R and t obtained from the OpenCV, is used as the input for the custom LM, so the OpenCV source code is included in test.cpp

Matrix K(std::vector<Matrix::Point3D>{{fx, 0, cx}, {0, fy, cy}, {0, 0, 1}});
cv::Mat K_CV(3, 3, CV_64F);
for (int i = 0; i < K_CV.rows; i++)
{
    for (int j = 0; j < K_CV.cols; j++)
    {
        K_CV.at<double>(i, j) = K[i][j];
    }
}

std::vector<Matrix::Point3D> pWorld = {
    {-24.02275467, -61.03339767, 6.6065011},
    // ... other test data ...
};

// opencv input
cv::Mat objPoints(pWorld.size(), 3, CV_64F);
for (int i = 0; i < pWorld.size(); i++)
{
    objPoints.at<double>(i, 0) = pWorld[i].x;
    objPoints.at<double>(i, 1) = pWorld[i].y;
    objPoints.at<double>(i, 2) = pWorld[i].z;
}

// random 2d test data
for (int i = 0; i < 10; i++)
{
    std::cout << "test: " << i << std::endl;

    Matrix R_random(3, 3);
    Matrix t_random(3, 1);
    generate_random_rotation_matrix(R_random, t_random);
    std::vector<Matrix::Point2D> pImage = Matrix::reProjectPoints(R_random, t_random, K, pWorld).toPoint2DVector();

    std::vector<cv::Point2d> points2D_ideal;
    for (auto p : pImage)
    {
        points2D_ideal.push_back(cv::Point2d(p.x, p.y));
    }

    cv::Mat imgPoints(pImage.size(), 2, CV_64F);
    for (int i = 0; i < pImage.size(); i++)
    {
        imgPoints.at<double>(i, 0) = pImage[i].x;
        imgPoints.at<double>(i, 1) = pImage[i].y;
    }

    cv::Mat rvec(3, 1, CV_64F), tvec(3, 1, CV_64F);
    cv::Mat discoff(1, 4, CV_64F);
    CvMat c_objectPoints = cvMat(objPoints), c_imagePoints = cvMat(imgPoints);
    CvMat c_cameraMatrix = cvMat(K_CV), c_distCoeffs = cvMat(discoff);
    CvMat c_rvec = cvMat(rvec), c_tvec = cvMat(tvec), matR, matT;

    cvFindExtrinsicCameraPa(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
                            (c_distCoeffs.rows && c_distCoeffs.cols) ? &c_distCoeffs : 0,
                            &c_rvec, &c_tvec, matR, matT, false);
    cv::Mat R(3, 3, CV_64F);
    cv::Rodrigues(rvec, R);
    EulerAngles pose = radiansToDegrees(rotationMatrixToEulerAngles2(cvMatToMatrix(R)));

    std::cout << "R CV" << std::endl;
    std::cout << R << std::endl;
    std::cout << "t CV" << std::endl;
    std::cout << tvec << std::endl;
    std::cout << "pose CV" << std::endl;
    std::cout << pose << std::endl;

    // matR: opencv output the R_initial, so as matT
    Matrix R_pnp = cvMatToMatrix(cv::cvarrToMat(&matR)), t_pnp = cvMatToMatrix(cv::cvarrToMat(&matT));
    PnP::levenbergMarquardtOptimization(R_pnp, t_pnp, K, pImage, pWorld);
    EulerAngles pose_pnp = radiansToDegrees(rotationMatrixToEulerAngles2(R_pnp));

    std::cout << "R Custom" << std::endl;
    std::cout << R_pnp << std::endl;
    std::cout << "t Custom" << std::endl;
    std::cout << t_pnp << std::endl;
    std::cout << "pose Custom" << std::endl;
    std::cout << pose_pnp << std::endl;
}

4. Acknowledgement

• SVD section thanks to implementation of @wangjt23.
• The implementation of the LM part is thanks to here a blog for the detailed derivation of the equation.