Radiale Distanz zu XYZ

Treue Seele

Beruf: Student, Hardware- und Softwareentwicklung als wissenschaftliche Hilfskraft

10.06.2015, 16:45

Radiale Distanz zu XYZ

Moin,

ich wollte eigentlich nur mal fix die radialen Distanzwerte eines 3D Sensors in XYZ Koordinaten umrechnen und anzeigen. Aber irgendwie komme ich da nicht so recht weiter.

Zum Testen habe ich mir eine Matrix generiert, in welcher jede radiale Distanz genau 1.0 beträgt. Der Sensor ist so aufgebaut, dass +y nach oben und +x nach links geht und dass +z die Blickrichtung des Sensors ist. Also ein normales rechtshändiges Koordinatensystem.
Für die Formeln der Umrechnung habe ich folgende Webseite herangezogen: http://mathworld.wolfram.com/SphericalCoordinates.html

Meine Funktion sieht so aus:

C-/C++-Quelltext

    pcl::PointCloud<pcl::PointXYZRGB> cloud;

    cloud.height = 144;
    cloud.width = 176;
    cloud.resize(cloud.height * cloud.width);

    // KoordinatenSystem:
    // +Y -> Oben
    // +X -> Links
    // +Z -> Raus
    // Gegeben: Radiale Entfernung (depth), Blickrichtung +Z

    // http://mathworld.wolfram.com/SphericalCoordinates.html
    // Horizontaler Startwinkel (tetha) (Zwischen X und Y)
    // 90 - Halber Öffnungswinkel des Sensors
    // Weil: +X geht nach Links und Blickrichtung +Z, somit liegt der Nullpunkt bei 90°
    float horAngel = 90.0 - (44.0 / 2.0);
    // Vertikaler Startwinkel (phi) (Zwischen Z und XY Ebene)
    // Halber Öffnungswinkel
    float vertAngel = +(35.0 / 2.0);

    // Winkelschritt zwischen zwei Pixeln = Öffnungswinkel / Anzahl Pixel
    float horAngelStep = 44.0 / 176.0;
    float vertAngelStep = 35.0 / 144.0;

    for (int i = 0; i < cloud.height; i++)
    {
        for (int j = 0; j < cloud.width; j++)
        {
            cloud.at(j, i).x = depth[i][j] * std::sin(vertAngel * (M_PI / 180.0)) * std::cos(horAngel * (M_PI / 180.0));
            cloud.at(j, i).y = depth[i][j] * std::sin(vertAngel * (M_PI / 180.0)) * std::sin(horAngel * (M_PI / 180.0));
            cloud.at(j, i).z = depth[i][j] * std::cos(vertAngel * (M_PI / 180.0));

            cloud.at(j, i).r = 255;
            cloud.at(j, i).g = 255;
            cloud.at(j, i).b = 255;

            horAngel += horAngelStep;
        }
        horAngel = 90.0 - (44.0 / 2.0);
        vertAngel -= vertAngelStep;
    }

    return cloud;

Ich würde ja eigentlich ein Kugelausschnitt in z Richtung um den Ursprung herum erwarten. Allerdings sieht mein Ergebnis etwas anders aus (siehe ergebnis1.png im Anhang).

Ich habe zwischenzeitlich mal etwas mit den Startwinkeln rumgespielt. Dabei habe ich ein richtiges aussehendes Ergebnis bekommen. Allerdings in y-Richtung und nicht in z-Richtung (siehe ergebnis2.png im Anhang).

Irgendwo muss ich da wohl mit den Winkeln oder Richtungen durcheinander gekommen sein. Ich finde es aber nicht.
Hat jemand eine Idee?

»Tankard« hat folgende Bilder angehängt:

Zum Seitenanfang

Treue Seele

Beruf: Student, Hardware- und Softwareentwicklung als wissenschaftliche Hilfskraft

10.06.2015, 22:26

Letztendlich will ich "einfach nur" die radialen Rohdaten des Sensors in eine XYZ Punktewolke umwandeln. Meine Testdaten sind alle 1.0, womit ich einen Kugelausschnitt bekommen sollte der in Richtung +Z zu sehen ist und dessen Mittelpunkt genau mein Ursprung ist.

Für die Umwandlung wollte ich mir die entsprechenden Winkel der Kugelkoordinaten anhand des Öffnungswinkel, der Anzahl der Pixel und aktueller Pixelposition berechnen.

Zum Seitenanfang

Alter Hase

10.06.2015, 22:54

Ich Verstehe die Rechnung nicht so Ganz.

Du Iterierst über die Höhe und Breite der Cloud, aber gleichzeitig Addierst du den Step der Winkel.

Lege doch gleich die Höhe und Breite der Cloud auf ein Winkel um, und Iteriere über die Steps der Winkel.

Das wäre für mich zumindestens inuitiver.

Edit: Übrigens ist nicht ersichtlich was death[][] ist. (Rest war sinnfrei)

Wer aufhört besser werden zu wollen hört auf gut zu sein!

aktuelles Projekt:Rickety Racquet

Dieser Beitrag wurde bereits 3 mal editiert, zuletzt von »Koschi« (10.06.2015, 23:33)

Zum Seitenanfang

Treue Seele

Beruf: Student, Hardware- und Softwareentwicklung als wissenschaftliche Hilfskraft

10.06.2015, 23:45

depth[][] ist eine Matrix (2 dimensionales Array) wo die radialen Distanzen des Sensors gespeichert werden. Für meine Testdaten habe ich diese alle auf 1 gesetzt. Diese radialen Distanzen möchte ich nun in XYZ Koordinaten umrechnen und in einer PointCloud speichern.

Dazu iteriere ich über meine radialen Distanzen. Zuvor berechne ich mir die beiden Startwinkel (horizontal und vertikal) der Kugelkoordinaten anhand des Sichtfeldes des Sensors (horAngel, vertAngel) und Anhand des Sichtfeldes und der Anzahl der Pixel in den jeweiligen Richtungen die Winkeländerung, welche ich pro Zeilen-/Spaltenschritt habe (horAngelStep, vertAngelStep). Ich starte auf diese Weise ja mit dem Pixel oben Links, also der der am weitesten in +x und +y Richtung liegt.

Die Daten sind Zeilenweise gespeichert. Somit gehe ich in der äußeren Schleife die Zeilen und in der inneren die Spalten durch. Ich fange also bei dem Pixel oben links an und gehe erstmal die erste Zeile durch bis zum Pixel oben rechts. Daher berechne ich mir in jedem Schritt nach der Formel in dem Link aus der radialen Distanz (depth[][]) und den beiden Winkeln die kartesischen Koordinaten. Anschließend addiere ich den Winkelschritt auf den Winkel, da ich ja im nächsten Schleifenschritt einen Pixel weiter links bin. Wenn die Zeile fertig ist wird der horizontale Winkel wieder zurück gesetzt und für die nächste Zeile der vertikale Winkel um den Winkelschritt erhöht.

So war es für mich intuitiv. Die Matrix mit den Daten zeilenweise iterieren und an jeder Stelle die aktuellen Winkel berechnen.

@Koschi
Was genau meinst du mit "die Höhe und Breite der Cloud auf ein Winkel umlegen"?

Zum Seitenanfang

Alter Hase

11.06.2015, 00:07

Zitat von »Tankard«

@Koschi
Was genau meinst du mit "die Höhe und Breite der Cloud auf ein Winkel umlegen"?

das machst du im Prinzip ja schon (ist mir vorher nicht so klar gewesen), du sagst mit der Cloud höhe und Breite nur wie fein du diese Dargstellt haben möchtest

Was du aber mal noch Probiern könntest, nur ein Quadranten abzubilden (Spiegelachse wäre Horizontale und Vertikale 0° Linie) die anderen 3 lassen sich dann über die Symetrie bestimmen. Vielleicht Hilft das ja etwas,das Erebnis zu verbessern.

Edit: Noch was ist mir aufgefallen:

Zitat

Ich würde ja eigentlich ein Kugelausschnitt in z Richtung um den Ursprung herum erwarten. Allerdings sieht mein Ergebnis etwas anders aus (siehe ergebnis1.png im Anhang).

Um den Ursprung ist nicht Möglich da der Horizontal Winkel nur von 68 ° bis 112° abgebildet wird. Die 68 ° Ergeben sich aus float horAngel = 90.0 - (44.0 / 2.0); dann addierst du dazu wieder 44 ° aufgeteil in 176 schritten (float horAngelStep = 44.0 / 176.0; horAngel += horAngelStep

Wenn es um den Ursprung sein soll müsste es doch von +22° bis -22° grad sein. Wenn du 90 ° drauf addierst bist du ja eigentlich in Blickrichtung X oder?

Wer aufhört besser werden zu wollen hört auf gut zu sein!

aktuelles Projekt:Rickety Racquet

Dieser Beitrag wurde bereits 1 mal editiert, zuletzt von »Koschi« (11.06.2015, 00:36)

Zum Seitenanfang

Treue Seele

Beruf: Student, Hardware- und Softwareentwicklung als wissenschaftliche Hilfskraft

11.06.2015, 14:10

Zitat von »Koschi«

Was du aber mal noch Probiern könntest, nur ein Quadranten abzubilden (Spiegelachse wäre Horizontale und Vertikale 0° Linie) die anderen 3 lassen sich dann über die Symetrie bestimmen. Vielleicht Hilft das ja etwas,das Erebnis zu verbessern.

Das würde mir nicht helfen. Ich habe jetzt zwar Testdaten, mit denen dies möglich wäre, aber meine realen Daten sind nachher ja nicht mehr symmetrisch.

Zitat von »Koschi«

Edit: Noch was ist mir aufgefallen:

Zitat

Ich würde ja eigentlich ein Kugelausschnitt in z Richtung um den Ursprung herum erwarten. Allerdings sieht mein Ergebnis etwas anders aus (siehe ergebnis1.png im Anhang).

Um den Ursprung ist nicht Möglich da der Horizontal Winkel nur von 68 ° bis 112° abgebildet wird. Die 68 ° Ergeben sich aus float horAngel = 90.0 - (44.0 / 2.0); dann addierst du dazu wieder 44 ° aufgeteil in 176 schritten (float horAngelStep = 44.0 / 176.0; horAngel += horAngelStep
Wenn es um den Ursprung sein soll müsste es doch von +22° bis -22° grad sein. Wenn du 90 ° drauf addierst bist du ja eigentlich in Blickrichtung X oder?

Ich arbeite ja im 3D. Wenn die Winkel nicht um 0° liegen, dann verschiebt sich nur die Lage des Kugelabschnitts im Raum, aber nicht der Mittelpunkt der Kugel. Die Punkte liegen immer um den Ursprung, da sie ja alle vom Ursprung ausgehend die selbe Länge haben.

Zitat von »[Anonymer Benutzer]«

Versuch mal damit:

die Winkel müssen getauscht werden da bin cih mir fast sicher da hast du es verwechselt zwischen horizontal und vertikal bei der rechnung

vertikaler startWinkel=0°
schrittweite vertikel=90/144

horizontaler startwinkel=0
schrittweite horizontal= 180/176

poste das bild wie es aussieht

Ich habe mal die Winkel wie in deinem Post gewählt. In ergebnis3.png ist das Ergebnis mit diesen Werten und ich habe horAngel und vertAngel bei der Berechnung vertauscht. In ergebnis4.png habe ich die beiden Winkel dann wieder zurück getauscht, so wie sie in meinem Startpost genutzt werden.

In beiden Fällen ist die Punktwolke nicht auf +Z Blickrichtung ausgerichtet. Aber in ergebnis3.png sieht man wieder das Phänomen, dass in Richtung +Z plötzlich ein Kreuz entsteht, wie bei ergebnis1.png aus meinem Startpost.

»Tankard« hat folgende Bilder angehängt:

Zum Seitenanfang

Alter Hase

11.06.2015, 14:41

Zitat

Ich arbeite ja im 3D. Wenn die Winkel nicht um 0° liegen, dann verschiebt sich nur die Lage des Kugelabschnitts im Raum, aber nicht der Mittelpunkt der Kugel. Die Punkte liegen immer um den Ursprung, da sie ja alle vom Ursprung ausgehend die selbe Länge haben.

Ja, aber du hast geschrieben das du in Z-Richtung die Wolke erwartest, wenn du diese um 90 Grad verschiebst ist ja nicht mehr Z-Richtung sondern in Richtung X odér Y Achse (abhängig ob horizontal oder vertikal verschoben).

Wer aufhört besser werden zu wollen hört auf gut zu sein!

aktuelles Projekt:Rickety Racquet

Zum Seitenanfang

Treue Seele

Beruf: Student, Hardware- und Softwareentwicklung als wissenschaftliche Hilfskraft

11.06.2015, 15:03

Ja, die Verschiebung hatte ich gemacht, da die Winkel aus Sicht des Sensors und die Winkel, wie sie in den Kugelkoordinaten verwendet werden, nicht überein stimmen. Wenn ich mir die Grafik in dem Link (http://mathworld.wolfram.com/SphericalCoordinates.html) angegucke, dann ist theta 0° ja auf der x-Achse. Wenn ich mit dem Sichtfeld des Sensors arbeite, dann wäre theta 0° aber auf der y-Achse, was ja eigentlich 90° entsprechen würde. Daher habe ich dann 90°-(Öffnungswinkel/2) als start genommen. Nach meinem Verständnis bin ich dann von der y-Achse aus um den halben Öffnungswinkel in Richtung +x gegangen.
Bei mir ist ja +x nach Links und +y nach Oben. Also vom Ursprung entlang der z-Achse wären es bei mir theta = 0° und phi = 0° und nicht wie in der Grafik theta = 90° und phi = 0°.
Zumindest habe ich das so verstanden.

Zum Seitenanfang

Treue Seele

Beruf: Student, Hardware- und Softwareentwicklung als wissenschaftliche Hilfskraft

11.06.2015, 15:36

Zitat von »[Anonymer Benutzer]«

Also waren die Werte doch korrekt bezüglich horizontal vertikal.
Also ergebnis 4 sieht doch gut aus es ist genau der Kugel ausschnitt den du mti den Formal berechnen kannst

Obwohl man den vertikalen winkel zwischen 0 und 90° halten sollte versuch mal folgendes:

startwinkel -90°
schrittweite 180/144

C-/C++-Quelltext

    float horAngel = 90.0;
    float vertAngel = -90.0;

    float horAngelStep = 180.0 / 176.0;
    float vertAngelStep = 180.0 / 144.0;

    for (int i = 0; i < cloud.height; i++)
    {
        for (int j = 0; j < cloud.width; j++)
        {
            cloud.at(j, i).x = depth[i][j] * std::sin(vertAngel * (M_PI / 180.0)) * std::cos(horAngel * (M_PI / 180.0));
            cloud.at(j, i).y = depth[i][j] * std::sin(vertAngel * (M_PI / 180.0)) * std::sin(horAngel * (M_PI / 180.0));
            cloud.at(j, i).z = depth[i][j] * std::cos(vertAngel * (M_PI / 180.0));

            cloud.at(j, i).r = 255;
            cloud.at(j, i).g = 255;
            cloud.at(j, i).b = 255;

            horAngel += horAngelStep;
        }
        horAngel = 90.0;
        vertAngel += vertAngelStep;
    }

Dann bekomme ich eine Halbkugel in z-Richtung (ergebnis5.png). Also prinzipiell scheint das ja schonmal zu stimmen mit der Berechnung.

Nun muss ich das Ganze ja wieder zurück auf mein Sensorsichtfeld bringen. Und daran scheitert es wieder. Ich habe ja nur ein Sichtfeld von 44°x35° und nicht von 180°x180°. Ich bin die ganze Zeit am rumprobieren, aber irgendwie versteh ich das Verhalten einfach nicht, wenn ich die Startwinkel änder.

Edit:
Ich glaube der ganze Ansatz funktioniert nicht. Sobald ich beim vertikalen Winkel um 0° herum arbeite, wo dann die Punktwolke in Richtung z ausgerichtet ist, passieren blöde Sachen. Je weiter ich an 0° heran komme, desto weiter wird die Punktwolke an der Stelle verengt. Daraus resultiert dann so ein Bild wie bei ergebnis1.png. Also ein Kreuz. Wenn ich die Startwinkel so wähle, dass die Punktwolke nicht in Richtung z ausgerichtet ist, sondern irgendwo anders, sieht das Ergebnis OK aus, wie z.B. in ergebnis2.png.

Dieser Beitrag wurde bereits 1 mal editiert, zuletzt von »Tankard« (11.06.2015, 15:49)

Zum Seitenanfang

Alter Hase