Выключил робота на ночь, но перед этим выгрузил все данные дня в Parquet, и агент ночью разбирался без робота. Нашел шикарное: ZED SDK несколько секунд после релокализации отдает координаты в миллиметрах, хотя в настройках стоят метры. За день 21 576 таких поз. Отсюда в логах «прыжки» на 4 километра и испорченная карта с препятствиями там где робот только что проехал. Еще статус KNOWN_MAP через 5-8 секунд снова сменяется на INITIALIZING, так что «нашел себя» теперь засчитывается только после 10 секунд стабильного статуса.
Фильтр получился простой: если поза прыгнула дальше чем робот мог доехать, она 4 секунды на карантине. Вернулась назад, значит это был глюк. Осталась на новом месте, значит это настоящая коррекция.
def update(self, now, position):
if self.accepted is None or self.reachable(self.accepted, now, position):
self.accepted = (now, position)
self.pending = None
return True
if self.pending is not None and self.reachable(self.pending[1:], now, position):
if now - self.pending[0] >= self.settle_s:
self.accepted = (now, position)
return True
return False
self.pending = (now, now, position)
return False
