Koordinatentransformation

Das Modul zur Umsetzung der Koordinatentransformation besteht aus der Schnittstellendatei coordinate_transformation.h und ihrer Implementierung coordinate_transformation.c. Der Funktionsprototyp sieht folgendermaßen aus:

void coord_transform(const urg_t* lidar_handle, long* distance_array, int numMeasurements, long* x_coord, long* y_coord, int trans_x, int trans_y);

Die Eingangsparameter sind:

1. lidar_handle: Lidarobjekt dient dem Zugriff auf die erstellte LiDAR Instanz und ihre Funktionalitäten
2. distance_array: dynamisch erstellte Array der empfangenen Schrägentfernungswerte
3. numMeasurements: Anzahl der durchgeführten Messungen
4. x_coord: Dieser leere, dynamische Array wird über Call-by-Reference übergeben. Die transformierten x-Koordinaten werden in diesem Array geschrieben
5. y_coord: Dieser leere, dynamische Array wird über Call-by-Reference übergeben. Die transformierten y-Koordinaten werden in diesem Array geschrieben
6. trans_x: x-Komponente des Translationsvektors, falls nötig, zwischen Ursprung dem KoS des Fahrzeugs und dem des LiDARs
7. trans_y: y-Komponente des Translationsvektors, falls nötig, zwischen Ursprung dem KoS des Fahrzeugs und dem des LiDARs

Die Funktion ist void. Es wird kein Rückgabewert zurückgeliefert. Die Ausgangsparameter der Funktion bzw. ihre Resultate werden in den übergebenen, dynamischen, leeren Arrays gespeichert. Im Folgenden wird die Definition der Koordinatentransformationsfunktion in detaillierten Schritten erklärt.

Zuerst findet die Deklaration der lokalen Variablen statt

	long distance; // einen Schrägentfernungswert von dem distance_array ablesen und speichern pro Durchlauf
	double angle_rad; // den entsprechenden Winkel ermitteln und in dieser Variable speichern pro Durchlauf
        long x; // die daraus berechnete x-Koordinate hier speichern pro Durchlauf
	long y; // die daraus berechnete y-Koordinate hier speichern pro Durchlauf

Hiernach kommt die Hauptschleife. Innerhalb dieser Schleife wird ein Entfernungswert von dem übergebenen Array distance_array abgelesen und in eine x- und y- Koordinate überführt. Diese bilden das x-y Koordinatenpaar dieses bestimmten Messpunktes. Dies passiert, indem die folgenden mathematischen Gleichungen angewendet werden:

$x = radius * cos(\vartheta )$

$y = radius * sin(\vartheta )$

Gegebenenfalls erfolgt eine Translation anhand von dem übergebenen Translationsvektor-Argument. Am Ende vom Durchlauf wird jede Koordinate an der richtigen Stelle in dem entsprechenden, leeren Array, der über Call-by-Reference übergeben wurde, gespeichert.

	
        /*Main loop*/
	for (int i = 0 ; i < numMeasurements ; i++) {
		distance = distance_array[i]; // Entfernungswert ablesen und speichern
		angle_rad = urg_index2rad(lidar_handle, i); // Entsprechenden Winkel ermitteln mit der verfügbaren Member-Method                   
		x = (long)(distance * cos(angle_rad)); // x-Koordinate aus Polarkoordinaten ermitteln in mm
		y = (long)(distance * sin(angle_rad)); // y-Koordinate aus Polarkoordinaten ermitteln in mm

		/*Je nachdem ist die Translation erforderlich*/
		//x = (long)(trans_x + x); // Translate x
		//y = (long)(trans_y + y); // Translate y

		/*Speichern in Arrays*/
		x_coord[i] = x;
		y_coord[i] = y;
	}

→ zurück zum Artikel:Objekttracking_mit_LiDAR