Project Tango onPoseAvailable() und getPoseAtTime() Diskrepanzen

Question

Ich sehe erhebliche Diskrepanzen zwischen den Posen von dem onPoseAvailable() Rückruf und Tango.getPoseAtTime(). Ich schrieb ein Testprogramm, wo ich in onPoseAvailable() die gelieferte Pose protokollierte und getPoseAtTime() verwendete, um die Pose unter Verwendung des Zeitstempels von 2 Rückrufen früher anzufordern. KEY_BOOLEAN_SMOOTH_POSE ist konfiguriert false. Hier ist der Code, der tut das (die timestamps_ Membervariable ist ein LinkedList<Double>):

@Override 
public void onPoseAvailable(TangoPoseData poseData) { 
    if (poseData != null && poseData.statusCode == TangoPoseData.POSE_VALID) { 
     Log.v("bug", 
     String.format("onPoseAvailable t: %f, base: %d, target %d, p: (%f, %f, %f)", 
      poseData.timestamp, 
      poseData.baseFrame, 
      poseData.targetFrame, 
      poseData.translation[0], poseData.translation[1], poseData.translation[2])); 
     timestamps_.add(poseData.timestamp); 
     if (timestamps_.size() > 3) 
     timestamps_.remove(); 
    } 

    if (timestamps_.isEmpty()) 
     return; 

    TangoCoordinateFramePair framePair = new TangoCoordinateFramePair(
     TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE, 
     TangoPoseData.COORDINATE_FRAME_DEVICE); 
    poseData = tango_.getPoseAtTime(timestamps_.getFirst(), framePair); 
    if (poseData != null && poseData.statusCode == TangoPoseData.POSE_VALID) { 
     Log.v("bug", 
     String.format("getPoseAtTime t: %f, base: %d, target %d, p: (%f, %f, %f)", 
      poseData.timestamp, 
      poseData.baseFrame, 
      poseData.targetFrame, 
      poseData.translation[0], poseData.translation[1], poseData.translation[2])); 
    } 
} 

Hier ist ein Auszug aus einem aktuellen log (Ich habe die protokollierten Anrufe für Klarheit entschachtelt):

onPoseAvailable t: 2732.762486, base: 2, target 4, p: (0.280245, 0.412468, 0.562201) 
onPoseAvailable t: 2732.802553, base: 2, target 4, p: (0.296951, 0.420919, 0.599938) 
onPoseAvailable t: 2732.852638, base: 2, target 4, p: (0.317444, 0.429809, 0.646445) 
onPoseAvailable t: 2732.882689, base: 2, target 4, p: (0.330845, 0.434106, 0.676810) 
onPoseAvailable t: 2732.932774, base: 2, target 4, p: (0.350995, 0.439777, 0.723639) 
onPoseAvailable t: 2732.962825, base: 2, target 4, p: (0.363319, 0.442731, 0.754508) 
onPoseAvailable t: 2732.992875, base: 2, target 4, p: (0.373911, 0.445289, 0.784786) 
onPoseAvailable t: 2733.032943, base: 2, target 4, p: (0.387709, 0.448182, 0.822682) 
onPoseAvailable t: 2733.062994, base: 2, target 4, p: (0.398502, 0.450481, 0.852662) 
onPoseAvailable t: 2733.073011, base: 2, target 4, p: (0.401869, 0.451084, 0.862530) 
onPoseAvailable t: 2733.103062, base: 2, target 4, p: (0.411136, 0.452486, 0.890441) 

getPoseAtTime t: 2732.712401, base: 2, target 4, p: (0.269301, 0.410911, 0.549182) 
getPoseAtTime t: 2732.732435, base: 2, target 4, p: (0.277217, 0.415130, 0.567040) 
getPoseAtTime t: 2732.762486, base: 2, target 4, p: (0.288928, 0.421914, 0.595162) 
getPoseAtTime t: 2732.802553, base: 2, target 4, p: (0.305241, 0.429648, 0.632158) 
getPoseAtTime t: 2732.852638, base: 2, target 4, p: (0.324359, 0.437655, 0.680300) 
getPoseAtTime t: 2732.882689, base: 2, target 4, p: (0.332997, 0.442538, 0.712727) 
getPoseAtTime t: 2732.932774, base: 2, target 4, p: (0.353665, 0.447269, 0.759725) 
getPoseAtTime t: 2732.962825, base: 2, target 4, p: (0.369174, 0.451645, 0.790263) 
getPoseAtTime t: 2732.992875, base: 2, target 4, p: (0.382584, 0.454754, 0.819555) 
getPoseAtTime t: 2733.032943, base: 2, target 4, p: (0.396857, 0.456922, 0.856626) 
getPoseAtTime t: 2733.062994, base: 2, target 4, p: (0.409672, 0.460060, 0.888748) 

Werfen Sie einen Blick auf den letzten Eintrag getPoseAtTime() mit dem Zeitstempel 2733.062994. Beachten Sie, dass seine Positionswerte nicht mit der Pose von onPoseAvailable mit identischem Zeitstempel übereinstimmen. Etwas stimmt hier nicht.

Ich dachte, dass ein Spline-Fit der Pose nicht notwendigerweise durch die Kontrollpunkte gehen muss, aber ich denke nicht, dass das eine akzeptable Erklärung ist. Vor allem macht es wenig Sinn, eine API zu haben, die unterschiedliche Werte für die gleiche Messung liefert. Aber die tatsächlichen Zahlen unterstützen diese Vermutung nicht.

Betrachten Sie den getPoseAtTime() Y-Wert, 0.460060. Dies ist außerhalb des Y-Bereichs aller onPoseAvailable() Y-Werte, sowohl davor als auch danach (über das gesamte Protokoll als eine Tatsache). Kein sinnvolles Interpolationsmodell kann diesen Wert erzeugen.

Ich denke, die Frage ist, was hier los ist? Die Posen sind inkonsistent, so dass mindestens einer falsch ist (wenn nicht beide). Meine Vermutung wäre, dass die onPoseAvailable() eher wahr ist. Hier

ist ein Graph, der Y-Position gegenüber der Zeit der beide pose Methoden (Nash release) mit dem Tablett stationär in seinem Dock:

Die blaue Linie ist der onPoseAvailable() Rückruf und die rote Linie ist die getPoseAtTime() Abfrage. Diese Ergebnisse sind irgendwie seltsam. Wenn die Posen überhaupt anders aussehen, würde ich erwarten, dass der abgefragte Wert glatter wäre, da er gefiltert werden könnte, indem Beiträge von Samples vor und nach der Pollzeit verwendet würden, während der Callback-Wert entweder ungefiltert oder nur unter Verwendung von vorher gefiltert würde Proben. Aber das sehen wir nicht - der Wert erscheint viel lauter.

Hier ist eine ähnliche Grafik, während ich das Tablet auf und ab bewegte. Der abgefragte Wert weist immer noch höhere Frequenzen auf, und die beiden Signale werden nicht besonders genau verfolgt.

Answer 1

Dank rhashimoto dass für den Hinweis auf!

EDIT

Ich habe meine frühere Post zu bearbeiten. Ich behauptete, dass ich eine größere Drift bei der Verwendung von GetPoseAtTime anstelle der Pose des OnPoseAvailable Callbacks hatte.

Es ist nur umgekehrt. Ich bekomme viel bessere Ergebnisse mit GetPoseAtTime.

Ich machte einen Scan, indem ich 360 ° auf meinem Stuhl drehte. Ich begann und blieb an meinem Schreibtisch stehen, wie Sie auf dem Bild sehen können.

Beginn und Ende des Abtastzyklus (klicken Sie für eine größere Auflösung)

Die Punktwolken über den Einsatz stellt GetPoseAtTime und die Punktwolken unter Verwendung Pose am OnPoseAvailable Rückruf. Beide wurden gleichzeitig gefangen genommen. Die Drift mit GetPoseAtTime ist marginal, aber mit OnPoseAvailable Callback wirklich sehr groß.

Was ich bisher herausgefunden habe, ist, dass GetPoseAtTime ein Pose-Diagramm verwendet und die Posen korrigiert, wenn ein Loop-Closure erkannt wird see this article. Ich habe getestet, ob die Ergebnisse besser werden, ob ich sofort mit einer verfügbaren Punktwolke auf die Pose zugreife oder erst am Ende, wenn ich alle Punktwolken zusammenfasse.

Und tatsächlich ist das Ergebnis viel besser. Also meine Erfahrung so weit ist:

OnPoseAvailabe Rückruf < GetPoseAtTime sofort mit einem verfügbaren Punktwolke < GetPoseAtTime am Ende des Scannens