java - 如何使用 3d-to-2d-point-correspondences 估计相机姿势(使用 opencv)

Question

您好，我的目标是开发用于飞机(模拟器)驾驶舱的头部跟踪功能，以提供 AR 以支持平民飞行员在视觉条件不佳的情况下着陆和飞行。

我的方法是检测我知道 3D 坐标的特征点(在黑暗的模拟器 LED 中)，然后计算估计的(头戴式相机的)姿势 [R|t](旋转与平移连接)。

我确实遇到的问题是估计的姿势似乎总是错误的，并且我的 3D 点的投影 (我也用来估计姿势) 与 2D 图像点不重叠 或不可见) .



我的问题是:

如何使用一组给定的 2D 到 3D 点对应来估计相机姿势。

为什么我尝试它的方式不起作用，哪里可能是错误来源？

测量(3D 和 2D 点以及相机矩阵)必须有多精确才能使理论解决方案在现实生活环境中工作？

理论上该方法是否适用于共面点(x，y 轴已更改)？

我使用的硬件是 Epson BT-200。

在飞机中，我定义了一个固定的纵坐标，我期望作为程序结果的相对平移和旋转。该程序检测(独特)LED 的图像坐标并将它们与相应的 3D 坐标相匹配。使用我使用 open-cv 示例 android 代码 (https://github.com/Itseez/opencv/tree/master/samples/android/camera-calibration) 获得的相机矩阵，我尝试使用 solvePnP 估计姿势。

我的相机矩阵和失真略有不同。以下是我从程序中收到的一些值。我确保打印出来的圆形图案的圆形距离与源代码中写的相同(以米为单位)。

以下是一些示例以及我如何创建它的 OpenCV Mat。

//  protected final double[] DISTORTION_MATRIX_VALUES = new double[]{
//          /*This matrix should have 5 values*/
//          0.04569467373955304,
//          0.1402980385369059,
//          0,
//          0,
//          -0.2982135315849994
//  };

//  protected final double[] DISTORTION_MATRIX_VALUES = new double[]{
//          /*This matrix should have 5 values*/
//          0.08245931646421553,
//          -0.9893762277047577,
//          0,
//          0,
//          3.23553287438898
//  };

//  protected final double[] DISTORTION_MATRIX_VALUES = new double[]{
//          /*This matrix should have 5 values*/
//          0.07444480392067945,
//          -0.7817175834131075,
//          0,
//          0,
//          2.65433773093283
//  };
    protected final double[] DISTORTION_MATRIX_VALUES = new double[]{
            /*This matrix should have 5 values*/
            0.08909941096327206,
            -0.9537960457721699,
            0,
            0,
            3.449728790843752
    };

    protected final double[][] CAMERA_MATRIX_VALUES = new double[][]{
            /*This matrix should have 3x3 values*/
//          {748.6595405553738, 0, 319.5},
//          {0, 748.6595405553738, 239.5},
//          {0, 0, 1}
//          {698.1744297982436, 0, 320},
//          {0, 698.1744297982436, 240},
//          {0, 0, 1}
//          {707.1226937511951, 0, 319.5},
//          {0, 707.1226937511951, 239.5},
//          {0, 0, 1}
            {702.1458656346429, 0, 319.5},
            {0, 702.1458656346429, 239.5},
            {0, 0, 1}
    };

    private void initDestortionMatrix(){
        distortionMatrix = new MatOfDouble();
        distortionMatrix.fromArray(DISTORTION_MATRIX_VALUES);
    }

    private void initCameraMatrix(){
        cameraMatrix = new Mat(new Size(3,3), CvType.CV_64F);
        for(int i=0;i<CAMERA_MATRIX_VALUES.length; i++){
            cameraMatrix.put(i, 0, CAMERA_MATRIX_VALUES[i]);
        }
    }

为了估计相机姿势，我确实使用了在多个位置( solvePnPRansac 、 1 、 2 、 3 )中描述的 solvePnP(和 4 )。我将 solvePnP 的结果用作投影 ( Calib3d.projectPoints ) 的输入。连接结果的倒数 [R|t] 我确实用作估计姿势。

因为我在生产环境中的结果太糟糕了，所以我创建了一个测试环境。在那个环境中，我将相机(这是因为它是 3D 形状(它是玻璃)在 table 边缘略微向下旋转。我确实将这个边缘用作世界坐标系的纵坐标。我搜索了 open-cv坐标系可能是定向的并找到了不同的答案(一个在 stackoverflow 上，一个在官方 youtube-talk about opencv 中)。无论如何，我测试了我是否通过图像上的投影 3D 点(在该坐标系中描述)获得了正确的坐标系，并且检查给定的世界形状是否保持不变。

所以我想到了 z 指向前方，y 向下，x 向右。


为了更接近我的解决方案，我估计了我测试环境中的姿势。平移 vector 输出和欧拉角输出指的是 [R|t] 的倒数。欧拉角可能显示不正确(如果我们考虑顺序，它们可能会交换或错误)，因为我使用对流(我假设引用飞机坐标系)方程，使用 open-cv 坐标系计算它。 (计算发生在我将附加的 Pose 类中)。但无论如何，即使是(逆的)平移 vector 似乎也是错误的(在我的简单测试中)。


在对该图像的一次测试中，我的滚动(可能是飞机坐标中的俯仰角)为 30°，并且平移向上为 50 cm 。这似乎更合理。所以我假设因为我的点是共面的，我可能会得到模棱两可的结果。所以我实现了另一个测试，其中一个点在 Z 轴上发生了变化。但是通过这个测试，甚至投影也失败了。

对于 solvePnP，我为 ransac algorithm 尝试了所有不同的求解算法标志和不同的参数。

也许你能以某种方式帮助我找到我的错误，或者向我展示解决我最初问题的好方法。我还将附上带有许多 println 语句和调试图像的调试源代码。 此代码包含我的点测量值 。

提前感谢您的帮助。

Main.java 类:
Pose.java 类:
0.png


1.png


编辑 22.03.2015:
终于，我找到了我犯的错误。

我在 for 循环中修改了一个 Mat 对象，因为 OpenCV 工作很多
通过引用调用，我在这里不够小心。所以
用于重新投影的 tvec 和 rvec 不正确。

我在测试环境中的一个观点(在图像中
坐标)，由于轴方向混淆而被标记为错误。

所以我的方法总的来说是正确的。我在我的测试数据集中没有收到至少(通常)有效的重新投影。

不幸的是，OpenCV PnP 算法:“ITERATIVE、P3P、EPNP”返回各种结果，即使使用非常不准确但接近的内在猜测，结果也只是有时是正确的。 P3P algorithm 应该提供 3 种解决方案，但 OpenCV 只提供一种。 EPNP 应该返回良好的结果，但使用 EPNP OpenCV 返回最差的结果，根据我的人类观察评估。

现在的问题是，如何过滤不准确的值或确保 OpenCV 函数返回有效值。 (也许我应该修改 native 代码以接收 3 个 PnP 解决方案)。

compressed images here (37MB) 确实显示了我当前的结果(使用 ITERATIVE PnP-Solver)，具有零旋转和 75 cm 向上的内在猜测。打印输出具有向前的 x 轴，向左和 z 向下的 y 轴，以及相应的滚动、俯仰和偏航角。

Answer 1

我在尝试实现我的头部跟踪系统期间学到的一件事是，您应该从简单的问题开始，而不是转向更复杂的问题。你的问题很复杂，不幸的是我没有时间分析它并搜索你代码中的错误或逻辑错误，所以至少我会尝试给你一些提示和工作示例。
Here是用于查找对象平移和旋转的 OpenCV 教程。它是用 Python 编写的，如果有问题 here我旧 C++ 项目的一部分。
我的项目使用 solvePnP 或 solvePnPRansac 函数(您可以更改模式)执行相同的任务。请注意，我的代码是一些旧的“游乐场”项目的一部分，因此即使在我执行的清洁之后它也非常困惑。运行时，向相机显示打印的棋盘，按“p”开始位置和旋转估计，按“m”更改模式(0-ransac、1-pnp、2-posit 似乎不起作用...)或 'd' 使用离散系数打开/关闭。
这两个项目都依赖于寻找棋盘模式，但修改它们以使用其他一些对象应该很容易。
相机校准 - 虽然我一直在研究我的头部跟踪系统，但我从来没有设法用相同的结果校准相机两次......所以我决定使用我在 github 上找到的一些校准文件，它运行良好 - here你可以找到更多关于这个文件的链接的信息。
编辑:
尝试从尽可能简单的解决方案开始，在某些(甚至是简单的)情况下提供良好的结果。在我看来，一个好的开始是用教程 ( this one ) 中的打印棋盘替换测试环境中的一张纸并使其工作。从这个转移到你的问题会比从你的问题开始容易得多。尝试使用任何编程语言制作任何可行的解决方案 - 考虑使用 Python 或 C++ 版本的 OpenCV - 有比 Java 版本更多的教程/示例，并且将您的代码结果与某些工作代码的结果进行比较会使其更容易。当您有一些可行的解决方案时，请尝试修改它以适用于您的测试环境。有很多事情可能会导致它现在无法正常工作 - 点数不足，代码中甚至 OpenCV Java 包装器中的错误，结果的错误解释等......
编辑2:
使用您的代码中的点，我设法获得了以下结果:

rvec = [[-158.56293283], [ 1.46777938], [ -17.32569125]]
tvec = [[ -36.23910413], [ -82.83704819], [ 266.03157578]]

不幸的是，对我来说很难说结果是否好...对我来说唯一可能错的是2个角度不同于0(或180)。但是如果你改变 points2d的最后一行来自 (355,37), (353,72), (353,101)到

(355,37), (355,72), (355,101)

(我想这是你的错误，而不是正确的结果)你会得到:

rvec = [[-159.34101842], [ 1.04951033], [ -11.43731376]]
tvec = [[ -25.74308282], [ -82.58461674], [ 268.12321097]]

这可能更接近正确的结果。改变相机矩阵会改变很多结果，所以考虑测试来自 this post 的值.
请注意，所有 rvec 值都乘以 180.0/3.14 - 在 C++ 和 python 中，solvePnPRansac 返回的 rvec vector 包含以弧度表示的角度。