计算的x，y坐标（3D）图像从点(Computing x,y coordinate (3D) fro

我有一个任务来定位三维物体的坐标系。因为我得几乎精确的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);

让所有的矩阵后，这个等式，可以帮助我与转化的图像点坐标世界国际：

其中M是cameraMatrix，R - rotationMatrix，叔 - 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已经undistort的imagePoints（除非你不及格的畸变系数，或者将它们传递为空）。你可能会undistorting那些imagePoints两次，如果你是从失真的图像让他们，而这最终会在弯道引起增加错误。

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