我有一项任务是在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);
在拥有所有矩阵之后，该方程式可以帮助我将图像点转换为世界坐标：
其中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之间，我想对此进行改进。谁能帮助我，您有什么建议吗？