Berechnung der x, y-Koordinate (3D) vom Bildpunkt

Ich habe die Aufgabe, ein Objekt im 3D-Koordinatensystem zu lokalisieren. Da ich fast exakte X- und Y-Koordinaten erhalten muss, habe ich beschlossen, eine Farbmarkierung mit bekannter Z-Koordinate zu verfolgen, die wie die orangefarbene Kugel in diesem Bild oben auf dem sich bewegenden Objekt platziert wird:

Zuerst habe ich die Kamerakalibrierung durchgeführt, um intrinsische Parameter zu erhalten, und danach habe ich cv :: SolvePnP verwendet, um den Rotations- und Translationsvektor wie in diesem folgenden Code zu erhalten:

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

Nachdem ich alle Matrizen habe, kann diese Gleichung mir bei der Transformation des Bildpunkts in Wolrd-Koordinaten helfen:

Dabei ist M cameraMatrix, R-rotationMatrix, t-tvec und s unbekannt. Zconst stellt die Höhe dar, in der sich die orangefarbene Kugel befindet. In diesem Beispiel beträgt sie 285 mm. Also muss ich zuerst die vorherige Gleichung lösen, um "s" zu erhalten, und nachdem ich die X- und Y-Koordinate durch Auswahl des Bildpunkts herausfinden kann:

Wenn ich das löse, kann ich die Variable "s" anhand der letzten Zeile in Matrizen herausfinden, da Zconst bekannt ist. Hier ist der folgende Code dafür:

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;

Danach erhielt ich das Ergebnis: P = [-2629,5, 1272,6, 285.]

und wenn ich es mit dem Messen vergleiche, ist das: Preal = [-2629.6, 1269.5, 285.]

Der Fehler ist sehr klein, was sehr gut ist, aber wenn ich diese Box an die Ränder dieses Raums schiebe, sind die Fehler vielleicht 20-40 mm und ich möchte das verbessern. Kann mir jemand dabei helfen, haben Sie Vorschläge?

In Anbetracht Ihrer Konfiguration sind Fehler von 20 bis 40 mm an den Kanten durchschnittlich. Es sieht so aus, als hättest du alles gut gemacht.

Ohne eine Änderung der Kamera- / Systemkonfiguration ist es schwierig, bessere Ergebnisse zu erzielen. Sie können versuchen, die Kamerakalibrierung zu wiederholen und auf bessere Ergebnisse zu hoffen, dies wird sie jedoch nicht wesentlich verbessern (und Sie erhalten möglicherweise schlechtere Ergebnisse, löschen Sie also nicht die tatsächlichen Instrumentenparameter).

Wie von count0 gesagt, sollten Sie mehrere Messungen durchführen, wenn Sie mehr Präzision benötigen.

Erhalten Sie die grünen Punkte (imagePoints) aus dem verzerrten oder unverzerrten Bild? Weil die Funktion lösenPnP die imagePoints bereits unverzerrt hat (es sei denn, Sie übergeben die Verzerrungskoeffizienten nicht oder übergeben sie als Null). Möglicherweise verzerren Sie diese imagePoints zweimal, wenn Sie sie aus dem unverzerrten Bild abrufen. Dies würde zu einem erhöhten Fehler in den Ecken führen.

https://github.com/Itseez/opencv/blob/master/modules/calib3d/src/solvepnp.cpp