Calcola x, y coordinate (3D) dal punto immagine

Ho un compito per localizzare un object nel sistema di coordinate 3D. Dato che devo ottenere coordinate X e Y quasi esatte, ho deciso di tracciare un marker di colore con una coordinata Z nota che verrà posizionata sulla parte superiore dell’object in movimento, come la pallina arancione in questa immagine:

Innanzitutto, ho eseguito la calibrazione della telecamera per ottenere parametri intrinseci e successivamente ho usato cv :: solvePnP per ottenere il vettore di rotazione e traduzione come nel seguente codice:

• Posizione della telecamera in coordinate globali da cv :: solvePnP

std::vector imagePoints; std::vector objectPoints; //img points are green dots in the picture imagePoints.push_back(cv::Point2f(271.,109.)); imagePoints.push_back(cv::Point2f(65.,208.)); imagePoints.push_back(cv::Point2f(334.,459.)); imagePoints.push_back(cv::Point2f(600.,225.)); //object points are measured in millimeters because calibration is done in mm also objectPoints.push_back(cv::Point3f(0., 0., 0.)); objectPoints.push_back(cv::Point3f(-511.,2181.,0.)); objectPoints.push_back(cv::Point3f(-3574.,2354.,0.)); objectPoints.push_back(cv::Point3f(-3400.,0.,0.)); cv::Mat rvec(1,3,cv::DataType::type); cv::Mat tvec(1,3,cv::DataType::type); cv::Mat rotationMatrix(3,3,cv::DataType::type); cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec); cv::Rodrigues(rvec,rotationMatrix); 

Dopo avere tutte le matrici, questa equazione che può aiutarmi a trasformare il punto dell’immagine in coordinate wolrd:

dove M è cameraMatrix, R – rotationMatrix, t – tvec e s è uno sconosciuto. Zconst rappresenta l’altezza in cui si trova la palla arancione, in questo esempio è 285 mm. Quindi, prima devo risolvere l’equazione precedente, per ottenere “s”, e dopo aver trovato le coordinate X e Y selezionando il punto dell’immagine:

Risolvendo questo posso trovare la variabile “s”, usando l’ultima riga in matrici, perché Zconst è noto, quindi ecco il seguente codice per questo:

 cv::Mat uvPoint = (cv::Mat_(3,1) << 363, 222, 1); // u = 363, v = 222, got this point using mouse callback cv::Mat leftSideMat = rotationMatrix.inv() * cameraMatrix.inv() * uvPoint; cv::Mat rightSideMat = rotationMatrix.inv() * tvec; double s = (285 + rightSideMat.at(2,0))/leftSideMat.at(2,0)); //285 represents the height Zconst std::cout << "P = " << rotationMatrix.inv() * (s * cameraMatrix.inv() * uvPoint - tvec) << std::endl; 

Dopo questo, ho ottenuto il risultato: P = [-2629,5, 1272,6, 285.]

e quando lo paragono alla misurazione, che è: Preal = [-2629.6, 1269.5, 285.]

l’errore è molto piccolo, il che è molto buono, ma quando sposto questa scatola sui bordi di questa stanza, gli errori sono forse di 20-40mm e vorrei migliorarlo. Qualcuno può aiutarmi, hai qualche suggerimento?