OpenCV从2个已知的平行平面获取准确的真实世界坐标

Question

所以我一直在修改opencv，我希望能够使用相机图像来获取某些平放在平面上的对象的位置。这些对象是简单的形状，例如圆形正方形等。它们的高度相同，均为5cm。为了将现实世界的点与相机上的像素相关联，我在飞机上绘制了四个白色正方形，它们之间的距离已知。

所以我一直在采取的步骤是：

初始化：

1. 使用棋盘图像校准我的相机并保存校准数据。
2. 获取输入图像。使用相机的校准数据致电cv::undistort。
3. 找到图像中四个正方形的中心点，并将该数据和正方形的实际坐标传递给cv::solvePnP函数。保存rvec和tvec返回参数。
4. 扭曲图像的透视图，以便从图像获得自顶向下的视图。这实际上是遵循本教程的：https://docs.opencv.org/3.4.1/d9/dab/tutorial_homography.html
5. 使用生成的图像再次找到4个白色正方形，然后计算“每米像素”转换常数，该常数可以将点之间的一定像素差异与该4个正方形所在的平面上的真实世界距离相关联。

查找对象，这是在初始化之后完成的：

1. 获取输入图像。使用相机的校准数据致电cv::undistort。
2. 扭曲图像的透视图，以便从图像获得自顶向下的视图。这与初始化期间的步骤4相同。
3. 找到要检测的对象的中心点。

4. 由于对象的中心点在更高的平面上，因此我在此处进行校准，因此使用以下公式对此进行校正（d =距图像中心的像素偏移。camHeight是我通过以下方法测得的cameraHeight使用卷尺。h是物体的高度）：

   d = x - (h * (x / camHeight))
   

因此，在这里举例说明我如何获得这种配方：

但是坐标仍然不匹配...

所以我完全想知道这是否正确。具体来说，我有以下问题：

1. 使用cv::undistort之前使用cv::solvenPnP是否正确？ cv::solvePnP还将相机校准数据作为输入，因此我不确定是否必须将未失真的图像传递给它。
2. 类似于1.在查找对象期间，我调用cv::undistort-> cv::warpPerspective。这是必要的吗？
3. 我的计算是否可以纠正步骤4中的平行平面？我觉得自己缺少了一些东西，但看不到。我想知道的一件事是，一旦solvePnp完成，是否可以从opencv获取摄像机高度。

我是CV的新手，所以如果有什么其他完全错误的地方，也请向我指出。

感谢您阅读这段文字！