Визуальная и лидарная одометрия для уличного мобильного робота: ORB-SLAM 3, RtabMap, OpenVins, Fast-LIO 2 и DLIO

Одна из самых сложных и важных задач мобильного робота — определить собственное местоположение в пространстве, то есть локализация. В общем случае её делят на два вида.

Глобальная — как правило, привязанная к координатам земной поверхности: полярным, например WGS84 (World Geodetic System 1984), или ECEF (Earth-Centered, Earth-Fixed) — геоцентрической декартовой трёхмерной системе координат, которая вращается вместе с планетой, и т.п. Чтобы определить положение в такой системе без накопления дрейфа и без установки начального положения, нужны абсолютные датчики местоположения: GPS-датчик положения, курса и скорости, а также подобные ему системы, работающие через спутники или наземные маяки — вышки сотовой связи, оптические или лидарные системы с предварительно загруженными картами местности (лидарными или, например, спутниковыми снимками Земли с летательных аппаратов).

Локальная — отсчитывающая положение относительно стартовых координат, обычно в декартовой трёхмерной системе. Собственно процесс счисления пройденного пути по оборотам колёс либо сопоставлением лидарных сканов или снимков с камеры и называется одометрией. Этот процесс необратимо накапливает погрешность из-за дискретизации: камера отсчитывает путь пикселями, и хоть пиксель мал, у него есть физический размер (разрешение); лидар — точками, которые тоже измеряются с некоторой, пусть и небольшой, погрешностью; колесо — оборотами, которые считаются не идеально. Даже если колесо катится абсолютно точно, без пробуксовок, эта погрешность будет накапливаться очень медленно и зависеть от разрешающей способности энкодера, которая есть даже у аналогового датчика.

Оба подхода работают по-разному, но стремятся решить одну проблему — максимально точно определить местоположение робота в пространстве. В идеальной локализации этот процесс должен быть устойчив к изменениям внешней среды и к её воздействиям на робота. Накопление погрешности или неточности измерений должно происходить строго из-за физических ограничений самих датчиков: разрешающей способности, точности измерений сенсора, длины пройденного пути.

Но в реальном мире методы локализации, к сожалению, несовершенны, и погрешность накапливается ещё и из-за ограничений и недостатков самих методов — весьма условно, для простоты, можно сказать, из-за ошибок в реализации алгоритма, то есть в самом софте. Эти ошибки зачастую во много раз превышают теоретически возможную неточность самих сенсоров, и называть их неточностью технически вообще неверно. Например, если алгоритм начал выдавать абсолютно неверные данные и одометрия перестала правильно работать — это уже не стандартное техническое ограничение софта и не обычное накопление ошибки, а по сути технический отказ, поломка алгоритма. Из-за того, что алгоритм работает неправильно, накапливается большая погрешность, и зачастую продолжить работу без полной перезагрузки уже невозможно.

У глобальных систем в ситуации отказа есть важное техническое преимущество — релокализация, то есть восстановление своей работы и определение местоположения после падения благодаря предварительно загруженной карте или сигналам спутника. Локальная же система вынуждена начинать отсчёт от точки отказа заново, и восстановить полную точную траекторию движения зачастую уже не представляется возможным (если во время сбоя аппарат покинул отсканированный локальный участок карты или дрейф нарушил её целостность). Однако преимущество локальных систем в том, что, как правило, все компоненты этой системы работают на борту робота с его скромными вычислительными ресурсами и не зависят от внешней инфраструктуры. Кроме того, они способны работать на совершенно незнакомой местности, где нет готовой предварительно загруженной карты и радиосигналов для локализации, — то есть в режиме абсолютной автономности.

В этом обзоре я рассматриваю именно системы локальной навигации — визуальной и лидарной, — потому что эти датчики больше всего распространены в мобильной робототехнике: они дают оптимальный баланс между точностью, доступностью системы (в плане приобретения и требований к инфраструктуре) и приемлемой надёжностью локализации. К тому же они являются важной частью других систем робота — например, системы ухода от препятствий и оценки проходимости, без которых он не может полноценно функционировать.

Разработка алгоритмов одометрии — сложнейшая задача в робототехнике, которой занимаются целые институты. Разработчиком этих систем я не являюсь и потому буду оценивать их работу как простой пользователь: путём сравнения их друг с другом, а также с точки зрения пригодности и соответствия задачам мобильной робототехники.

ORB-SLAM 3

Тест 1

Тест начинается с инициализации камеры — её перемещения в пространстве. Это позволяет ORB-SLAM 3 найти соответствие между визуальным перемещением и данными от IMU, чтобы откалибровать его.

Маршрут был примерно таким:

Поначалу в среде, богатой геометрией (различными деревьями и домами частного сектора) и визуальными признаками (точками, по которым алгоритм находит в кадре какие-то уникальные места), ORB-SLAM 3 работает хорошо. Но при подъёме в гору сцена начинает деградировать: множество ключевых точек переключаются на облака, что не несёт никакой пользы алгоритму — облака расположены очень далеко и фактически от кадра к кадру остаются неподвижными, а значит, по ним нельзя оценить движение машины. Также растительности по краям дороги становится всё меньше, она удаляется от камеры, а удаление мешает точной оценке расстояния, а значит, и точной оценке позиции машины относительно неё. Низкая трава на земле без крупных домов и растений приводит к вырождению геометрии: при сопоставлении облака точек между кадрами нет чёткой разницы. Все части поля с травой в плане геометрии выглядят примерно одинаково — текущий кадр можно подставить в любое место на карте этого поля, и он с точки зрения алгоритма и сопоставления точек будет одинаково хорошо подходить. А значит, точно определить, где находится машина на карте (этого поля), нельзя — начинается дрейф. Мы видим, как зелёный маркер на карте, обозначающий позицию камеры (а значит, и машины), начинает вилять по дороге из стороны в сторону. Он ещё не потерял отслеживание и наблюдает какие-то точки на дороге, но ошибки в алгоритме уже не позволяют точно сопоставить кадры и определить, в какой части дороги находится машина. Это также приводит к нарушению построения карты, что ещё больше усугубляет ситуацию с локализацией, когда с неправильно построенной картой алгоритм пытается сопоставить текущий кадр. Но всё же благодаря инерциальной системе алгоритм дольше не уходит в полный отказ и переживает кратковременные провалы визуального трекинга — правда, с сильным дрифтом.

Когда машина прибывает на кладбище, геометрия благодаря деревьям и постройкам снова возвращается в кадр, отслеживание и карта становятся заметно лучше. Но в момент резкого разворота отслеживание внезапно полностью теряется, и ORB-SLAM 3 приходится полностью перезапустить, а также заново откалибровать. Сложно объяснить причины такого поведения в этой ситуации: хотя ключевых точек на карте было много, по какой-то причине их сопоставление оказалось ненадёжным. Подобные непредсказуемые краши для мобильного робота абсолютно неприемлемы.

После чего начинается уже новый тест с возвращением обратно. Правда, он прерывается: я решаю закомментировать часть мусорных отладочных сообщений, чтобы видеть важные ошибки, о которых сообщает ORB-SLAM 3, после чего запускаю realsense-viewer, где можно увидеть, какое большое количество шума генерирует карта глубины. Например, дорога и облака (на небе) закрашены одним цветом, хотя физически они не могут быть на одной дистанции: облака в километрах и высоко, дорога — в десятках метров. Если бы глубина была честной, облака были бы невалидными (чёрными), а не красными. Значит, алгоритм карты глубины ошибочно считает их валидными и близкими — и это ошибка, а не шум.

После чего тест перезапускается снова уже где-то на бездорожье. Какое-то время отслеживание работает нормально, но в один момент происходит дрифт, и новая траектория машины пересекает старую, хотя машина едет обратно в своей полосе, то есть параллельно старой траектории. Дрифт незначительный и к потере отслеживания не приводит, хотя точность тут явно упала до метров и более.

Запускаю новый тест — режим RGB-D без инерциальных датчиков (только по визуальным данным). Погода заметно ухудшается, и тут же можно заметить постоянные потери отслеживания в режиме RGB-D. ORB-SLAM 3 автоматически обнаруживает потерю трека и стартует заново, но все попытки проваливаются: алгоритм не может начать стабильное отслеживание до самого конца этого видео, где всё же небольшой фрагмент пути он работает.

Тест 2

Начинается уже в солнечный день, без туч. В режиме RGB-D + Inertial по прямой отслеживание ещё более-менее работало, но на повороте случилась потеря отслеживания, после чего алгоритм перезапустился заново. Как можно заметить, новая траектория начинается не параллельно старой, как можно было ожидать, а из какой-то своей стартовой точки. На тайминге 5:38, несмотря на то что машина продолжает ехать прежним курсом, начинается сильный дрейф: большинство ключевых точек переключились на облака в небе, что делает их абсолютно бесполезными, а геометрии, кроме небольших растений и очень дальних ориентиров-столбов, в кадре нет. 6:20 отслеживание балансирует на грани развала, на 7:30 начинается совсем уже очевидный сильный дрейф, где траектории переплетаются друг с другом, хотя реальная траектория должна быть такой:

Ну и возвращаясь обратно, параллельных треков тоже соответственно не видно — только отдельные участки пути соседствуют друг с другом.

Тест 3

Тоже солнечный день, режим RGB-D без IMU. Отслеживание пропадает спустя пару сотен метров на старте. Пытается возобновиться, но тщетно. И по сути всё то же самое происходит всю дорогу на этом видео, поэтому не вижу смысла его комментировать.

Если нести камеру в руках, а не на машине, отслеживание работает заметно точнее, и тут можно показать хорошую картинку, как всё работает:

Но даже при медленном передвижении в руках в сценах, где есть геометрия, случаются потери отслеживания:

Как это произошло тут. Тем не менее в большинстве случаев ORB-SLAM 3 держался и работал.

Что можно сказать по итогу? Режим RGB-D + Inertial показал себя явно лучше, чем обычный чисто визуальный RGB-D. Алгоритм работал на улице, но ненадёжно: были моменты с сильными дрейфами и полной потерей отслеживания, после чего приходилось останавливаться и перекалибровывать его заново. Сказать точно, насколько сильными были отклонения, без наземной истинной траектории от RTK-GPS нельзя, но очевидно, что это не сантиметровая точность, и использовать ORB-SLAM 3 как источник одометрии для уличного наземного робота — плохая идея.

Stella vslam / REVO

Некоторые пакеты не выдерживают самые простые тесты. Как Stella vslam, работающий без IMU в режиме RGB-D, и REVO, строящий одометрию на основе контуров, — они начинают терять отслеживание от совсем небольших скачков картинки. Имхо, тестировать их дальше смысла нет.

REBiVO / DSO / Vins mono

Были также пакеты, которые с трудом тянули работу в realtime на моём мини-ПК с i7: REBiVO, работающий на основе контуров, но использующий также инерциальные данные, а также VI Stereo DSO, тоже использующий данные IMU. Использовать их из-за низкой производительности в таком виде, как их распространяет разработчик, нельзя.

А вот с Vins mono причина другая. По какой-то причине мне никак не удавалось заставить его стабильно работать в realtime: он постоянно жаловался на данные IMU-датчиков или на синхронизацию IMU и камеры, хотя на данных с датасетов работал хорошо. Впрочем, я решил, что в датасетах IMU гораздо лучше, чем Bosch BMI055, встроенный в realsense D455, и синхронизация тоже сделана более качественно, потому не стал его мучить.

Vins RGB-D

Этот пакет довольно стабильно работал в помещении, но на улице начались проблемы:

Я отправился по маршруту, аналогичному тому, что ехал в прошлые разы с ORB-SLAM 3:

Спустя буквально сотню метров Vins RGB-D ушёл в ошибку, и его пришлось перезапускать несколько раз, так как на ходу он ни в какую не хотел перезапускаться. Впрочем, проработал он недолго и через десяток метров опять упал. Тогда я решил найти место, где можно спокойно остановиться, провести более тщательную калибровку в самом начале и медленно двигаться вперёд:

Однако одометрия через сотню метров упала снова на повороте. Я решил сделать контрольный тест, и более эпичного дрифта я ещё не видел: камера, совершив виток вокруг своей оси, пошла летать по всей карте. Идея Vins RGB-D заключалась в том, чтобы совместить преимущества двух режимов работы камеры — RGB-D + Mono. RGB-D должен был быть более устойчив к резким движениям и искать ближние ориентиры, а Mono благодаря его неограниченной стереобазе — искать дальние. Но в условиях улицы это работало не намного лучше.

RtabMap

Пакет RtabMap был самым громоздким и сложным в установке из исходников: он включал десяток библиотек, каждая из которых включала в себя ещё десяток библиотек. Но плюсом сборки из исходников было то, что новейшие версии этих библиотек зачастую лучше оптимизированы и даже включали ускорение на GPU благодаря CUDA. Ранее все пакеты работали исключительно на CPU, и от этого страдала их скорость — например, тот же ORB-SLAM 3 при составлении большой карты мог начать подтормаживать весьма заметно.

Как обычно, я выбрал свой стандартный маршрут:

И с первых минут RtabMap работал весьма неплохо:

RtabMap работал в режиме RGB-D, и хотя он использует также данные от IMU, но только для уточнения вращения камеры, то есть IMU тут используется только как курсовертикаль, а не как ИНС, которая может работать непродолжительное время в режиме dead reckoning. Это делает режим работы RtabMap ближе к чистому RGB-D, как у Stella vslam или ORB-SLAM 3 без Inertial, но, как можно заметить, тут это работало значительно лучше!

В этом моменте я чрезмерно сильно отдалил камеру назад, что вызвало отрисовку всех точек на карте, падение производительности CPU и как следствие падение одометрии. К сожалению, в тот момент я ещё этого не понял и просто хотел сравнить получившийся трек с траекторией движения машины — впрочем, он идеально совпадал.

До момента подъёма в гору трек шёл почти идеально, но на горе в геометрически обеднённой сцене он начал дрифтить аналогично ORB-SLAM 3, а в момент, когда сцена стала практически идеально плоской , одометрия совсем перестала работать и ушла в отказ.

Тогда спустя какое-то время, как я поехал по плоской сцене, где одометрия отказывалась работать совсем, выдавая красный экран ошибки, я перезагрузил RtabMap и продолжил тест, пытаясь замкнуть эту петлю:

И попытка провалилась, но на этот раз не по причине падения одометрии, а из-за попадания в зону рендера старого хвоста из точек карты, что привело к падению производительности CPU-рендера и как следствие падению всей одометрии. В тот момент я уже стал догадываться, что стало причиной падения, и решил стартовать круг ближе к повороту, чтобы уменьшить длину хвоста из отрисованных точек, — и это помогло! Одометрия проехала наконец это проклятое место, и я смог хоть и с трудом замкнуть петлю. Можно заметить, что даже на этом небольшом участке дрейф был огромным, и до включения механизма замыкания петли (то есть узнавания уже пройденной местности по карте) сами показания одометрии увы не замыкались. Но потом карта производит резкий скачёк и замыкает траекторию в петлю.

После чего я решил сделать тест на бездорожье: , где ранее тестировал только ORB-SLAM 3:

И тут RtabMap показала себя даже лучше, чем ORB-SLAM 3: она пережила разворот на дороге на 180 градусов, и более того, на дороге наконец-то видны параллельные треки! Впрочем, несмотря на небольшую их протяжённость, в конце можно заметить, как они уже расходятся, создавая на карте две расходящиеся дороги, хотя дорога-то, конечно, была одна. То есть это снова дрифт, но уже значительно, значительно меньше, чем в ORB-SLAM 3. После чего, добравшись до разворота, я повернул карту, чтобы оценить получившуюся траекторию, и… нет, на реальный маршрут, что был изображён на карте, она увы не похожа. А из-за отрисовки всех точек на карте рендер снова съел всю производительность CPU, и одометрия RtabMap начала падать, в результате RtabMap пришлось перезагрузить.

Мною было принято решение пересобрать RtabMap с новейшими доступными библиотеками и в каждой из них, где это доступно, включить ускорение CUDA, чтобы победить проблемы с производительностью на CPU. Это помогло решить проблему, и я снова отправился по маршруту ORB-SLAM 3:

На этот раз, кстати, ключевые точки SURF на GPU заработали, так как OpenCV тоже был пересобран на CUDA. Они гораздо крупнее и по идее должны значительно лучше работать на улице, чем обычные ORB и иже с ними. Ну в общем-то начало, как всегда с RtabMap, неплохое:

Я немного пошарился в настройках и смог вывести на визуализации базовый вектор гравитации от IMU, благодаря чему смог увидеть, почему при входе в затяжной поворот кадр от камеры начинает наклоняться. Ранее я уже писал, что RtabMap — это слабосвязанная система, где IMU используется только как курсовертикаль; при воздействии на него в затяжном повороте центробежной силы акселерометр начинает складывать её с вектором гравитации и наклонять кадр. Поправить его в этот момент RtabMap не может, как это делают тесносвязанные системы — например, тот же ORB-SLAM 3, который «визуально видит», что наклона дороги в этом месте нет, и вычитает центробежное ускорение. После чего я добрался до подъёма в гору, и естественно на этом месте RtabMap перестала работать; одометрия, впрочем, этот момент был вырезан.

Я решил ещё раз попробовать заехать в горку с одометрией, начав этот маршрут почти с самого начала, но одометрия снова упала на том-же месте. За траекторию я уже молчу: в окне Graph view видно, что она просто постоянно дрейфовала. И в конце концов окно одометрии засветилось красным, что говорит о её падении.

После чего я решил снова сделать тест на замыкание петли в том же самом месте, что и ранее испытывал на CPU:

И… в этот раз одометрия на повороте отказала. Я решил сделать ещё одну попытку, и она снова ушла в отказ. Как впоследствии пояснил МатьеЛаббе, причина была в том, что я выбрал эти самые ключевые точки SURF на GPU, хотя, как мне тогда казалось, если они больше и сложнее, то позволят разгрузить CPU и ещё лучше должны работать. Но практика показала другое: прошлый выбор ключевых точек на CPU был надёжнее, и там петлю всё же удалось замкнуть.

После чего я решил проверить свою теорию с вырождением геометрии. Идея заключалась в том, что именно вырождение геометрии в сцене, а не отсутствие ключевых точек, приводит к падению одометрии. Для этого было необходимо найти такое место, где чередуется вырожденная сцена и сцена с геометрией. И если одометрия будет всегда переставать работать в вырожденной сцене и снова запускаться в сцене, где геометрии много, при этом в обоих случаях в кадре будет присутствовать много ключевых точек, то можно считать, что причина падения именно в геометрии. Сцен с вырожденной геометрией в теории может быть довольно много; главное условие вырожденной сцены — что по мере движения машины текущий кадр практически ничем не отличается от ряда других предыдущих, из-за этого их нельзя объединить в цепочку и как следствие восстановить траекторию движения. Но в общем случае наиболее ярких таких примеров два: длинные тоннели и поля — там каждый новый скан облака точек одинаково похож на весь ряд предыдущих.

И в общем-то с первых минут попытки запуска одометрии в планарной сцене проваливаются, а вот запуск в сцене с геометрий получается с первого раза, и одометрия не мигает ни жёлтым, ни красным цветом. Я действительно хотел следовать плану: проехать лесополосу насквозь, а затем выехать в поле и заехать в новую лесополосу. Но на седане этого сделать увы не получилось: впереди была колея и глубокие лужи на дороге. И пришлось ехать обратно; одометрия при этом продолжала корректно работать, а SLAM замыкать петли, узнавая пройденный путь. Окно с отрисовкой карты я просто закрыл, чтобы оно не тормозило RtabMap, но траекторию можно было видеть в другом окне Graph view — даже лучше. При покидании лесополосы одометрия начала сразу же дрейфовать и вилять вокруг старой траектории (хотя очевидно, что машина не виляет), и, совершив разворот на 90 градусов и начав движение по полю, одометрия падает, при этом ключевые точки присутствуют в кадре. После чего я попытался её перезапустить, но она сразу показывала ненадёжную оценку траектории жёлтым цветом экрана и очень быстро упала. Далее я сделал ещё несколько неудачных попыток её завести, и на этом тест закончился.

Последний тест я сделал во дворе аналогично тестам ORB-SLAM 3:

И RtabMap его прошёл нормально, ничем не хуже ORB-SLAM 3.

Что по итогу можно сказать о RtabMap? Этот пакет показал себя весьма неплохо! Тут всё работает без калибровок с вращением камеры, как в ORB-SLAM 3, одометрия довольно стабильна и падает относительно редко, дрейфует, но меньше, чем ORB-SLAM 3. Однако главный минус — в очень большом размере этого пакета, куче зависимостей от разных библиотек и сложности сборки. Это затрудняет его интеграцию в проект, а разделять ресурсы датчиков без ROS сложно и неудобно. По этой причине я решил, что мне он не подходит.

OpenVins

Тесты этого пакета изначально выглядели очень неплохо:

OpenVins работал очень быстро и выдерживал такую динамику вращений и перемещений, от которой ORB-SLAM 3, например, сразу терял отслеживание:

Помимо этого, у него были уникальные фичи, очень полезные для мобильного робота. Например, ZUPT — калибровка IMU не перемещением камеры, как в предыдущих пакетах, а простой остановкой. Или динамическая инициализация, позволяющая запустить пакет прямо на ходу, что довольно удобно в случае потери отслеживания — не тормозить мобильный робот. Есть функция, использующая Aruco-маркеры как ориентиры для навигации в сценах, где отслеживание нестабильно (условно, расклеить их на стены, где происходят дрифты). А также в тот момент разрабатывался модуль для планарной навигации и новый тип визуальных ориентиров — не точки, а линии, что-то среднее между контурами и точками. Multi-State Constraint Kalman Filter (MSCKF), на котором работал этот пакет, использовался также в марсианском вертолёте Ingenuity. При этом, в отличие от ORB-SLAM 3 или RtabMap, пакет был небольшим и модульным. В общем, это выглядело как самый перспективный пакет для мобильной робототехники. И потому я сразу же приступил к его тестам:

Начало весьма неплохое: OpenVins нигде не упал и даже вернулся в точку старта — физически замкнул маршрут, хотя это и не SLAM-замыкание петли, — правда, с большим дрифтом:

После проверки я решил двигаться дальше, на более дальний маршрут:

И OpenVins, хотя и с невероятным дрифтом, но снова полностью прошёл по этому маршруту и не упал!

Далее я попытался в Rviz отыскать последнюю петлю траектории, которую только что сделал, так как поверить весь маршрут из-за его огромного масштаба не представлялось возможным — траекторию просто не было видно:

Ну и конечно, ничего похожего на петлю там не было. Тогда я решил перезапустить OpenVins, чтобы повторить эту конкретную петлю снова, и на первом же повороте начался дрифт опять.

Что тут по итогу можно сказать за OpenVins? Он сильно дрифтует, впрочем как и все пакеты визуальной навигации, но хотя бы не падает. А также имеет весьма полезные функции и компактный размер, что облегчает его интеграцию в приложение. Я решил добавить его в качестве источника одометрии для построения воксельной карты:

Но карта не сходилась: дрейф был слишком большим, стены дублировались и росли в толщине многократно. Это следствие дрейфа — он был настолько большим, что даже грубую воксельную карту построить не получалось.

Тогда я решил сделать отдельный тест в этом же месте, где стало очевидно:

OpenVins работает, но дрифтует постоянно. ORB-SLAM 3 и RtabMap на этой локации так себя не вели: одометрия хоть как-то, с перебоями, но работала и отдавала честную траекторию. OpenVins хотя показал себя надёжнее в плане крашей, но постоянно врал.

На этом тесты визуальной одометрии закончились, и я решил приобрести лидар, чтобы решить эту проблему с постоянными отказами и дрейфами. И первый же пакет лидарно-инерциальной одометрии с самыми базовыми настройками на улице показал результат на голову выше всех пакетов визуальной одометрии.

Fast-lio 2

Первый тест происходил во дворе, и, несмотря на все манёвры ровера, одометрия не дрейфовала. Я решил сделать тесты на улице. Закрепить лидар на машине и проехать с ним на улице, к сожалению, я не мог, и потому тест был не совсем честным по дистанции. Также я не смог найти настройку, что выводит траекторию движения ровера, как это было с визуальной одометрией, но впрочем, косвенно качество траектории можно оценить по карте — особенно при возвращении назад: если бы произошёл сильный дрейф, карта начинала бы задваиваться:

Тест прошёл очень хорошо: ровер вернулся обратно, и позиция никуда не уплыла. Маршрут был следующим:

Придраться было не к чему: в сравнении с визуальной одометрией — ни к надёжности, ни к точности. Однако если с точностью я догадывался, почему это работает (лидар измеряет дистанции очень далеко, и на любом расстоянии его заявленная точность, в отличие от камеры, должна быть одинаковой, в пределах 1–2 см), то найти пределы надёжности его одометрии было для меня обязательной задачей. Но батареи моего квадроцикла были слабыми и не держали заряд, я не мог далеко уехать, чтобы протестировать его работу в поле. В машину-седан он тоже не помещался, и пришлось просить всех знакомых и друзей, чтобы протестировать аппарат в поле, за что хочу выразить благодарность Владу за помощь, если он это когда-то прочтёт. И благодаря чему такой тест удалось провести:

По итогу теста, как я и подозревал, пределы лидарно-инерциальной одометрии всё же существуют. И этот предел наступает в планарной сцене: как только ровер удалился на значительное расстояние от заброшенного здания и оно вышло из его поля зрения, инерциальная система какое-то время ещё рассчитывала свою позицию автономно, но вскоре разошлась, и начался дрейф. По итогу, когда ровер возвращался обратно, здание задвоилось на карте.

DLIO

По точности и надёжности Fast-lio 2 меня всем устраивал. Более того, пакет был гораздо компактнее любой визуальной одометрии и потреблял гораздо меньше дефицитных ресурсов CPU, что было просто замечательно. Однако довольно поздно, уже после добавления в приложение ровера пакета Fast-lio 2, я заметил, что частота выдачи одометрии у Fast-lio 2 составляла всего 10 Гц, что было очень мало и равнялось частоте сканирования. Это было странно: ведь Fast-lio 2 — это лидарно-инерциальная одометрия. Причина была в том, что измерения IMU накапливались в Fast-lio 2 до момента прихода облака точек, а потом все они быстро прогонялись через фильтр IKFOM, чтобы получить из них измерения одометрии и тут же применить к соответствующим им точкам в лидарном скане, чтобы исправить их искажения от вращения и смещения в момент сканирования. То есть рассчитывать одометрию из показаний IMU Fast-lio 2 мог, но не отдавал их наружу, а копил эти измерения для исправления искажений в облаке точек. Тогда я решил исправить эту ситуацию и дробить скан на части, соответствующие интервалам между измерениями IMU. Скан лидара — это примерно 20 000 точек, за время скана приходит около 20 измерений IMU, значит, скан делится на 20 частей по ~1000 точек, и каждое измерение IMU применяется к своей части точек. И это даже работало дома, но на улице я не успел это протестировать, так как параллельно процессу исправления Fast-lio 2 я начал добавлять в своё приложение пакет, где эта проблема была уже исправлена и сделано лучше, чем у меня.

DLIO тоже копит измерения IMU, но прогоняет их через свой фильтр не в момент прихода следующего скана, а сразу в момент прихода измерения от IMU. То есть если Fast-lio 2 копил именно измерения от IMU, то DLIO копил позиции робота, рассчитанные своим фильтром по данным IMU (их ~20 за скан). А затем интерполировал эти позиции на каждую из 20 000 точек скана — так у каждой точки была своя поза, а не одна на 1000 точек, как в Fast-lio 2. Это делало карты DLIO лучше, без разрывов, а частота одометрии достигала 200 Гц — то есть частоты выдачи измерений от IMU. В VIO вроде OpenVINS частота выхода могла быть такой же высокой, потому что фильтр пропагируется по IMU, а не ждёт кадр. Я был удивлён, как в Fast-lio 2 вообще догадались решить проблему таким способом, пожертвовав частотой:

По итогу DLIO тоже завёлся сразу с дефолтными настройками из коробки и работал на голову лучше пакетов визуальной навигации. Но в сравнении с Fast-lio 2 он потреблял больше ресурсов CPU, причём значительно, из-за nanoGICP. При этом это по-прежнему было намного меньше любой визуальной навигации, и так как всё работало на этом пакете, я и остановился.

Итоги

Если свести всё вместе, картина получилась довольно однозначной.

Визуальная одометрия на улице — пока не готова. Ни один из протестированных пакетов не дал приемлемой для мобильного робота надёжности:

• ORB-SLAM 3 — самый качественный, когда работает, но именно на улице он ведёт себя непредсказуемо: то дрейфует, то полностью теряет отслеживание, то крашится на ровном месте. Режим RGB-D + Inertial заметно лучше чистого RGB-D, но всё равно не тянет на источник одометрии для уличного робота.

• Stella vslam и REVO отваливаются на простых тестах — от небольших скачков картинки.

• REBiVO и VI Stereo DSO не тянут realtime на моём железе, а Vins mono так и не удалось стабильно запустить на данных реального сенсора.

• Vins RGB-D в помещении стабилен, на улице — падения через каждую сотню метров.

• RtabMap — приятное открытие: стабильнее ORB-SLAM 3, меньше дрейфует, не требует калибровки с вращением камеры, умеет замыкать петли. Но пакет огромный, с кучей зависимостей, тяжёлый в сборке и интеграции — для проекта без ROS это боль. Плюс выяснилось, что ключевые точки SURF на GPU сделали хуже, а главная причина отказов — вырождение геометрии в сцене, а не нехватка ключевых точек.

• OpenVins — быстрый, живучий, с полезными фичами, но дрифтует постоянно и настолько, что по нему не строится даже грубая воксельная карта.

Лидарно-инерциальная одометрия — качественно другой уровень. Первый же пакет с дефолтными настройками на улице оказался на голову выше всех визуальных:

• Fast-lio 2 — точность и надёжность, к которым не придраться, компактность и низкая нагрузка на CPU. Единственный минус — выход одометрии всего 10 Гц.

• DLIO — та же надёжность, но с нормальной частотой до 200 Гц, линейной интерполяцией между измерениями IMU и более качественными картами. Платит за это заметно большим потреблением CPU из-за nanoGICP, но всё равно это намного меньше, чем у любой визуальной навигации.

Главный вывод. Для уличного наземного мобильного робота визуальная одометрия в текущем виде — источник постоянных отказов и дрейфов. Фундаментальная проблема — вырожденная геометрия сцены и повторяющиеся текстуры (поля, длинные тоннели, однообразная растительность). Я ожидал, что визуальная одометрия за счёт работы по текстурам, а не только по геометрии, будет устойчивее в таких сценах, — оказалось наоборот: там, где текстуры однообразны и повторяются, feature matching видит движение пикселей, но не может однозначно привязать его к месту, потому что патч травы в текущем кадре похож на патч травы в предыдущем в разных местах. Это не «отсутствие геометрии», а aliasing/повторяемость текстуры. Лидар в этой ситуации страдает по схожей причине, но у него есть дальность и 3D-структура: даже редкие кусты или столбы на горизонте дают ему точки, которых нет у камеры. Лидарно-инерциальная одометрия решает проблемы дрейфа гораздо лучше, но и у неё есть предел — планарные сцены, где лидар теряет ориентиры, а ИНС начинает расходиться. Из тестов следует, что ограничения лидарной и визуальной одометрии по принципу похожи: и лидарная, и визуальная одометрия в основе имеют оценку движения по сопоставлению наблюдений. Лидар сопоставляет облака точек (ICP и его варианты), камера — признаки и ошибку репроекции. В вырожденной сцене оба метода получают слабый и неоднозначный сигнал о продольном смещении, ошибка накапливается быстро, и дрейф становится неприемлемым. Лидар при этом получает дистанцию аппаратно — через время полёта света, — а камера оценивает её триангуляцией по признакам (в RGB-D глубина приходит уже готовой, но получена она той же триангуляцией на стереопаре), что вносит дополнительную погрешность. Плюс ограниченное поле зрения: около 90° у камеры против 360° у лидара. В уличных сценах, которые я тестировал, визуальная одометрия проиграла лидарной по надёжности, и её преимущества (дешевизна, масса) не компенсировали этого. Для моей задачи она непригодна.