Convertir punto 2D a ubicación 3D

Tengo una cámara fija con conocidos cameraMatrixy distCoeffs. También tengo un tablero de ajedrez que también está arreglado, transformy los rotationvectores también se calculan usando solvePnP.

Me pregunto cómo es posible obtener la ubicación 3D de un punto 2D en el mismo plano en el que se encuentra el tablero de ajedrez, como en la imagen a continuación:

Una cosa segura es que la Z de ese punto es 0, pero cómo obtener X e Y de ese punto.

Puede resolver esto con 3 simples pasos:

Paso 1:

Calcule el vector de dirección 3D, expresado en el marco de coordenadas de la cámara, del rayo correspondiente al punto de imagen 2D dado invirtiendo el modelo de proyección de la cámara:

std::vector<cv::Point2f> imgPt = {{u,v}}; // Input image point
std::vector<cv::Point2f> normPt;
cv::undistortPoints     (imgPt, normPt, cameraMatrix, distCoeffs);
cv::Matx31f ray_dir_cam(normPt[0].x, normPt[0].y, 1);
// 'ray_dir_cam' is the 3d direction of the ray in camera coordinate frame
// In camera coordinate frame, this ray originates from the camera center at (0,0,0)

Paso 2:

Calcule la dirección 3D del vector de este rayo en el marco de coordenadas adjunto al tablero de ajedrez, usando la pose relativa entre la cámara y el tablero de ajedrez:

// solvePnP typically gives you 'rvec_cam_chessboard' and 'tvec_cam_chessboard'
// Inverse this pose to get the pose mapping camera coordinates to chessboard coordinates
cv::Matx33f R_cam_chessboard;
cv::Rodrigues(rvec_cam_chessboard, R_cam_chessboard);
cv::Matx33f R_chessboard_cam = R_cam_chessboard.t();
cv::Matx31f t_cam_chessboard = tvec_cam_chessboard;
cv::Matx31f pos_cam_wrt_chessboard = -R_chessboard_cam*t_cam_chessboard;
// Map the ray direction vector from camera coordinates to chessboard coordinates
cv::Matx31f ray_dir_chessboard = R_chessboard_cam * ray_dir_cam;

Paso 3:

Encuentre el punto 3d deseado calculando la intersección entre el rayo 3d y el plano del tablero de ajedrez con Z = 0:

// Expressed in the coordinate frame of the chessboard, the ray originates from the
// 3d position of the camera center, i.e. 'pos_cam_wrt_chessboard', and its 3d
// direction vector is 'ray_dir_chessboard'
// Any point on this ray can be expressed parametrically using its depth 'd':
// P(d) = pos_cam_wrt_chessboard + d * ray_dir_chessboard
// To find the intersection between the ray and the plane of the chessboard, we
// compute the depth 'd' for which the Z coordinate of P(d) is equal to zero
float d_intersection = -pos_cam_wrt_chessboard.val[2]/ray_dir_chessboard.val[2];
cv::Matx31f intersection_point = pos_cam_wrt_chessboard + d_intersection * ray_dir_chessboard;

Dado que su caso se limita a las llanuras, la forma más sencilla es utilizar la homografía.

Primero distorsiona tu imagen. Luego use findHomography para calcular la matriz de Homography que transforma su coordenada de píxeles (imagen) en coordenada real (espacio euclidiano, por ejemplo, en cm). Algo similar a este:

#include <opencv2/calib3d.hpp>
//...

//points on undistorted image (in pixel). more is better
vector<Point2f>  src_points = { Point2f(123,321), Point2f(456,654), Point2f(789,987), Point2f(123,321) };
//points on chessboard (e.g. in cm)
vector<Point2f>  dst_points = { Point2f(0, 0), Point2f(12.5, 0), Point2f(0, 16.5), Point2f(12.5, 16.5) }; 
Mat H = findHomography(src_points, dst_points, RANSAC);

//print euclidean coordinate of new point on undistorted image (in pixel)
cout << H * Mat(Point3d(125, 521, 0)) << endl;