У меня есть задача найти объект в трехмерной системе координат. Поскольку мне нужно получить почти точные координаты X и Y, я решил отследить один цветной маркер с известной координатой Z, который будет помещен поверх движущегося объекта, как оранжевый шар на этом рисунке:

Сначала я выполнил калибровку камеры, чтобы получить внутренние параметры, и после этого я использовал cv :: solvePnP, чтобы получить вектор поворота и перемещения, как в следующем коде:

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

Имея все матрицы, это уравнение, которое может помочь мне с преобразованием точки изображения в координаты Уолрда:

где M - cameraMatrix, R - вращение Matrix, t - tvec, а s - неизвестно. Zconst представляет высоту, где находится оранжевый шар, в этом примере это 285 мм. Итак, сначала мне нужно решить предыдущее уравнение, чтобы получить «s», и после того, как я смогу определить координаты X и Y, выбрав точку изображения:

Решая это, я могу найти переменную "s", используя последнюю строку в матрицах, потому что Zconst известен, поэтому вот следующий код для этого:

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;

После этого я получил результат: P = [-2629,5, 1272,6, 285.]

и когда я сравниваю это с измерением, а именно: Preal = [-2629.6, 1269.5, 285.]

ошибка очень мала, что очень хорошо, но когда я перемещаю эту коробку к краям этой комнаты, ошибки могут быть 20-40 мм, и я хотел бы улучшить это. Может ли кто-нибудь помочь мне с этим, у вас есть какие-либо предложения?