Rozliczenia projektu Tango onPoseAvailable() i getPoseAtTime()

Question

Widzę znaczące rozbieżności między pozami z oddzwaniania onPoseAvailable() i Tango.getPoseAtTime(). Napisałem program testowy, w którym w onPoseAvailable() logowałem dostarczoną pozę i użyłem getPoseAtTime(), aby poprosić o pozę za pomocą znacznika czasu z 2 wcześniejszych wywołań zwrotnych. KEY_BOOLEAN_SMOOTH_POSE jest skonfigurowany false. Oto kod, który robi (zmienna timestamps_ członkiem jest 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])); 
    } 
} 

Oto fragment z rzeczywistego log (I przeplatane zalogowanego połączeń dla jasności):

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) 

Spójrz na ostatni wpis getPoseAtTime() z sygnaturą czasową 2733.062994. Zauważ, że jego wartości pozycji nie pasują do pozy z onPoseAvailable z identycznym znacznikiem czasu. Coś tu jest nie tak.

Uznałem, że dopasowanie pozycyjne niekoniecznie musi przejść przez punkty kontrolne, ale nie sądzę, że jest to dopuszczalne wyjaśnienie. Przede wszystkim nie ma sensu posiadanie API, który dostarcza różne wartości dla tego samego pomiaru. Ale dodatkowo rzeczywiste liczby nie poprą tego przypuszczenia.

Sprawdź wartość Y getPoseAtTime(), 0,460060. Jest to poza zakresem Y wszystkich wartości Y onPoseAvailable(), zarówno przed jak i po (w zasadzie na cały dziennik). Żaden rozsądny model interpolacji nie może wytworzyć tej wartości.

Chyba pytanie, co się tutaj dzieje? Pozycje są niespójne, więc przynajmniej jedna z nich jest zła (jeśli nie obie). Domyślam się, że onPoseAvailable() jest bardziej prawdopodobne, że jest poprawne.

Oto wykres pozycji Y w funkcji czasu z dwóch stanowią metody (NASH uwalnianiu) z nieruchomym tabletki w stacji dokującej:

niebieska linia jest onPoseAvailable() zwrotna i linia czerwona to sondowanie getPoseAtTime(). Te wyniki są dziwne. Jeśli pozy w ogóle będą inne, to spodziewam się, że wartość odpytywana byłaby bardziej płynna, ponieważ mogłaby być filtrowana przy użyciu wkładów z próbek przed i po czasie odpytywania, podczas gdy wartość wywołania zwrotnego byłaby albo niefiltrowana, albo filtrowana tylko za pomocą wcześniejszych wartości. próbki. Ale to nie jest to, co widzimy - wartość ankietowa wydaje się dużo głośniejsza.

Oto podobny wykres uchwycony podczas przesuwania tabletu w górę iw dół. Wartość odpytywana ma jeszcze więcej wysokich częstotliwości, a dwa sygnały nie śledzą szczególnie blisko.

Answer 1

Dzięki rhashimoto za wskazanie, że obecnie!

EDIT

muszę zmieniać mój poprzedni post. Stwierdziłem, że miałem większy dryft podczas korzystania z GetPoseAtTime, a nie poza wywołaniem zwrotnym OnPoseAvailable.

Jest po prostu odwrotnie. Mam lepsze wyniki z GetPoseAtTime.

Wykonałem skanowanie obracając o 360 ° na moim krześle. Zacząłem i zatrzymałem się przy biurku, jak widać na zdjęciu.

początek i koniec cyklu skanowania (kliknij go do większej rozdzielczości)

punktu chmury nad użytku stwarza GetPoseAtTime i chmury punktu poniżej użytkowania stwarzają przez OnPoseAvailable zwrotnego. Oba przechwycone w tym samym czasie. Dryft z GetPoseAtTime jest marginalny, ale z funkcją oddzwaniania OnPoseAvailable naprawdę ogromną.

Do tej pory odkryłem, że GetPoseAtTime używa wykresu pozowania i koryguje pozy w przypadku wykrycia zamknięcia pętli see this article. Testowałem, czy wyniki się poprawiają, czy natychmiast uzyskuję dostęp do pozy z dostępną chmurą punktów, czy tylko na końcu, gdy łączę wszystkie chmury punktów.

Rzeczywiście, wynik jest o wiele lepszy. tam

OnPoseAvailabe zwrotna < GetPoseAtTime natychmiast dostępnej chmury punktów < GetPoseAtTime pod koniec skanowania