Cálculo de la coordenada x, y (3D) desde el punto de la imagen
Tengo una tarea para localizar un objeto en el sistema de coordenadas 3D. Como tengo que obtener las coordenadas X e Y casi exactas, decidí rastrear un marcador de color con una coordenada Z conocida que se colocará en la parte superior del objeto en movimiento, como la bola naranja en esta imagen:
Primero, realicé la calibración de la cámara para obtener parámetros intrínsecos y, después, utilicé cv :: solvePnP para obtener la rotación y el vector de traducción como en este código siguiente:
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);
Después de tener todas las matrices, esta ecuación que puede ayudarme a transformar el punto de la imagen a las coordenadas del mundo:
donde M es cameraMatrix, R - rotationMatrix, t - tvec, y s es una incógnita. Zconst representa la altura donde está la bola naranja, en este ejemplo es 285 mm. Entonces, primero necesito resolver la ecuación anterior, obtener "s", y luego puedo averiguar las coordenadas X e Y seleccionando el punto de la imagen:
Resolviendo esto, puedo encontrar la variable "s", usando la última fila en matrices, porque Zconst es conocido, así que aquí está el siguiente código para eso:
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;
Después de esto, obtuve el resultado: P = [-2629.5, 1272.6, 285.]
y cuando lo comparo con la medición, que es: Preal = [-2629.6, 1269.5, 285.]
el error es muy pequeño, lo que es muy bueno, pero cuando muevo esta caja a los bordes de esta sala, los errores son quizás de 20 a 40 mm y me gustaría mejorar eso. ¿Alguien puede ayudarme con eso? ¿Tiene alguna sugerencia?