Computer vision 基于opencvc++；，解算PNP函数

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

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

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

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

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

我使用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（列）

摄像机参数

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=-40cm

---

欢迎来到机器人学，尼廷。目前，这是一个关于OpenCV的问题，而不是关于机器人系统的问题。我看到你开始了一个相同的问题，我想你是因为在这里没有得到回应而感到沮丧。因为这个问题是关于你遇到的软件问题，而且因为你在这里没有很快得到答案，我将把这个问题迁移到。@Chuck谢谢你Chuck。希望我能找到一些解决办法。