Calcul des coordonnées x,y (3D) à partir du point d'image

j'ai une tâche pour localiser un objet dans le système de coordonnées 3D. Puisque je dois obtenir des coordonnées X et Y presque exactes, j'ai décidé de suivre un marqueur de couleur avec des coordonnées Z connues qui seront placées sur le dessus de l'objet en mouvement, comme la boule orange dans cette image:

<!-D'abord, j'ai fait le calibrage de la caméra pour obtenir des paramètres intrinsèques et ensuite j'ai utilisé cv:: solvePnP pour obtenir la rotation et le vecteur de traduction comme dans ce qui suit code:

std::vector<cv::Point2f> imagePoints;
std::vector<cv::Point3f> 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<double>::type);
cv::Mat tvec(1,3,cv::DataType<double>::type);
cv::Mat rotationMatrix(3,3,cv::DataType<double>::type);

cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
cv::Rodrigues(rvec,rotationMatrix);

après avoir eu toutes les matrices, cette équation qui peut m'aider à transformer le point d'image en coordonnées wolrd:

<!-Où M est cameraMatrix, r-rotationMatrix, t-tvec, et s est un inconnu. Zconst représente la hauteur où se trouve la boule orange, dans cet exemple elle est de 285 mm. Donc, d'abord je dois résoudre l'équation précédente, pour obtenir "s", et après je peux trouver X et y coordonnée en sélectionnant l'image point:

résoudre ceci je peux trouver la variable "s", en utilisant la dernière ligne dans matrices, parce que Zconst est connu, donc voici le code suivant pour cela:

cv::Mat uvPoint = (cv::Mat_<double>(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<double>(2,0))/leftSideMat.at<double>(2,0)); 
//285 represents the height Zconst

std::cout << "P = " << rotationMatrix.inv() * (s * cameraMatrix.inv() * uvPoint - tvec) << std::endl;

après cela, j'ai obtenu le résultat: P = [-2629.5, 1272.6, 285.]

et quand je le compare à la mesure, qui est: Preal = [-2629.6, 1269.5, 285.]

l'erreur est très petite, ce qui est très bien, mais lorsque je déplace cette boîte aux bords de cette salle, les erreurs sont peut-être 20-40mm et Je voudrais l'améliorer. Quelqu'un peut-il m'aider, avez-vous des suggestions?