По результатам тестов из прошлой статьи выяснилось: из-за дрейфа одометрии (в основном по вертикальной оси, оси Z, у лидарной одометрии), шума датчика глубины и ограничений углов обзора датчиков...

По результатам тестов из прошлой статьи выяснилось: из-за дрейфа одометрии (в основном по вертикальной оси, оси Z, у лидарной одометрии), шума датчика глубины и ограничений углов обзора датчиков простой наивный подход — прямое проецирование точек с камеры на карту — не работает. Шум глубины создаёт на карте артефакты, а скачки и дрейф одометрии усугубляют проблему. Чтобы её исправить, я решил позаимствовать CUDA-ядра обработки точек из пакета Elevation Mapping cupy, где эти проблемы были решены с помощью одномерного фильтра Калмана для каждой ячейки, накапливающего измерения датчика в ячейках карты по времени, и алгоритма компенсации дрейфа. В общем виде принцип работы ядер Elevation Mapping cupy (EMC) показан на рисунке; насколько это возможно, он был точно перенесён на CUDA:

Перед тестом нового визуализатора меша я ещё раз запустил HD-карту из прошлой статьи в помещении:

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

В EMC эта проблема решена проще и изящнее. В моём случае робот ищет поверхность, по которой движется, отсчитывает от неё высоту, ищет в этом столбике препятствия и оставляет на карте высот только самое высокое. Это требует обязательного наличия поверхности под роботом для удаления потолка. В EMC сделано проще, над высотой датчика, допустим 1 м, удаляются все точки, включая потолки. Если подобрать эту высоту точно под высоту робота, он сможет проходить под препятствиями выше него (типа стола) точно так-же. Но это позволяет EMC не зависеть от поверхности, что делает алгоритм надёжнее.

Платой за простоту является ограниченный обзор при заезде на подъём: верхняя планка поля зрения робота буквально упирается в горку и срезает её вершину. Где подход с приоритетным обходом слоёв из HD-карты будет следовать по поверхности подъёма выше в горку. Впрочем в реальности из-за ограниченного поля зрения робота этот не всегда работает ведь вершина может быть банально не от сканирована или в низком разрешении. Однако если облако, загружаемое в EMC, будет не зарегистрированным, то есть находиться в системе координат (СК) корпуса робота, то когда робот заедет на подъём, его вершина перестанет обрезаться. Что-бы имитировать этот эффект можно привязать зону обрезки к углам тангажа и крена робота, чтобы при подъёме в гору зона обрезки огибала рельеф. Это частично компенсирует проблему слепоты робота на подъёме, а учитывая простоту и надёжность подхода, такой компромисс можно назвать лучшим.

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

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

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

На этом тест визуализатора карты высот был завершён, и я приступил к замене CUDA-ядер построения воксельной карты на ядра EMC.

Поскольку ядра добавлялись постепенно, шаг за шагом, первый раз карта была запущена на одном лидаре. Его отрицательные углы склонения не позволяют видеть пол, а ключевые кадры обновляются очень медленно, поэтому карта была построена по обычным кадрам, очищенным от искажений, а ровер приходилось носить приподнятым на 45 градусов вверх за задний мост. Но таким образом удалось построить лидарную карту помещения в EMC: