使用opencv C++、SolvePnP函数进行相机姿态估计

Question

我正在尝试测量相机的姿势，我已经完成了以下操作。

1. 标记世界 3-D（假设 z=0，因为它是平的）点在平面上正方形的角上，并假设一个世界坐标系。（以厘米为单位）

将正方形的左上角作为我的原点，并按以下顺序给出世界点（x，y）或（col，row）：（0,0），（12.8,0），（12.8,12.8） ,(0,12.8) - 厘米

2. 检测图像中的那些点。（以像素为单位）图像点和世界点的顺序相同。

3. 我已经针对内在矩阵和失真系数校准了我的相机。

4. 我使用SolvePnP函数来获取 rvec 和 tvec。

5. 我使用Rodrigues函数来获取旋转矩阵。

6. 为了检查 rvec 和 tvec 是否正确，我使用ProjectPoints将 3-D 点（z=0）投影回图像平面，并且我在图像上正确获得了点，X 轴上的误差为 3 个像素。

7. 现在我继续使用公式计算我的相机在世界框架中的位置：

cam_worl_pos = - inverse(R) * tvec。（这个公式我已经在很多博客中验证过，这也是有道理的）

8. 但是我在 cms 中的cam_worl_pos x、y 和 z 似乎不正确。

我的疑问是，如果我能够使用 rvec 和 tvec 将 3-D 世界点投影回图像平面（X 轴上有3 个像素错误，Y 轴上几乎没有错误，希望它不会太糟糕），然后为什么我没有在世界框架中获得正确的相机位置。

另外，我对 SolvPnP rvec 和 tvec 解决方案有疑问，它们可能是多种解决方案之一，但不是我想要的。

我如何从 SolvPnp 获得正确的 rvec 和 tvec 或任何其他获得 rvec 和 tvec 的建议也会有所帮助。

编辑：

图像尺寸- 720（行）* 1280（列）

相机参数

cameraMatrix_Front=[908.65   0     642.88
                     0     909.28   364.95
                     0        0        1]

distCoeffs_Front=[-0.4589, 0.09462, -1.46*10^-3, 1.23*10^-3]

OpenCV C++ 代码：

vector<Point3f> front_object_pts;
Mat rvec_front;
Mat tvec_front;
Mat rotation_front;
Mat world_position_front_cam;


//Fill front object points(x-y-z order in cms)
//It is square of side 12.8cms on Z=0 plane
front_object_pts.push_back(Point3f(0, 0, 0));
front_object_pts.push_back(Point3f(12.8, 0, 0));
front_object_pts.push_back(Point3f(12.8,12.8,0));
front_object_pts.push_back(Point3f(0, 12.8, 0));


//Corresponding Image points detected in the same order as object points
front_image_pts.push_back(points_front[0]);
front_image_pts.push_back(points_front[1]);
front_image_pts.push_back(points_front[2]);
front_image_pts.push_back(points_front[3]);

//Detected points in image matching the 3-D points in the same order
//(467,368)
//(512,369)
//(456,417)
//(391,416)

//Get rvec and tvec using Solve PnP
solvePnP(front_object_pts, front_image_pts, cameraMatrix_Front,
         Mat(4,1,CV_64FC1,Scalar(0)), rvec_front, tvec_front, false, CV_ITERATIVE);

//Output of SolvePnP
//tvec=[-26.951,0.6041,134.72]  (3 x 1 matrix)
//rvec=[-1.0053,0.6691,0.3752]  (3 x 1 matrix)


//Check rvec and tvec is correct or not by projecting the 3-D object points to image
vector<Point2f>check_front_image_pts
projectPoints(front_object_pts, rvec_front, tvec_front, 
             cameraMatrix_Front, distCoeffs_Front, check_front_image_pts);


//Here to note that I have made **distCoefficents**, 
//a 0 vector since my   image points are detected after radial distortion is removed

//Get rotation matrix
Rodrigues(rvec_front, rotation_front);

//Get rotation matrix inverse
Mat rotation_inverse;
transpose(rotation_front, rotation_inverse);

//Get camera position in world cordinates
world_position_front_cam = -rotation_inverse * tvec_front;

//相机的实际位置（手动测量）

X=47cm

Y=18cm

Z=25cm

//获取位置

X=110cm

Y=71cm

Z=-40cm