Obliczanie współrzędnych x, y (3D) z punktu obrazu
Mam zadanie zlokalizowania obiektu w układzie współrzędnych 3D. Ponieważ muszę uzyskać prawie dokładną współrzędną X i Y, postanowiłem śledzić jeden znacznik koloru o znanej współrzędnej Z, który zostanie umieszczony na górze poruszającego się obiektu, jak pomarańczowa piłka na tym obrazku:
Po pierwsze, wykonałem kalibrację kamery, aby uzyskać parametry wewnętrzne, a następnie użyłem cv :: resolvePnP, aby uzyskać wektor obrotu i translacji, jak w poniższym kodzie:
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);
Po posiadaniu wszystkich macierzy to równanie, które może pomóc mi w przekształceniu obrazu, wskazuje na współrzędne wilka:
gdzie M to cameraMatrix, R - rotationMatrix, t - tvec, a s jest nieznane. Zconst reprezentuje wysokość, na której znajduje się pomarańczowa kula, w tym przykładzie wynosi 285 mm. Więc najpierw muszę rozwiązać poprzednie równanie, aby uzyskać „s”, a po tym, jak mogę znaleźć współrzędne X i Y, wybierając punkt obrazu:
Rozwiązując to, mogę znaleźć zmienną „s”, używając ostatniego wiersza w macierzach, ponieważ Zconst jest znany, więc oto poniższy kod:
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;
Po tym otrzymałem wynik: P = [-2629.5, 1272,6, 285.]
a kiedy porównuję to do pomiaru, który jest: Preal = [-2629.6, 1269,5, 285.]
błąd jest bardzo mały, co jest bardzo dobre, ale kiedy przesunę to pudełko na krawędzie tego pomieszczenia, błędy mogą wynosić 20-40 mm i chciałbym to poprawić. Czy ktoś może mi w tym pomóc, czy masz jakieś sugestie?