По результатам тестов из прошлой статьи выяснилось: из-за дрейфа одометрии (в основном по вертикальной оси, оси Z, у лидарной одометрии), шума датчика глубины и ограничений углов обзора датчиков простой наивный подход — прямое проецирование точек с камеры на карту — не работает. Шум глубины создаёт на карте артефакты, а скачки и дрейф одометрии усугубляют проблему. Чтобы её исправить, я решил позаимствовать CUDA-ядра обработки точек из пакета Elevation Mapping cupy, где эти проблемы были решены с помощью одномерного фильтра Калмана для каждой ячейки, накапливающего измерения датчика в ячейках карты по времени, и алгоритма компенсации дрейфа. В общем виде принцип работы ядер Elevation Mapping cupy (EMC) показан на рисунке; насколько это возможно, он был точно перенесён на CUDA:
Первый запуск HD-карты в помещении
Перед тестом нового визуализатора меша я ещё раз запустил HD-карту из прошлой статьи в помещении:
На видео карта высот строится по данным лидара и камеры: лидар в основном направлен на стены и поток, а камера оцифровывает поверхность пола. На лидарной карте, где облако точек окрашено по интенсивности, видно, что лидар видит всю комнату, включая потолок; недоступной остаётся лишь небольшая мёртвая зона — воронка на потолке. Поначалу карта помещения строится нормально, не считая багов с размером стен. Но после начала движения возникает новый баг, связанный с функцией поиска высоты робота (приоритетного слоя, по которому он движется): удаление потолков даёт сбой, и карта высот перекрывается потолком. Правда, тогда я не сразу это понял, но это ещё раз подчёркивает, что простые наивные подходы как этот, в котором под роботом (позицией сенсора) ищется поверхность для удаления над ней потолка плохо работает, ведь иногда поверхности может и не быть, что и приводит к сбоям.
Как эта проблема решена в EMC
В EMC эта проблема решена проще и изящнее. В моём случае робот ищет поверхность, по которой движется, отсчитывает от неё высоту, ищет в этом столбике препятствия и оставляет на карте высот только самое высокое. Это требует обязательного наличия поверхности под роботом для удаления потолка. В EMC сделано проще, над высотой датчика, допустим 1 м, удаляются все точки, включая потолки. Если подобрать эту высоту точно под высоту робота, он сможет проходить под препятствиями выше него (типа стола) точно так-же. Но это позволяет EMC не зависеть от поверхности, что делает алгоритм надёжнее.
Платой за простоту является ограниченный обзор при заезде на подъём: верхняя планка поля зрения робота буквально упирается в горку и срезает её вершину. Где подход с приоритетным обходом слоёв из HD-карты будет следовать по поверхности подъёма выше в горку. Впрочем в реальности из-за ограниченного поля зрения робота этот не всегда работает ведь вершина может быть банально не от сканирована или в низком разрешении. Однако если облако, загружаемое в EMC, будет не зарегистрированным, то есть находиться в системе координат (СК) корпуса робота, то когда робот заедет на подъём, его вершина перестанет обрезаться. Что-бы имитировать этот эффект можно привязать зону обрезки к углам тангажа и крена робота, чтобы при подъёме в гору зона обрезки огибала рельеф. Это частично компенсирует проблему слепоты робота на подъёме, а учитывая простоту и надёжность подхода, такой компромисс можно назвать лучшим.
Наблюдения по ходу видео
Дальнейший просмотр можно начать отсюда, что-бы пропустить попытки разобраться в причинах неправильной работы карты.
И тут можно заметить как ведёт себя меш на сыром облаке точек от камеры без фильтрации: все треугольники постоянно двигаются из-за шума измерений, а карта стоимости из-за этого тоже не является идеально гладкой. Тем не менее в этом шуме ровер хорошо видит предмет, если бросить его прямо под колёса: такие пиксели на карте стоимости окрашиваются зелёным, жёлтым или пурпурным, если высота предмета слишком велика, чтобы его переехать. Ну и на меш-карте его тоже хорошо видно как бугорок.
Так как амплитуда шума здесь не слишком велика по сравнению с размером предмета, на карте также видно, как я перемещаю предмет по комнате. Таким образом, в пределах одного скана карта работала, и главной причиной артефактов был именно дрейф одометрии по оси Z, который наивное проецирование точек на карту никак не компенсировало. Далее такой же тест с предметами покрупнее проводился уже с включённой картой стоимости, и карта правильно визуализировала предмет как непроходимую зону.
Замена CUDA-ядер на ядра EMC
На этом тест визуализатора карты высот был завершён, и я приступил к замене CUDA-ядер построения воксельной карты на ядра EMC.
Поскольку ядра добавлялись постепенно, шаг за шагом, первый раз карта была запущена на одном лидаре. Его отрицательные углы склонения не позволяют видеть пол, а ключевые кадры обновляются очень медленно, поэтому карта была построена по обычным кадрам, очищенным от искажений, а ровер приходилось носить приподнятым на 45 градусов вверх за задний мост. Но таким образом удалось построить лидарную карту помещения в EMC:
Как можно заметить, лидарная карта построена довольно неплохо: несмотря на то, что облако не очищено от движущихся точек, на карте почти нет артефактов и выбросов! Однако визуализация ещё не до конца исправлена, поэтому модель ровера ездит боком).
Объединённое облако лидара и камеры
Позднее я добавил объединённое облако точек лидара и камеры. Здесь выбросов на карте стало значительно больше даже в помещении, хотя карта стоимости выглядит почти идеально:
Но реальная причина в том, что в этом тесте чувствительность уклонов карты стоимости сильно занижена: с такой настройкой она даже человека, стоящего перед ровером, не считает значительным препятствием.
На карте появилось большое количество выбросов: их хорошо видно как точки за пределами карты помещения. Это тоже вина RGB-D камеры — даже с фильтрацией и в условиях помещения она продолжает генерировать мусор, с которым фильтры EMC также не справляются. Эти выбросы также нарушают раскраску карты высот, так как растягивают её высоту значительно сильнее, чем она есть на самом деле, как пример: окрас карты, построенной одним лидаром, значительно лучше, ведь на ней нет выбросов:
Финальный тест ядер EMC на улице
Ну и финальный тест ядер EMC на улице:
Погода идеальная, солнечный день, и в целом карта построена неплохо: артефактов стало где-то раза в два меньше, и по этой карте уже даже можно проехать! Но всё же с фильтром Калмона карта построенная RGB-D камерой не стала идеальной гладкой, артефакты тоже периодически встречаются, создавая на карте фантомные препятствия, перекрывающие дорогу. Часть из них оставлена мной, так как карта по-прежнему строилась не на keyframe’ах, очищенных от движущихся точек, а на обычных лидарных сканах.
Интересно, что сами ETH Zürich делают пакет карты высот уже много лет:
И подобные проблемы с выбросами на карте там, где их в принципе не должно быть, бывали и у них:
Выводы
По итогу можно сделать вывод: для качественного построения карты проходимости RGB-D камера подходит плохо, нужны лидары, и облако точек лидара должно проходить обработку в пакете одометрии для отделения движущихся и неподвижных точек в реальном времени, чтобы не зависеть от низкой частоты keyframe’ов и получать облако точек аналогичного качества.
Источник: habr.com