Wie Rotation und Translation finden (Transformations-Matrix) zwischen entsprechenden Punkten in zwei verschiedenen coorinate systens

Question

ich auf einer Kamera-Lidar-Kalibrierung arbeite und ich bin seit einiger Zeit auf dem folgende Problem fest:

Ich verwende eine USB-Kamera und ein 2D-Lidar. Ich habe die Koordinaten der entsprechenden Punkte sowohl im Lidar-Frame als auch im Kamerarahmen (nehmen wir an, dass ich 3 Punkte und ihre Koordinaten im Lidar-Frame und die Koordinaten derselben 3 Punkte im Kamerarahmen habe).

Beispiel für einen Punkt:
lidar_pt1 (xl, yl)
camera_pt1 (xc, yc, zc)
...
sind bekannt.

Wenn ich die Transformationsmatrix fest codiere, bekomme ich ein erwartetes Ergebnis. Jetzt versuche ich nicht, es hart zu codieren, sondern es automatisch mit den bekannten Werten der Koordinaten zu berechnen. Was ich habe, sind 3 Punkte in 2D-Koordinaten im Lidar-Rahmen und genau 3 Punkte in 3D-Koordinaten im Kamerarahmen. Es ist hier, wo ich mit der Mathematik zu kämpfen habe, um irgendwie die Drehung basierend auf dem Koordinatenwert zu berechnen, den ich habe. Gibt es eine Möglichkeit, diese Rotation zu bekommen?

camera_pt1 = TransformMat * lidarpt1
TransformMat =?

Ich sah einige Beispiele mit SVD (http://nghiaho.com/?page_id=671), aber ich denke, dass sie größere Datensätze benötigen und das Minimum von 3 Punkten würde nicht das beste Ergebnis geben.

Answer 1

Wenn Sie nur drei Koordinatenpaare von jedem System nehmen, ist die Mathematik ziemlich einfach. Hier ist ein einfaches Beispiel:

| 
    4 |      (R) 
    |      : ', 
    |      : ', 
    |      :  ', 
    3 |      :  (P) 
    |      :  ,' 
    |      : ,' 
    |      : ,' 
    2 |  (A).....(B)  (Q) 
    |  :  ,' 
    |  : ,' 
    |  : ,' 
    1 |  (C) 
    | 
    | 
    | 
    0 +------------------------------------- 
    0  1  2  3  4 

Angenommen, Sie ein Dreieck ABC haben, die PQR auf ein anderes Dreieck bildet. Sie können ihre Scheitelpunkte in homogeneous coordinates wie folgt darstellen:

 .-  -.   .-  -. 
     | 1 2 1 |   | 4 3 1 | 
ABC = | 2 2 1 | PQR = | 3 2 1 | 
     | 1 1 1 |   | 3 4 1 | 
     '-  -'   '-  -' 

Sie benötigen eine Matrix M zu finden, die ABC auf PQR Karten (das heißt, ABC × M = PQR). Dazu multiplizieren nur PQR durch die Inverse von ABC:

if ABC × M = PQR, 
then ABC⁻¹ × ABC × M = ABC⁻¹ × PQR 
so M = ABC⁻¹ × PQR 

Es gibt viele Referenzen auf, wie eine 3 × 3-Matrix invertieren. Dies sollte Ihnen folgendes Ergebnis geben:

.-   -. 
    | -1 -1 0 | 
M = | 1 -1 0 | 
    | 3 6 1 | 
    '-   -'