У меня есть задача найти объект в 3D-системе координат. Поскольку мне нужно получить почти точную координату 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);
После наличия всех матриц это уравнение, которое может помочь мне преобразовать точку изображения в wolrd координаты:
где M является CameraMatrix, R - rotationMatrix, t - tvec и s неизвестно. Zconst представляет собой высоту, где находится оранжевый шар, в этом примере это 285 мм.
Итак, сначала мне нужно решить предыдущее уравнение, получить "s", и после того, как я смогу найти координаты X и Y, выбрав точку изображения:
Решая это, я могу узнать переменную "s", используя последнюю строку в матрицах, потому что Zconst известен, так что вот для этого следующий код:
cv::Mat uvPoint = cv::Mat::ones(3,1,cv::DataType<double>::type); //u,v,1
uvPoint.at<double>(0,0) = 363.; //got this point using mouse callback
uvPoint.at<double>(1,0) = 222.;
cv::Mat tempMat, tempMat2;
double s;
tempMat = rotationMatrix.inv() * cameraMatrix.inv() * uvPoint;
tempMat2 = rotationMatrix.inv() * tvec;
s = 285 + tempMat2.at<double>(2,0); //285 represents the height Zconst
s /= tempMat.at<double>(2,0);
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 мм, и я хотел бы улучшить это. Может ли кто-нибудь помочь мне в этом, есть ли у вас какие-либо предложения?
