你好，我的目标是发展用于飞机（模拟器）驾驶舱的头部跟踪功能，以提供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（并且解决PnPRansac）如多个位置所述（1,2,3,4）。我使用solvePnP的结果作为投影的输入（Calib3d.projectPoints）。我确实将连接结果 [R|t] 的倒数用作估计姿态。
因为我在生产环境中的结果太差了，所以我创建了一个测试环境。在那种环境中，我将相机（这是因为它是 3D 形状（它是一个玻璃））稍微向下旋转到桌子的边缘。我确实使用该边缘作为世界坐标系的纵坐标。我搜索了 open-cv 如何坐标系可能会定向并找到不同的答案（一对一）堆栈溢出以及官方 youtube 上的一篇关于 opencv 的讨论）。无论如何，我测试了是否通过在图像上投影 3D 点（在该坐标系中描述）来获得正确的坐标系，并检查给定的世界形状是否保持不变。
So I came up wiht z pointing foreward, y downward and x to the right.
To get closer to my solution I estimated the pose in my testing environment. The translation vector-output and euler angel output refers to the inverse of [R|t]. The euler angels might not be displayed correct (they might be swaped or wrong, if we take order into account) because I compute it with the convetional (I assume refering to the airplane coordinate system) equations, using an open-cv coordinate system. (The computation happens in the class Pose which I will attach). But anyways even the translation vector (of the inverse) appeard to be wrong (in my simple test).
In one test with that Image I had a roll (which might be pitch in airplane coordinates) of 30° and a translation upwards of 50cm. That appeard to be more reasonable. So I assumed because my points are coplanar, I might get ambiguous results. So I realized an other test with a point which changed in the Z-Axis. But with this test even the projection failed. 
对于solvePnP，我尝试了所有不同的求解算法标志和不同的参数兰萨克算法.
也许你可以以某种方式帮助我找到我的错误，或者向我展示解决我最初问题的好方法。我还将附上带有许多 println 语句和调试图像的调试源代码。该代码包含我的点测量值.
提前感谢您的帮助。
Class Main.java:
Class Pose.java:
0.png
1.png
编辑 2015 年 3 月 22 日：终于我能够找到自己所犯的错误了。
所以我的方法总的来说是正确的。我在我的测试数据集中没有至少（经常）收到有效的重投影。
不幸的是，OpenCV PnP 算法：“ITERATIVE、P3P、EPNP”返回各种结果，即使使用非常不准确但接近的内在猜测，结果也只是有时正确。这P3P算法应该提供3种解决方案，但OpenCV只提供一种。 EPNP 应该会返回良好的结果，但是EPNP根据我的观察，OpenCV 返回了最差的结果。
现在的问题是，如何过滤不准确的值或确保 OpenCV 函数返回有效的值。 （也许我应该修改本机代码以获得 3 个 PnP 解决方案）。
The 此处压缩图像 (37MB)，确实显示了我当前的结果（使用迭代 PnP 求解器），具有零旋转和向上 75 厘米的内在猜测。打印输出的 x 轴向前，y 轴向左，z 轴向下，以及相应的横滚角、俯仰角和偏航角。
我在尝试实现头部跟踪系统时学到的一件事是，您应该从简单的问题开始，而不是转向更复杂的问题。你的问题很长，不幸的是我没有时间分析它并搜索代码中的错误或逻辑错误，所以至少我会尝试给你一些提示和工作示例。
Here是用于查找对象平移和旋转的 OpenCV 教程。是Python写的，如果有问题的话here我旧的 C++ 项目的一部分。
我的项目使用solvePnP或solvePnPRansac函数执行相同的任务（您可以更改模式）。请注意，我的代码是一些旧的“游乐场”项目的一部分，因此即使在我执行的清理之后，它仍然非常混乱。当你运行它时，向相机显示打印的棋盘，按“p”开始位置和旋转估计，按“m”更改模式（0-ransac、1-pnp、2-posit 这似乎不起作用......）或“d”使用畸变系数打开/关闭。
这两个项目都依赖于查找棋盘模式，但应该很容易修改它们以使用其他一些对象。
相机校准 - 虽然我一直在研究我的头部跟踪系统，但我从未成功地校准过相机两次并获得相同的结果......所以我决定使用一些我在 github 上找到的校准文件，它运行良好 -here您可以通过此文件的链接找到更多有关该信息的信息。
尝试从尽可能简单的解决方案开始，在某些（甚至简单）情况下给出良好的结果。我认为一个好的开始点是将测试环境中的一张纸替换为教程中的打印棋盘（this one）并使其正常工作。从这个问题转向你的问题比从你的问题开始要容易得多。尝试用任何编程语言制定任何工作解决方案 - 考虑使用 OpenCV 的 Python 或 C++ 版本 - 比 Java 版本有更多的教程/示例，并且将代码的结果与某些工作代码的结果进行比较会让事情变得更容易。当你会有一些工作解决方案尝试修改它以适应您的测试环境。有很多事情可能会导致它现在无法工作 - 没有足够的点、代码中甚至 OpenCV Java 包装器中的错误、结果的错误解释等等......
使用您的代码中的点，我设法得到以下结果：
rvec = [[-158.56293283]，[1.46777938]，[-17.32569125]]
tvec = [[-36.23910413]，[-82.83704819]，[266.03157578]]
不幸的是，对我来说，很难说结果是否好……唯一可能的是wrong对我来说，有两个角度不同于 0（或 180）。但是如果你改变最后一行points2d from (355,37), (353,72), (353,101) to
(355,37), (355,72),(355,101)
（我猜这是你的错误，不是正确的结果）你会得到：
rvec = [[-159.34101842]，[1.04951033]，[-11.43731376]]
tvec = [[-25.74308282],[-82.58461674],[268.12321097]]
这可能更接近正确的结果。更改相机矩阵会极大地改变结果，因此请考虑测试以下值这个帖子.
请注意，所有 rvec 值都乘以180.0/3.14- 在 c++ 和 python 中，solvePnPRansac 返回的 rvec 向量包含以弧度表示的角度。