从图像点计算x,y坐标(3D)

Question

从图像点计算x,y坐标(3D)

Ban*_*ana 54 opencv camera-calibration pose-estimation

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

首先,我已完成相机校准以获取内部参数,之后我使用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);

Run Code Online (Sandbox Code Playgroud)

拥有所有矩阵之后,这个可以帮助我转换图像的方程指向wolrd坐标:

transform_equation

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

解决这个问题我可以找到变量"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;

Run Code Online (Sandbox Code Playgroud)

在此之后,我得到了结果:P = [-2629.5,1272.6,285.]

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

错误是非常小的非常好,但当我把这个盒子移动到这个房间的边缘时,错误可能是20-40毫米,我想改善它.任何人都可以帮助我,你有什么建议吗？