OpenCV三角学：找到离相机最近的物体

Question

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

所以我有：

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

或类似的问题：我如何从这些数据中获得板相对于相机的角度（又名 'Yaw' 旋转角度）？

Answer 1

来自 solvePnP() 文档：

该图表示姿态估计问题，您想要估计旋转和平移，以允许将世界坐标系中表示的坐标转换为相机坐标系。

您需要将 Aruco 板框架中表示的每个 yellowMarkerPoints 转换为相机框架，并计算到相机框架的距离。类似于：

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) );
}

相当于：

OpenCV三角学：找到离相机最近的物体

OpenCV trigonometry: find object closest to the camera

3d

opencv

trigonometry

aruco