如何使用3d-to-2d-points估计相机姿势(使用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]);
        }
    }

为了估计相机姿势我使用solvePnP(和solvePnPRansac)作为在多个位置中所述(1,2,3,4).solvePnP的结果我用作Projection(Calib3d.projectPoints)的输入.连续结果的倒数[R | t]我用作估计的姿势.

因为我在生产环境中的结果太糟糕了,所以我创建了一个测试环境.在那个环境中我放置了相机(这是因为它的3D形状(它是一个玻璃)在桌子的边缘略微向下旋转.这个边缘我用作世界坐标系的纵坐标.我搜索了如何打开cv坐标系可能是定向的,并找到了不同的答案(一个在stackoverflow上,一个在官方youtube上谈论opencv).无论如何我测试了如果我通过投影3D点(在该坐标系中描述)得到坐标系统的图像和检查给定的世界形状是否保持不变.

所以我走向上方,向前指向y,向下指向x向右.

为了更接近我的解决方案,我在测试环境中估算了姿势.平移向量输出和欧拉天使输出是指[R | t]的倒数.欧拉天使可能无法正确显示(如果我们考虑到顺序,它们可能会被交换或错误),因为我使用开放式坐标系统以对流(我假设参考飞机坐标系)方程计算它.(计算发生在我将附加的Pose类中).但无论如何甚至翻译矢量(反向)似乎都是错误的(在我的简单测试中).

在使用该图像进行的一次测试中,我有一个30°的滚动(可能是飞机坐标的俯仰)和向上翻转50 厘米.这看起来更合理.所以我假设因为我的观点是共面的,我可能会得到模棱两可的结果.所以我实现了另一个测试,其中一个点在Z轴上发生了变化.但是通过这次测试,即使投影也失败了.

对于solvePnP,我尝试了所有不同的求解算法标志和ransac算法的不同参数.

也许你可以以某种方式帮助我找到我的错误,或者向我展示一条解决我最初问题的好途径.我还要附上我的调试源代码,其中包含许多println语句和调试图像.此代码包含我的点测量值.

感谢您的帮助.

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

1.png

编辑22.03.2015: 最后我找到了错误.

1. 我在for循环中修改了一个Mat对象,因为OpenCV通过引用调用很多,我在这里做得不够谨慎.因此重投的tvec和rvec是不对的.
2. 测试环境中的一个点(在图像坐标中)由于轴向混淆而被标记为错误.

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

不幸的是,OpenCV PnP算法:"ITERATIVE,P3P,EPNP"会返回各种结果,即使使用非常不准确但非常接近的内在猜测,结果有时也是正确的.该P3P算法应该提供3个解决方案,但OpenCV的只提供一个.EPNP应该会取得良好的效果,但EPNP OpenCV会从我的人类观察中评估出最差的结果.

现在的问题是,如何过滤不准确的值或确保OpenCV函数返回有效值.(也许我shuold修改本机代码以获得3个PnP解决方案).

这里 的压缩图像(37MB),显示我当前的结果(使用ITERATIVE PnP-Solver),内部猜测零旋转和75厘米向上.打印输出具有x轴向前,y轴向左和z向下,以及相应的滚动,俯仰和偏转角.

Answer 1

我在尝试实现头部跟踪系统时学到的一件事是，您应该从简单的问题开始，而不是转向更复杂的问题。你的问题很长，不幸的是我没有时间分析它并搜索代码中的错误或逻辑错误，所以至少我会尝试给你一些提示和工作示例。

这是查找对象平移和旋转的 OpenCV 教程。它是用 Python 编写的，如果这是我旧的 C++ 项目的一部分的问题。
我的项目使用solvePnP或solvePnPRansac函数执行相同的任务（您可以更改模式）。请注意，我的代码是一些旧的“游乐场”项目的一部分，因此即使在我执行的清理之后，它仍然非常混乱。当你运行它时，向相机显示打印的棋盘，按“p”开始位置和旋转估计，按“m”更改模式（0-ransac、1-pnp、2-posit 这似乎不起作用......）或“d”使用畸变系数打开/关闭。
这两个项目都依赖于查找棋盘模式，但应该很容易修改它们以使用其他一些对象。

相机校准 - 虽然我一直在研究我的头部跟踪系统，但我从未成功地校准过相机两次并获得相同的结果...所以我决定使用一些我在 github 上找到的校准文件，它运行良好 -在这里您可以找到有关此文件的链接的更多信息。

编辑：

尝试从尽可能简单的解决方案开始，在某些（甚至简单）情况下给出良好的结果。在我看来，一个好的开始点是用教程（这个）中的打印棋盘替换测试环境中的一张纸并使其正常工作。从这个问题转向你的问题比从你的问题开始要容易得多。尝试用任何编程语言制定任何工作解决方案 - 考虑使用 OpenCV 的 Python 或 C++ 版本 - 比 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), (35 5 ,72), (35 5 ,101)

（我猜这是你的错误，不是正确的结果）你会得到：

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

这可能更接近正确的结果。更改相机矩阵会极大地改变结果，因此请考虑测试本文中的值。

请注意，在 C++ 中，所有 rvec 值都乘以180.0/3.14-，solvePnPRansac 返回的 python rvec 向量包含以弧度为单位的角度。