OpenCV三角函数: find object closest to the camera

Question

我认为这是基本的三角学任务，但我的三角学不太好。我已经校准了相机，所以我知道相机矩阵和失真系数。我还能够检测 aruco 板(使用 opencv 的 aruco contrib 模块)及其位置，因此我有板的旋转矢量和平移矢量(在相机空间中？)。因此，我还可以使用相对于棋盘的坐标定义围绕棋盘中心的黄色圆圈，并使用函数 cv::projectPoints 绘制它们(黄色标记点、rvec、tvec、camMatrix、distCoeffs、imagePoints)；和 cv::circle。现在我需要计算离相机最近的圆。我不明白该怎么做!

所以我有什么:

Mat camMatrix, distCoeffs;
Vec3d rvec, tvec;//board pose
vector< Point3f > yellowMarkerPoints;//yellow circles positions

或类似的问题:如何从这些数据中获得板相对于相机的角度(又名“偏航”旋转角度)？

Answer 1

来自 solvePnP() 文档:



该图表示姿势估计问题，您要在其中估计允许将世界框架中表示的坐标转换为相机框架的旋转和平移。

您需要的是每 yellowMarkerPoints 转换一次在 Aruco 板框架中表示到相机框架中并计算到相机框架的距离。就像是:

cv::Mat R;
cv::Rodrigues(rvec, R);
cv::Mat cTw(4,4,CV_64F);
for (int i = 0; i < 3; i++) {
  for (int j = 0; j < 3; j++) {
    cTw.at<double>(i,j) = R.at<double>(i,j);
  }
  cTw.at<double>(i,3) = tvec.at<double>(i);
}

for (size_t i = 0; i < yellowMarkerPoints.size(); i++) {
  cv::Mat pts3D = (cv::Mat_<double>(4,1) << yellowMarkerPoints[i].x, yellowMarkerPoints[i].y, yellowMarkerPoints[i].z, 1);
  cv::Mat pts3D_cam = cTw * pts3D ;
  double dist_to_cam = sqrt( pts3D_cam.at<double>(0)*pts3D_cam.at<double>(0) + pts3D_cam.at<double>(1)*pts3D_cam.at<double>(1) + pts3D_cam.at<double>(2)*pts3D_cam.at<double>(2) );
}

这相当于: