OpenCV trigonometry: find object closest to the camera

Question

I think it is basic trigonometric task, but I not so good in trigonometry. I have calibrated camera so I know camera matrix and distortion coefficients. I also able to detect aruco board (using opencv's aruco contrib module) and its position so I have an rotation vector and translation vectors of the board (in camera space?). So I also able to define this yellow circles around board center using coordinates relative to board and draw them using functions cv::projectPoints(yellowMarkerPoints, rvec, tvec, camMatrix, distCoeffs, imagePoints); and cv::circle. Now I need calculate circle closest to the camera. I dont understand how to do this!

So what I have:

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

Or similar question: how from this data I can get an angle of the board (aka 'Yaw' rotation angle) relative to the camera?

Catree Catree · Accepted Answer · 2018-02-18T14:07:31

From the solvePnP() doc:

the figure represents the pose estimation problem where you want to estimate the rotation and the translation that allow to transform coordinates expressed in the world frame into the camera frame.

What you need is transform every yellowMarkerPoints expressed in the Aruco board frame into the camera frame and compute the distance to the camera frame. Something like:

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

Which is equivalent to:

OpenCV trigonometry: find object closest to the camera

1 Answers