从图像点计算x，y坐标（3D）

Question

我有一项任务是在3D坐标系中定位一个物体。由于我必须得到几乎精确的X和Y坐标，我决定使用已知的Z坐标跟踪一个颜色标记，该坐标将放置在移动对象的顶部，如此图片中的橙色球：

首先，我已完成相机校准以获取内部参数，之后我使用cv :: solvePnP获取旋转和平移向量，如下面的代码所示：

std::vector<cv::Point2f> imagePoints;
std::vector<cv::Point3f> objectPoints;
//img points are green dots in the picture
imagePoints.push_back(cv::Point2f(271.,109.));
imagePoints.push_back(cv::Point2f(65.,208.));
imagePoints.push_back(cv::Point2f(334.,459.));
imagePoints.push_back(cv::Point2f(600.,225.));

//object points are measured in millimeters because calibration is done in mm also
objectPoints.push_back(cv::Point3f(0., 0., 0.));
objectPoints.push_back(cv::Point3f(-511.,2181.,0.));
objectPoints.push_back(cv::Point3f(-3574.,2354.,0.));
objectPoints.push_back(cv::Point3f(-3400.,0.,0.));

cv::Mat rvec(1,3,cv::DataType<double>::type);
cv::Mat tvec(1,3,cv::DataType<double>::type);
cv::Mat rotationMatrix(3,3,cv::DataType<double>::type);

cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
cv::Rodrigues(rvec,rotationMatrix);

拥有所有矩阵之后，这个方程可以帮助我将图像转换为wolrd坐标：

其中M是cameraMatrix，R - rotationMatrix，t - tvec和s是未知的。 Zconst表示橙色球的高度，在本例中为285毫米。 所以，首先我需要解决前面的方程，得到“s”，然后通过选择图像点找出X和Y坐标：

解决这个问题我可以找到变量“s”，使用矩阵的最后一行，因为Zconst是已知的，所以这里有以下代码：

cv::Mat uvPoint = (cv::Mat_<double>(3,1) << 363, 222, 1); // u = 363, v = 222, got this point using mouse callback

cv::Mat leftSideMat  = rotationMatrix.inv() * cameraMatrix.inv() * uvPoint;
cv::Mat rightSideMat = rotationMatrix.inv() * tvec;

double s = (285 + rightSideMat.at<double>(2,0))/leftSideMat.at<double>(2,0)); 
//285 represents the height Zconst

std::cout << "P = " << rotationMatrix.inv() * (s * cameraMatrix.inv() * uvPoint - tvec) << std::endl;

在此之后，我得到了结果：P = [-2629.5,1272.6,285。]

当我将它与测量值进行比较时，即：Preal = [-2629.6,1269.5,285。]

错误非常小，非常好，但是当我将此框移动到这个房间的边缘时，错误可能是20-40mm，我想改进它。任何人都可以帮助我，你有任何建议吗？

Answer 1

根据您的配置，边缘处的误差为20-40mm是平均值。看起来你做得很好。

不修改相机/系统配置，做得更好将很难。您可以尝试重做相机校准并希望获得更好的结果，但这不会改善它们（并且您最终可能会得到更糟糕的结果，因此不要删除实际的内部参数）

正如count0所说，如果你需要更高的精度，你应该进行多次测量。

Answer 2

您是否从失真或未失真的图像中获得绿点（imagePoints）？因为函数solvePnP已经不存在imagePoints（除非你没有传递失真系数，或者将它们传递为null）。如果你从未失真的图像中获取它们，你可能会对这些imagePoints两次失真，这最终会导致角落中的错误增加。

https://github.com/Itseez/opencv/blob/master/modules/calib3d/src/solvepnp.cpp