OpenCV 3D-2D Transformation (projectPoints)

Hey Leute,

kleine Frage die mich seid einiger Zeit quält. Ich möchte eine Distanz 
vom Fahrzeug (in dem eine Kamera angebracht ist) in ein Kamerabild 
einzeichnen. Dazu bietet sich ja die Methode "projectpoints" von opencv 
an. Schonmal zu Beginn das Problem: ich bin der Meinung ich habe den 
richtigen Weg gewählt (den ich gleich erläutere) kriege allerdings einen 
leichten Versatz (eingezeichnet: 10-11m / real: 9m).

Beim Tippen ist mir aufgefallen, dass sicherlich der sourceCode zur 
Beurteilung des Fehlers hilfreich wäre, also befindet der sich auch im 
Anhang. Kurze Info, da ich die Klasse in der die Aufrufe stattfinden 
nicht beigefügt habe: Die .cpp ist chronologisch der aufrufe geordnet -> 
zuerst die setter, dann msgRcv und dann die Berechnung.


Mein Weg:
Ich habe die Werte der Translation sowie Rotation der Kamera in Bezug 
auf das Fahrzeugkoordinatensystem (Hinterachse) gegeben.
Da sich die Achsenbezeichnungen sich zwischen opencv-Koordinaten und 
Fahrzeugkoordinaten unterscheiden (Fahrzeug: X=tiefe bzw Roll, 
Y=horizontal bzw. Nick, Z=Höhe bzw. Gier / Opencv: x= horizontal, 
Y=Vertikal, Z=Tiefe), wandel ich diese als erstes um. Kurz gesagt: 
angle_camera_depthZ = angle_car_depthX , angle_camera_heightY = 
angle_car_yawZ.. Das selbe mit dem anderen Winkel und den Translationen. 
Bei Verwirrung leuchtet folgendes Bild vielleicht besser ein: 
https://de.wikipedia.org/wiki/Drehratensensor#/media/File:Roll_pitch_yaw_gravitation_center_de.png


Anschließend stelle ich mir meine Rotationsmatrizen auf, dazu wird die 
Matrix um bspw. die Y-Achse mit angle_camera_heightY bzw. angle_car_yawZ 
gefüllt. Habe ich alle 3 Matrizen aufgestellt, folgt R = Rx  Ry  Rz. 
Anschließend benutze ich den Rodrigues, um mir den Rotationsvektor 
berechnen zu lassen. Nun stelle ich mein Translationsvektor auf, der die 
Koordinatenverschiebung zum Weltkoordinatensystem enthält.
Zu guter letzt übergebe ich Projectpoints alle Werte.


Vielleicht kann mir jemand sagen, wo der Fehler liegt, dass ein Versatz 
entsteht. Grundsätzlich ergibt das Ergebnis wie erwähnt halbwegs Sinn 
(auch bei sehr hohen Entfernungen strebt er gegen Bildmitte).

Wäre über Hilfe sehr dankbar.

Euer Hansi