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

我试图测量相机的姿势，我做了以下工作

在平面上正方形的角上标记世界3-D（假设z=0，因为它是平的）点，并假设世界坐标系。（在cms中）

以正方形的左上角为原点，按以下顺序（x，y）或（col，row）给出世界点： cms中的（0,0），（-12.8,0），（-12.8,12.8），（0,12.8）-

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

我已经校准了相机的固有矩阵和失真系数

我使用SolvePnP函数获取rvec和tvec

我使用Rodrigues函数来获得旋转矩阵

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

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

凸轮工作位置=-反向（R）*tvec。（这个公式我在很多博客上都验证过，这也是有道理的）

但是我的cms中的x、y和z的凸轮轴位置似乎不正确

我的疑问是，如果我能够使用rvec和tvec将三维世界点投影到图像平面上（X轴上有3个像素的错误，Y轴上几乎没有错误，希望不是太糟糕），那么为什么我没有正确地获得世界帧中的相机位置

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

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

编辑：

图像大小-720（行）*1280（列）

摄像机看到的校准模式

新编辑

遵循右手法则的世界坐标系和图像中检测到的对应点

左边的正方形是我的世界坐标系，是一个12.8cm的正方形，左上角是世界原点（0,0）。红色点是在图像中检测到的三维世界点

所看到的图像是经过鱼眼镜头相机的径向畸变校正后的

摄像机参数

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;

vector front\u object\u pts；
Mat rvec_前部；
Mat tvec_前；
前垫旋转；
垫世界位置前凸轮；
//填充前对象点（x-y-z顺序，以厘米为单位）
//它是Z=0平面上12.8cm边的正方形
前向对象点向后推（点3f（0,0,0））；
前推后（点3F（-12.8,0,0））；
前物体后推（点3F（-12.8,12.8,0））；
前面的物体点向后推（点3f（0,12.8,0））；
//以与对象点相同的顺序检测到的相应图像点
前图像点向后推（点前[0]）；
前图像点向后推（点前[1]）；
前图像点向后推（点前[2]）；
前图像点向后推（点前[3]）；
//图像中检测到的点与三维点的顺序相同
//(467,368)
//(512,369)
//(456,417)
//(391,416)
//使用Solve PnP获取rvec和tvec
解算PNP（前对象点、前图像点、cameraMatrix前、，
Mat（4,1，CV_64FC1，标量（0）），rvec_锋，tvec_锋，false，CV_迭代）；
//pnp的输出
//tvec=[-26.951,0.6041134.72]（3 x 1矩阵）
//rvec=[-1.0053,0.6691,0.3752]（3 x 1矩阵）
//通过将三维对象点投影到图像，检查rvec和tvec是否正确
矢量检查\u前\u图像\u点
项目点（前对象点、rvec前、tvec前、，
cameraMatrix_Front、Discoefs_Front、check_Front_image_pts）；
//这里要注意的是，我已经做了**区分系数**，
//0矢量，因为在去除径向失真后检测到图像点
//获取旋转矩阵
罗德里格斯岛（rvec_前、旋转_前）；
//求旋转矩阵的逆
逆矩阵旋转；
转置（前旋转、后旋转）；
//获取摄像机在世界科迪纳茨的位置
世界位置前凸轮=-旋转逆*tvec前；

//摄像机的实际位置（手动测量的近似值）

X=-47厘米

Y=18cm

Z=25厘米

//获得位置

X=-110厘米

Y=71厘米

Z=40厘米

---

提前谢谢。

你怎么知道

cam\u worl\u pos

不正确

cam\u worl\u pos

是摄像头的位置。您可以尝试测量相机与三维点所在平面图之间的实际距离，并将其与

tvec

（

std:：sqrt（tx²+ty²+tz²）

）的标准值进行比较。如果不共享代码或不同参数的值，很难说出哪里出了问题

编辑

如果你计算距离tvec的距离，它会给你137.390683厘米：

std:：sqrt（26.951*26.951+0.6041*0.6041+134.72*134.72）

您可以通过测量实际距离来检查这是否正确

以及如何手动测量相机相对于世界帧的位置

EDIT2

如果没有获得正确的值，则solvePnP函数的参数有问题：

• 您的

  cameraMatrix_Front

  似乎是正确的。但要确保

  908.65*CCD（毫米）=焦距（毫米）

• 将从校准中获得的失真系数的实际值传递到solvePnP

• 您至少需要四个点来解算PNP（点越多越好），为了获得更高的精度，请不要使平面（正方形所在的位置）与图像平面平行，如：

但如此