Aruco Marker mit openCv, erhalten Sie die 3D-Koordinaten?

Question

ich eine gedruckte Markierung Aruco opencv Verwendung Detektieren 3.2:

aruco::estimatePoseSingleMarkers(corners, markerLength, camMatrix, distCoeffs, rvecs,tvecs); 

dies einen Vektor Translation und Rotation für den Marker zurück. Was ich brauche, sind die 3d Koordinaten für jede Ecke des Markers.

Als ich die Markierungslänge wissen, könnte ich so etwas wie

tut

corner1 = tvecs[0] - markerlength /2; 
corner2 = tvecs[0] + markerlength /2; 

....

Aber gibt es einen besseren Weg? Oder eine bestehende Funktion? Um es zusammenzufassen, ich habe:

ein 3D-Punkt in der Mitte eines 2d Platz.

die Länge der Seiten dieses Quadrats.

der Rotationswert des Quadrats.

Wie kann ich die 3D-Koordinaten der Ecken finden?

Answer 1

Zuerst nehmen wir an, dass wir nur mit side = 2 * half_side gegebenen Marker haben.

Zweitens aruco::detectMarker gibt die relative Position der Kamera in der Welt Marker. Daher nehme ich an, dass Sie nach den Koordinaten der Ecken in der Welt der Kamera suchen.

Dann Raum in Marker:

 [ half_side ]  [  0  ] 
E = [  0  ], F = [ half_side ] 
    [  0  ]  [  0  ] 

, wo das Zentrum des Quadrates Otvec hat Koordinate (in Welt der Kamera) und Rotationsmatte des Markers rot_mat durch cv::Rodrigues(rvec,rot_mat) berechnet.

Nun kann mit der Pinhole camera model, ist die Beziehung zwischen den Koordinaten eines Punktes P in Nocken der Welt und weltweit Marker ist:

[P_x_cam]    [P_x_marker] 
[P_y_cam] = rot_mat * [P_y_marker] + tvec 
[P_z_cam]    [P_z_marker]  

beispielsweise das Zentrum O, die [0,0,0] in Marker der Welt ist, ist tvec in der Welt der Kamera.

So werden die Koordinaten von E in Welt-Cam sind:

[E_x_cam]    [half_side] 
|E_y_cam| = rot_mat * | 0 | + tvec 
[E_z_cam]    [ 0 ] 

Magisch, es ist die Summe der rot_mat s erste Spalte multipliziert mit half_size und tvec. In ähnlicher Weise der von Koordinaten von F ist rot_mat ‚s zweite Spalte von half_size multipliziert und tvec.

Nun können die Ecken beispiels

C - O = (E - O) + (F - O), B - O = (E - O) - (F - O) 

wo E-O genau berechnet wird, werden rot_mat s erste Spalte von half_size multipliziert.

Mit allem daran, wir die Funktion zusammenstellen können:

vector<Point3f> getCornersInCameraWorld(double side, Vec3d rvec, Vec3d tvec){ 

    double half_side = side/2; 


    // compute rot_mat 
    Mat rot_mat; 
    Rodrigues(rvec, rot_mat); 

    // transpose of rot_mat for easy columns extraction 
    Mat rot_mat_t = rot_mat.t(); 

    // the two E-O and F-O vectors 
    double * tmp = rot_mat_t.ptr<double>(0); 
    Point3f camWorldE(tmp[0]*half_side, 
         tmp[1]*half_side, 
         tmp[2]*half_side); 

    tmp = rot_mat_t.ptr<double>(1); 
    Point3f camWorldF(tmp[0]*half_side, 
         tmp[1]*half_side, 
         tmp[2]*half_side); 

    // convert tvec to point 
    Point3f tvec_3f(tvec[0], tvec[1], tvec[2]); 

    // return vector: 
    vector<Point3f> ret(4,tvec_3f); 

    ret[0] += camWorldE + camWorldF; 
    ret[1] += -camWorldE + camWorldF; 
    ret[2] += -camWorldE - camWorldF; 
    ret[3] += camWorldE - camWorldF; 

    return ret; 
} 

---

Anmerkung 1: Ich hasse das SO nicht Mathjax

Anmerkung 2 hat: Es muss etwas schneller sein Implementierung, von der ich nicht weiß.