你好，我的目标是开发头部追踪功能，应用于飞机（模拟器）驾驶舱，以便在恶劣的视觉环境下为民用飞行员提供增强现实支持。
我的方法是检测特征点（在黑暗的模拟器LED中），我知道这些点的3D坐标，然后计算估计的（佩戴式摄像头的）姿态[R | t]（旋转拼接平移）。
我遇到的问题是估计的姿态似乎总是错误的，而我的3D点的投影（我也用来估计姿态）与2D图像点不重叠（或不可见）。
如何估计在给定的二维和三维点对应的情况下相机的姿态。
我尝试时为什么不起作用，可能会有哪些错误来源？
为了在实际环境中使理论解决方案起作用，测量（3D 和 2D 点以及相机矩阵）必须有多精确？
在理论上，这种方法是否适用于共面点（x，y 轴改变）？
我使用的硬件是爱普生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的结果用作投影（Calib3d.projectPoints）的输入。连接结果[R|t]的逆被我用作估计的姿态。
因为我的生产环境的结果太差了，所以我创建了一个测试环境。在这个环境中，我将相机（由于其3D形状（它是玻璃）略微向下旋转）放置在桌子边缘。我使用这个边缘作为世界坐标系的纵坐标。我搜索了open-cv坐标系可能定向的方式，并找到了不同的答案（一个在stackoverflow上，一个在关于opencv的官方youtube讲座上）。无论如何，我通过在图像上投影描述在该坐标系中的3D点并检查给定的世界形状是否保持不变来测试是否正确获取了坐标系。所以我想出了z指向前方，y向下，x向右的方向。
在一次测试中，我拍摄了这张图片，我有一个30°的横滚角（可能是飞机坐标系中的俯仰角）和一个向上50厘米的平移。这似乎更合理。所以我认为因为我的点是共面的，我可能会得到模棱两可的结果。所以我意识到需要进行另一个测试，该测试中点在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算法应该提供3个解决方案，但OpenCV只提供一个。EPNP应该返回良好的结果，但使用EPNP，OpenCV返回最差的结果，从我的人类观察中评估出来。现在的问题是如何过滤不准确的值或确保OpenCV函数返回有效值。（也许我应该修改本地代码以接收PnP的3个解决方案）。
这里的压缩图像（37MB）展示了我的当前结果（使用迭代PnP-Solver），初始猜测是零旋转和向上75厘米。打印出来的图像中，x轴向前，y轴向左，z轴向下，并对应滚动、俯仰和偏航角度。