您好，我的目标是开发用于飞机（模拟器）驾驶舱的头部跟踪功能，以提供 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 形状（它是玻璃）在桌子的边缘略微向下旋转。我将这个边缘用作世界坐标系的纵坐标。我搜索了 open-cv坐标系可能会被定向并找到不同的答案（一个在stackoverflow上，一个在官方youtube-talk about opencv中）。无论如何，我测试了我是否通过在图像上投影3D点（在该坐标系中描述）得到了坐标系，并且检查给定的世界形状是否保持不变。
所以我上来时 z 指向前方，y 向下，x 向右。
为了更接近我的解决方案，我估计了测试环境中的姿势。平移向量输出和欧拉角输出指的是 [R|t] 的倒数。欧拉天使可能显示不正确（如果我们考虑顺序，它们可能会被交换或错误），因为我使用传统（我假设是指飞机坐标系）方程计算它，使用 open-cv 坐标系。（计算发生在我将附加的类 Pose 中）。但无论如何，即使是（反向的）平移向量似乎也是错误的（在我的简单测试中）。
在对该图像的一项测试中，我有一个 30° 的滚动（可能是飞机坐标中的俯仰）和一个向上的平移 50厘米。这似乎更合理。所以我假设因为我的点是共面的，我可能会得到模棱两可的结果。所以我实现了另一个测试，其中一个点在 Z 轴上发生了变化。但是通过这个测试，即使投影也失败了。
对于solvePnP，我尝试了所有不同的求解算法标志和ransac算法的不同参数。
也许你能以某种方式帮助我找到我的错误，或者告诉我一个解决我最初问题的好方法。我还将附上带有许多 println 语句和调试图像的调试源代码。此代码包含我的点测量。
提前感谢您的帮助。
1.png
编辑 22.03.2015： 最后我能够找到我犯的错误。
- 我在 for 循环中修改了一个 Mat 对象，因为 OpenCV 在引用调用方面工作得很多，而且我在这里不够小心。所以重投影的 tvec 和 rvec 是不正确的。
- 我在测试环境中的一个观点（在图像坐标中）由于轴方向混淆而被标记为错误。
所以我的方法总的来说是正确的。我的测试数据集中至少（通常）没有收到有效的重新投影。
不幸的是，OpenCV PnP 算法：“迭代，P3P，EPNP”返回各种结果，即使使用非常不准确但接近的内在猜测，结果也只是有时是正确的。P3P 算法应该提供 3种解决方案，但 OpenCV 只提供了一种。EPNP 应该返回良好的结果，但使用EPNP OpenCV 返回最差的结果，这是根据我的人类观察评估的。
现在的问题是，如何过滤不准确的值或确保 OpenCV 函数返回有效值。（也许我应该修改本机代码以接收 3 个 PnP 解决方案）。
此处的压缩图像 (37MB)确实显示了我当前的结果（使用迭代 PnP 求解器），固有猜测为零旋转和向上 75 厘米。打印输出有一个向前的 x 轴、向左的 y 轴和向下的 z 轴，以及对应的滚动、俯仰和偏航角。