opencv - undistortPoints() 无法处理镜头畸变

Question

我使用 openCV 函数 projectPoints() 来旋转、平移和投影一组 3D 点，并使用 solvePnp() 来找到这个旋转和平移。当镜头畸变系数全部为零时，这很有效，否则就会失败。完全失败只需要这么少的失真:

 distCoeffs << 0.0, 0.01, 0.0, 0.0, 0.0;  

代码如下:

#include <iostream>
#include "opencv.hpp"
using namespace std;
using namespace cv;
#define DEG2RAD (3.1415293/180.0)
#define RAD2DEG (1.0/DEG2RAD)

int main() {
    const int npoints = 10; // number of points

    // extrinsic
    const Point3f tvec(10, 20, 30);
    Point3f rvec(3, 5, 7);
    cout << "Finding extrinsic parameters (PnP)" << endl;
    cout<<"Test transformations: ";
    cout<<"Rotation: "<<rvec<<"; translation: "<<tvec<<endl;
    rvec*=DEG2RAD;

    // intrinsic
    Mat_ <double>cameraMatrix(3, 3);
    cameraMatrix << 300., 0., 200., 0, 300., 100., 0., 0., 1.;
    Mat_ <double>distCoeffs(1, 5); //  (k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6]]) of 4, 5, or 8 elements.
    //distCoeffs << 1.2, 0.2, 0., 0., 0.;  // non-zero distortion
    distCoeffs << 0.0, 0.0, 0.0, 0.0, 0.0; // zero distortion
    cout<<"distrotion coeff: "<<distCoeffs<<endl;

    cout<<"============= Running PnP..."<<endl;
    vector<Point3f> objPts(npoints);
    vector<Point2f> imagePoints(npoints);
    Mat rvec_est, tvec_est;
    randu(Mat(objPts), 0.0f, 100.0f);

    // project
    projectPoints(Mat(objPts), Mat(rvec), Mat(tvec), cameraMatrix, distCoeffs, Mat(imagePoints));

    // extrinsic
    solvePnP(objPts, imagePoints, cameraMatrix, distCoeffs, rvec_est, tvec_est);
    cout<<"Rotation: "<<rvec_est*RAD2DEG<<endl;
    cout<<"Translation "<<tvec_est<<endl;

    return 0;
}

当所有失真系数为 0 时，结果正常:

寻找外在参数 (PnP)
测试转换:旋转:[3, 5, 7]；翻译:[10, 20, 30]
分布系数:[0, 0, 0, 0, 0]
============== 运行即插即用...
旋转:[2.999999581709123; 4.999997813985293； 6.999999826089725]
翻译 [9.999999792663072; 19.99999648222693； 29.99999699621362]

然而，当它们不为零时，结果是完全错误的:

寻找外在参数 (PnP)
测试转换:旋转:[3, 5, 7]；翻译:[10, 20, 30]
分布系数:[1.2, 0.2, 0, 0, 0]
============== 运行即插即用...
旋转:[-91.56479629305277; -124.3631985067845； -74.46486950666471]
翻译 [-69.72473511009439; -117.7463271636532； -87.27777166027946]

既然有人问，我加 中间输入 - 一些 3D 点及其对非零失真系数的预测。我的相机矩阵是
cameraMatrix << 300., 0., 200., 0, 300., 100., 0., 0., 1.;

3d 点 [53.0283, 19.9259, 40.1059];二维投影 [1060.34, 700.59]
3d 点 [81.4385, 43.7133, 24.879];二维投影 [6553.88, 5344.22]
3d 点 [77.3105, 76.2094, 30.7794];二维投影 [5143.32, 6497.12]
3d 点 [70.2432, 47.8447, 79.219];二维投影 [771.497, 611.726]

另一个有趣的观察结果:当 distCoeff 非零时应用 undistort 并没有真正起作用(但当畸变系数全部为 0 时，它确实会产生相同的 2D 点):

cout<<"applying undistort..."<<endl;
vector<Point2f> imagePointsUndistort(npoints);
undistortPoints(Mat(imagePoints), Mat(imagePointsUndistort), cameraMatrix, distCoeffs);
for (int i=0; i<4; i++)
    cout<<"2d original "<<imagePoints[i]<<"; 2d undistort "<<imagePointsUndistort[i]<<endl;

应用不失真...
2d 原版 [1060.34, 700.59];二维不失真 [0, 0]
二维原件 [6553.88, 5344.22];二维不失真 [0, 0]
二维原件 [5143.32, 6497.12];二维不失真 [0, 0]
二维原件 [771.497, 611.726];二维不失真 [0, 0]

我尝试 undistort() 的原因是，如果取消已知内在参数的影响，PnP 就会变成 Ax=0 形式的最小方向问题。它需要分钟。近似线性解得 6 分，这可能会用 LMA 进一步改进(flags=CV_ITERATIVE)。从技术上讲，只有 6DOF，因此需要 3 个点，因此其他方法(flags=CV_P3P、CV_EPNP)需要的点数更少。无论如何，无论采用何种方法或点数如何，对于非零失真系数，结果仍然无效。我将尝试的最后一件事是将所有点放在 3D 平面上。它仍然失败:

 for (int i=0; i<npoints; i++)
        objPts[i].z=0.0f;

寻找外在参数 (PnP)
测试转换:旋转:[3, 5, 7]；翻译:[10, 20, 30]
分布系数:[1.2, 0.2, 0, 0, 0]
============== 运行即插即用...
旋转:[-1830.321574903016; 2542.206083947917； 2532.255948350521]
翻译 [1407.918216894239; 1391.373407846455； 556.7108606094299]

Answer 1

如何让你的代码工作？

我能够使用您提供的代码重现所描述的行为，但是，以下两个选项之一可以解决问题:

替换 const Point3f tvec(10, 20, 30);来自 const Point3f tvec(10, 20, N);哪里N远低于 0(例如 -300)或远大于 100(例如 300)。

更换您的电话 solvePnP调用 solvePnPRansac .

为什么这些更改中的每一个都修复了不受欢迎的行为？

首先，考虑您的原始代码从 solvePnP 请求什么功能。您使用的旋转幅度相当小，因此为简单起见，我将假设旋转是身份。然后，将相机定位在世界坐标 X=10、Y=20 和 Z=30 处，并随机生成具有在 [0,100]3 中均匀绘制的世界坐标 (X,Y,Z) 的对象点。 因此，相机位于物点的可能范围的中间 ，如下图所示:



这意味着可以在非常靠近焦平面(即通过光学中心并垂直于光轴的平面)处生成物点。这些对象点在相机图像中的投影是不确定的。 然而，实际上undistortPoints的非线性优化算法即使对于靠近焦平面的物点也不稳定 .这种不稳定性导致 undistortPoints 的迭代算法发散，除非系数都为零，因为在这种情况下，初始值在估计期间保持严格恒定。

因此，避免这种行为的两种可能的解决方案如下:

避免在相机焦平面附近生成物点，即改变平移向量或物点坐标范围。

在 PnP 估计之前，例如使用 solvePnPRansac 消除离相机焦平面太近的物点，其未失真估计发散(异常值)。 .

详细原因 undistortPoints失败:

注意:正如我们所知的 3D 世界点，我使用以下调用来获得真实的未失真坐标，独立于 undistortPoints 的结果。 :

cv::projectPoints(obj_pts, rvec, tvec, cv::Mat_<double>::eye(3,3), cv::Mat_<double>::zeros(5,1), true_norm_pts);

下面的函数是什么 undistortPoints的简化版是在做:

void simple_undistort_point(const cv::Mat &img_pt,
                            const cv::Mat_<double> &K,
                            const cv::Mat_<double> &D,
                            cv::Mat &norm_pt)
{
    // Define temporary variables
    double k[8]={D.at<double>(0),
                 D.at<double>(1),
                 D.at<double>(2),
                 D.at<double>(3),
                 D.at<double>(4)},
           fx, fy, ifx, ify, cx, cy;
    fx = K.at<double>(0,0);
    fy = K.at<double>(1,1);
    ifx = 1./fx;
    ify = 1./fy;
    cx = K.at<double>(0,2);
    cy = K.at<double>(1,2);
    // Cancel distortion iteratively
    const int iters = 5;
    double x, y, x0, y0;
    x0=x=(img_pt.at<double>(0)-cx)*ifx;
    y0=y=(img_pt.at<double>(1)-cy)*ify;
    for(int j = 0; j < iters; ++j)
    {
        double r2 = x*x + y*y;
        double icdist = 1/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
        double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
        double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
        x = (x0 - deltaX)*icdist;
        y = (y0 - deltaY)*icdist;
    }
    // Store result
    norm_pt.create(1,2,CV_64F);
    norm_pt.at<double>(0) = x;
    norm_pt.at<double>(1) = y;
}

如果添加代码来检查如何 x和 y随着每次迭代的变化，您会看到迭代优化因 r2 而发散。一开始就很大。这是一个日志示例:

#0:   [2.6383300, 1.7651500]    r2=10.0766000, icdist=0.0299408, deltaX=0, deltaY=0
#1:   [0.0789937, 0.0528501]    r2=0.00903313, icdist=0.9892610, deltaX=0, deltaY=0
#2:   [2.6100000, 1.7462000]    r2=9.86128000, icdist=0.0309765, deltaX=0, deltaY=0
#3:   [0.0817263, 0.0546783]    r2=0.00966890, icdist=0.9885120, deltaX=0, deltaY=0
#4:   [2.6080200, 1.7448800]    r2=9.84637000, icdist=0.0310503, deltaX=0, deltaY=0
end:  [0.0819209, 0.0548085]
true: [0.9327440, 0.6240440]

当 r2很大， r2*r2*r2巨大因此 icdist非常小，因此下一次迭代从一个非常小的 r2 开始.当 r2很小， icdist接近 1，因此 x和 y分别设置为 x0和 y0我们回来了一个大 r2 ， 等等。

那么为什么是 r2一开始就这么大？因为这些点可能靠近焦平面生成，在这种情况下它们远离光轴(因此非常大 r2)。请参阅以下日志示例:

img_pt#0=[991.4992804037340, 629.5460091483255], r2=10.07660, norm(cv_undist-true)=1.0236800
img_pt#1=[5802.666489402056, 4402.387472311543], r2=554.4490, norm(cv_undist-true)=2.1568300
img_pt#2=[5040.551339386630, 5943.173381042060], r2=639.7070, norm(cv_undist-true)=2.1998700
img_pt#3=[741.9742544382640, 572.9513930063181], r2=5.749100, norm(cv_undist-true)=0.8158670
img_pt#4=[406.9101658356062, 403.0152736214052], r2=1.495890, norm(cv_undist-true)=0.1792810
img_pt#5=[516.2079583447821, 1038.026553216831], r2=10.88760, norm(cv_undist-true)=1.0494500
img_pt#6=[1876.220394606081, 8129.280202695572], r2=747.5450, norm(cv_undist-true)=2.2472900
img_pt#7=[236.9935231831764, 329.3418854620716], r2=0.599625, norm(cv_undist-true)=0.0147487
img_pt#8=[1037.586015858139, 1346.494838992490], r2=25.05890, norm(cv_undist-true)=1.2998400
img_pt#9=[499.9808133105154, 715.6213031242644], r2=5.210870, norm(cv_undist-true)=0.7747020

您可以看到，对于大多数点， r2非常大，除了少数(#3、#4 和#7)也是与最佳失真精度相关的那些。

这个问题是由于 OpenCV 中实现的特定不失真算法造成的，该算法因其效率而被选择。其他非线性优化算法(例如 Levenberg-Marquardt)会更准确但也更慢，并且在大多数应用程序中肯定是一种矫枉过正。