c++ - OpenCV solvePnP 获取相对于相机的图案原点位置

Question

我目前正在尝试通过获取两个摄像头都可以看到的跟踪模式的位置来获取两个 Kinect v2 的相对位置。不幸的是，我似乎无法获得图案原点的正确位置。

这是我当前获取图案相对于相机位置的代码:

std::vector<cv::Point2f> centers;
cv::findCirclesGrid( registeredColor, m_patternSize, centers, cv::CALIB_CB_ASYMMETRIC_GRID );
cv::solvePnPRansac( m_corners, centers, m_camMat, m_distCoeffs, m_rvec, m_tvec, true );

// calculate the rotation matrix 
cv::Matx33d rotMat;
cv::Rodrigues( m_rvec, rotMat );

// and put it in the 4x4 transformation matrix
transformMat = matx3ToMatx4(rotMat);

for( int i = 0; i < 3; ++i )
    transformMat(i,3) = m_tvec.at<double>(i);

transformMat = transformMat.inv();

cv::Vec3f originPosition( transformMat(0,3), transformMat(1,3), transformMat(2,3) );

不幸的是，当我将 originPosition 与点云中对应于在屏幕空间中找到的原点(保存在上面的 centers.at(0) 中)的点进行比较时，我得到一个非常不同的结果。下面的屏幕截图显示了来自 kinect 的点云，红色圆圈中图案原点的屏幕空间位置处的点和浅蓝色圆圈中浅蓝色的 originPosition 点。屏幕截图取自图案的正前方。 originPosition 也更靠前一点。

如您所见，红点完全位于图案的第一个圆圈中，而对应于 originPosition 的蓝点甚至不近。特别是它绝对不仅仅是从相机到原点的 vector 缩放问题。此外，findCirclesGrid 是在注册的彩色图像上完成的，内部参数是从相机本身获取的，以确保图像与点云计算之间没有差异。

Answer 1

你有 R|T 给定的变换 P->P'，要得到 R'|T' 给定的逆变换 P'->P，只需执行以下操作:

R' = R.t();
T' = -R'* T;

然后

P = R' * P' + T'