opencv - OpenCV三角法：找到离相机最近的物体

Question

我认为这是基本的三角学任务，但我的三角学不太好。我已经校准了相机，所以我知道相机矩阵和失真系数。我还能够检测 aruco 板（使用 opencv 的 aruco contrib 模块）及其位置，因此我有板的旋转矢量和平移矢量（在相机空间中？）。所以我还可以使用相对于棋盘的坐标定义围绕棋盘中心的黄色圆圈，并使用函数cv::projectPoints (yellowMarkerPoints, 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) );
}

这相当于：