Conversione di punti 2D in posizioni 3D

Ho una fotocamera fissa con noto cameraMatrixe distCoeffs. Ho anche una scacchiera anch'essa riparata transforme anche i rotationvettori vengono calcolati usando solvePnP.

Mi chiedo come sia possibile ottenere la posizione 3D di un punto 2D sullo stesso piano in cui si trova la scacchiera, come nella figura seguente:

Una cosa è certa che la Z di quel punto è 0, ma come ottenere X e Y di quel punto.

Puoi risolverlo con 3 semplici passaggi:

Passo 1:

Calcola il vettore di direzione 3d, espresso nel riquadro delle coordinate della videocamera, del raggio corrispondente al punto dell'immagine 2d indicato invertendo il modello di proiezione della videocamera:

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)

Passo 2:

Calcola la direzione 3d del vettore di questo raggio nella cornice di coordinate attaccata alla scacchiera, usando la posa relativa tra la telecamera e la scacchiera:

// 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;

Passaggio 3:

Trova il punto 3d desiderato calcolando l'intersezione tra il raggio 3d e il piano della scacchiera 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;

Poiché il tuo caso è limitato alle pianure, il modo più semplice è usare l'omografia.

Prima di tutto, distorce la tua immagine. Quindi utilizza findHomography per calcolare la matrice Homography che trasforma la tua coordinata pixel (immagine) in coordinata reale (spazio euclideo, ad esempio in cm). Qualcosa di simile a questo:

#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;