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 对应的蓝点甚至不近。特别是它绝对不仅仅是从相机到原点的矢量缩放问题。另外，findCirclesGrid是在配准的彩色图像上做的，内参数取自相机本身，保证图像和点云计算没有区别。

Answer 1

你有 R|T 给出的变换 P->P'，要得到 R'|T' 给出的逆变换 P'->P 只需做：

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

然后

P = R' * P' + T'

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

OpenCV solvePnP get position of pattern origin relative to camera

c++

opencv

opencv-solvepnp