小编kwm*_*men的帖子

我有一个任务是在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 …