OpenCV三角法：查找离摄影机最近的对象

我认为这是一项基本的三角函数任务，但我的三角函数不是很好。我已经校准了摄像机，所以我知道摄像机矩阵和失真系数。我还能够检测aruco板（使用opencv的aruco contrib模块）及其位置，因此我有板的旋转向量和平移向量（在相机空间？）。因此，我也能够使用相对于电路板的坐标定义电路板中心周围的黄色圆圈，并使用函数（yellowMarkerPoints、rvec、tvec、camMatrix、Discoefs、imagePoints）绘制它们；和cv：：圆。现在我需要计算离摄像机最近的圆。我不知道该怎么做

所以我有：

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

Mat-camMatrix，distcoefs；
Vec3d-rvec，tvec//棋盘姿势
向量黄色标记点//黄色圆圈位置

或者类似的问题：如何从这些数据中获得板相对于相机的角度（也称为“偏航”旋转角度）？

来自文档：

该图表示要估计旋转和平移的姿势估计问题，该旋转和平移允许将世界帧中表示的坐标转换为摄影机帧

您需要的是将Aruco board框架中表示的每个

黄色标记点

转换为相机框架，并计算到相机框架的距离。比如：

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

cv:：matr；
cv：：罗德里格斯（rvec，R）；
cv:：Mat cTw（4,4，cv_64F）；
对于（int i=0；i<3；i++）{
对于（int j=0；j<3；j++）{
cTw.at（i，j）=R.at（i，j）；
}
在（i，3）处的cTw＝在（i）处的tvec；
}
对于（size_t i=0；i

该图表示要估计旋转和平移的姿势估计问题，该旋转和平移允许将世界帧中表示的坐标转换为摄影机帧

您需要的是将Aruco board框架中表示的每个
黄色标记点
转换为相机框架，并计算到相机框架的距离。类似于：

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

cv:：matr；
cv：：罗德里格斯（rvec，R）；
cv:：Mat cTw（4,4，cv_64F）；
对于（int i=0；i<3；i++）{
对于（int j=0；j<3；j++）{
cTw.at（i，j）=R.at（i，j）；
}
在（i，3）处的cTw＝在（i）处的tvec；
}
对于（size_t i=0；icv:：Mat pts3D=（cv:：Mat_389;（4,1）非常有效！非常感谢您的详细解释和代码示例！非常有效！非常感谢您的详细解释和代码示例！