从OpenCV中的SolvPnP函数获取正确的rvec和tvec用于相机姿态估计

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

• Mark world 3-D（假设z = 0，因为它是平的）指向平面上正方形的角上并假设一个世界坐标系 . （以厘米为单位）

将广场的左上角作为我的原点并按以下顺序（x，y）或（col，row）给出世界点：（0,0），（ - 12.8,0），（ - 12.8， 12.8），（0,12.8） - 在cms

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

• 我已经为 intrinsic matrix 和 distortion coefficients 校准了我的相机 .

• 我使用 SolvePnP 函数获取rvec和tvec .

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

• 为了检查rvec和tvec是否正确，我使用 ProjectPoints 将三维点（z = 0）投影到图像平面中，我在图像上正确得到了点，X轴上的误差为3像素 .

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

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

• 但我在cms中的 cam_worl_pos x，y和z似乎不正确 .

我的疑问是，如果我能够使用rvec和tvec（ 3 pixel error on X-axis and almost no error on Y axis, hope it is not too bad ）将三维世界点投射回图像平面，那么为什么我没有在世界框架中获得相机位置 .

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

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

EDITS ：

Image Size - 720（行）* 1280（col）

Calibration pattern seen by camera

The link has the picture of calibration pattern used.

New Edits

World coordinate system following Right Hand Rule and the corresponding points detected in the image

This link has the picture of the world coordinate coordinate system and also the corresponding points detected in the image plane

左边的方格是我的世界坐标系，边长为12.8cm，左上角是世界原点（0,0） . 红点是图像中检测到的三维世界点 .

所见图像是在鱼眼镜头相机的径向畸变校正之后 .

camera parameters

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++ code ：

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 =18厘米

Z =25厘米

//获得的位置

X = -110cm

Y =71厘米

Z =40厘米

提前致谢 .