Локализация/картографирование, выбор SLAM метода для проекта.

Присоединяйтесь для обсуждения, и отслеживайте прогресс нашего проекта в Телеграм - https://t.me/it_s_working.

Введение

В данной статье мы бы хотели обсудить задачу локализации и построения карт.

В рамках реализуемого проекта мы предполагаем использование квадрокоптеров, перемещающихся внутри помещений. Для обеспечения универсальности нашего решения мы исходим из того, что изначально у нас не будет карты территории, на которой будет выполнять работу дрон. Мы предполагаем, что в течение времени территория может меняться: объекты могут менять расположение, могут появляться новые препятствия и т.д.
Из этого можно сформировать следующие требования:

• построение карты неизвестной территории по мере передвижений робота;
• динамическое обновление карты;
• определение роботом его положения в пространстве.

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

Описанный процесс очень похож на то, с чем обычно сталкиваются роботы/дроны при решении схожих задач. Только вместо мозга у робота — бортовой компьютер, вместо глаз — камера или лидар. У людей также как и у многих роботов есть «встроенный гироскоп», который помогает нам определять наше положение в пространстве.

Для решения спектра описанных выше задач в робототехнике прибегают к локализации и картографированию.

Локализация — процесс отслеживания положения объекта в пространстве. Знания о местонахождении объектов и самого робота являются основой:

• успешного физического взаимодействия;
• планирования/прокладывания маршрута.

Локализации различаются:

• в зависимости от плоскостей, в которых предполагается отслеживание положения робота:
• 2D — например, для колёсных роботов или плавающих надводных, передвигающихся по «плоскости»;
• 3D — для роботов, которые могут перемещаться также и по высоте (например, летающие дроны);
• в зависимости от среды, в которой работает робот:
• внутри помещений;
• на открытом пространстве;
• в динамически меняющейся среде.

Картографирование — это, собственно, построение карты местности и её обновление с учётом наблюдений/анализа окружения.

Глоссарий

Термины которые будут использоваться дальше и которые помогут лучше понять основные идеи

Visual Simultaneous Localization and Mapping (SLAM) systems — одновременная локализация и построение карты, метод, используемый в автономных средствах для построения карты в неизвестном пространстве, а также для обновления карты в заранее известном пространстве с одновременным контролем текущего местоположения и пройденного пути.

Visual Odometry (VO) — метод оценки положения и ориентации робота или иного устройства с помощью анализа последовательности изображений, снятых установленной на нём камерой.

Maximum-a-Posteriori (MAP) — обоснованный подход к построению картины окружающего мира робота, способ дать наиболее вероятную оценку обстановки, основываясь на двух параметрах:

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

Bundle Adjustment (BA) — алгоритм, который улучшает 3D-реконструкцию объектов и корректирует положение камер, с которых были сделаны снимки. Он работает как система проверки: сравнивает, где на фото должны быть точки (например, угол объекта) и где они на самом деле видны, а затем подстраивает модель («карту» 3D-точек) и параметры камер (направление, фокусное расстояние…), чтобы минимизировать ошибки.

Inertial Measurement Unit (IMU) — гироскопическое устройство, предназначенное для стабилизации отдельных предметов или приборов, а также для определения угловых отклонений предметов.

SLAM

Мы начнем наше знакомство с областью локализации и картографии с метода SLAM. Данный метод показывает хорошие результаты и используется в широком спектре сценариев:

• беспилотные автомобили;
• автономные летательные аппараты;
• подводные аппараты и прочие.

Когда SLAM выполняется с использованием только одной камеры (монокулярной камеры), этот метод называется Монокулярным SLAM. Использование одной камеры выгодно, поскольку монокулярные камеры обычно недороги и компактны, предлагая большую гибкость в сравнении с такими датчиками, как камеры глубины (depth cameras) или стереокамеры, у которых ограниченный диапазон действия.

Однако, основной вызов в Монокулярном SLAM связан с присущей ему неопределенностью масштаба — одна камера не может напрямую измерить абсолютное расстояние до объектов. Все в мире кажется масштабированным относительно движения камеры. Это означает, что истинный размер окружения и движение камеры известны только с точностью до неизвестного масштабного фактора. Со временем это может привести к дрейфу масштаба — когда оцениваемый масштаб окружения постепенно становится неточным.

Существуют различные способы обработки изображений алгоритмами SLAM для оценки движения и структуры.

Методы на основе признаков (Feature-Based):

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

Прямые методы (Direct Methods):

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

Рассматриваемые в данной статье методы:

• ORB-SLAM3;
• LSD-SLAM;

ORB-SLAM3

Одна из наиболее популярных в настоящее время реализаций SLAM — ​​ORB-SLAM3. Это первая система, способная выполнять визуальный, визуально-инерциальный и многокартографический SLAM с монокулярными, стерео и RGB-D камерами.

Визуально-инерциальный метод — метод оценки положения и ориентации робота или иного устройства с помощью анализа последовательности изображений, снятых установленной на нём камерой (или камерами) в комбинации с измерительными блоками (IMU) сбора данных о движении робота и его окружении.

Отличительные особенности данной реализации:

• тесно интегрированная визуально-инерциальная система SLAM на основе признаков, которая полностью полагается на оценку MAP, даже во время фазы инициализации IMU.
• система множественных карт, которая опирается на новый метод распознавания мест с улучшенной памятью. Благодаря этому метод ORB-SLAM3 способен «выживать» в длительные периоды плохой визуальной информации: когда он «теряется», то начинает новую карту, которая будет плавно объединена с предыдущими картами при повторном посещении картографированных областей. По сравнению с визуальными системами одометрии, которые используют только информацию за последние несколько секунд, ORB-SLAM3 — первая система, способная повторно использовать на всех этапах алгоритма всю предыдущую информацию.

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

1. Краткосрочная ассоциация данных — сопоставление элементов карты, полученных за последние несколько секунд. Это единственный тип ассоциации данных, используемый большинством систем VO, «забывающих» элементы среды, как только те выходят из их поля зрения. Это приводит к постоянному дрейфу оценок, даже когда система перемещается в одной и той же области.
2. Среднесрочная ассоциация данных — сопоставление близких к камере элементов карты, накопленный дрейф которой ещё мал. Эти элементы можно сопоставлять и использовать в BA таким же образом, как и краткосрочные наблюдения, и они позволяют достичь нулевого дрейфа, когда система перемещается в картографированной области. Сопоставление таких элементов является ключом к лучшей точности системы на маршруте по сравнению, например, с системами VO, опирающимися лишь на обнаружение петель (обнаружение или замыкание петли — событие, когда робот понимает, что вернулся точно туда, где уже был, и использует это понимание для коррекции своих данных).
3. Долгосрочная ассоциация данных — помогает системе «вспомнить» ранее посещённые места и использовать эту информацию для улучшения карты и точности навигации:
4. Обнаружение (замыкание) петли: элементы наблюдений сопоставляются с элементами ранее посещенных областей и при этом используется метод распознавания, не зависящий от накопленного дрейфа. Это позволяет исправить накопившиеся неточности в местоположении.
5. Объединение карт: при обнаружении системой соответствия текущей локации и локации нанесённой на карту ранее (даже если в другой части маршрута), новая и старая карты объединяются, создавая более согласованную картину мира.
6. Восстановление после потери отслеживания: после временной утраты системой ориентации (например, из-за плохих данных с датчиков), долгосрочная ассоциация помогает восстановить правильное положение и скорректировать карту.
   
   Долгосрочная ассоциация данных позволяет сбросить дрейф и исправить карту с помощью оптимизации графа поз (Pose Graph, PG) или, точнее, с помощью BA. Это ключ точности SLAM в средних и больших средах.

Что такое Pose Graph или граф поз?

Представьте, что вы рисуете карту, двигаясь по комнате. Каждый шаг или поворот — это «поза» (положение и направление взгляда). В Pose Graph каждая такая поза — это узел графа, а рёбра между узлами показывают, как они связаны (например, «от позы A до позы B 2 шага вперёд и поворот на 30 градусов»).

Зачем это нужно?

Когда робот или дрон (а точнее — камера) перемещается, небольшие ошибки в измерениях (например, из-за скользкого пола или шума, погрешностей датчиков) накапливаются, и карта «расползается». Это и называется дрейфом. Pose Graph Optimization (оптимизация графа поз) — это способ «подогнать» все позы так, чтобы они согласовывались друг с другом, уменьшая ошибки.

Принцип работы

Atlas — это многократное представление, состоящее из набора несвязанных карт. Есть активная карта, где поток отслеживания локализует входящие кадры, постоянно оптимизируется и расширяется новыми ключевыми кадрами — локальным потоком картирования. Другие карты в Атласе называются неактивными картами. Система создает уникальную базу данных ключевых кадров DBoW2, которая используется для перераспределения, замыкания цикла и слияния карт.

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

Local Mapping — локальный поток сопоставления. Он добавляет ключевые кадры и точки к активной карте, удаляет избыточные и уточняет карту с помощью визуальной или визуально-инерциальной корректировки, работая в локальном окне ключевых кадров, близком к текущему кадру. Кроме того, в инерциальном случае параметры IMU инициализируются и уточняются потоком сопоставления с использованием новой техники оценки MAP.

Loop and map merging — поток объединения циклов и карт обнаруживает общие области между активной картой и всем Атласом с частотой ключевых кадров. Если общая область принадлежит активной карте, она выполняет коррекцию цикла, а если она принадлежит другой карте, то обе карты плавно объединяются в одну, которая становится активной картой. После коррекции цикла запускается полный BA в независимом потоке для дальнейшего уточнения карты, не влияя на производительность в реальном времени.

LSD-SLAM

Large-Scale Direct Monocular SLAM

LSD-SLAM предназначен для построения крупномасштабных, согласованных карт окружения. LSD-SLAM работает в реальном времени на центральном процессоре (CPU).

Принцип работы

Алгоритм работы состоит из трёх основных компонентов: отслеживание (tracking), оценка карты глубины (depth map estimation) и оптимизация карты (map optimization).

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

Отличительные особенности реализации LSD-SLAM:

• Отслеживание в реальном времени (Real-time Tracking):
  система непрерывно оценивает движение камеры по поступающим изображениям. Для расчета движения камеры она использует прямое выравнивание изображений, сравнивая интенсивности пикселей нового изображения с эталонным изображением, называемым ключевым кадром — keyframe (KF). Этот процесс явно учитывает меняющуюся неопределенность оценок глубины для разных пикселей.
• Полуплотная оценка карты глубины (Semi-Dense Depth Map Estimation):
  вместо того, чтобы пытаться найти глубину для каждого отдельного пикселя (плотное картирование), LSD-SLAM фокусируется на вычислении глубины для пикселей в областях изображения с достаточно сильными градиентами интенсивности (областях, где цвет или яркость быстро меняются — например, края). Это приводит к полуплотным картам глубины, связанным с KF. Глубина для этих пикселей уточняется со временем путем выполнения множества сравнений с близлежащими отслеживаемыми кадрами, используя вероятностный подход фильтрации. Система также отслеживает неопределённость или дисперсию этих оценок глубины. Карта глубины и дисперсия определены только для подмножества пикселей с достаточно большим градиентом интенсивности, отсюда и «полуплотность».
• Ключевые Кадры (Keyframes, KF):
  это выбранные важные изображения, которые представляют окружение и используются для построения глобальной карты. Новый ключевой кадр создается, когда камера достаточно далеко отошла от текущего эталонного ключевого кадра. Каждый ключевой кадр масштабируется таким образом, чтобы его средняя «обратная глубина» (inverse depth), которая связана с тем, как далеко находятся объекты, равнялась единице.
• Представление карты и оптимизация графа поз (Pose Graph):
  глобальная карта в LSD-SLAM как и в ORB-SLAM3 представлена в виде графа поз (Pose Graph). Для поддержания согласованности на больших территориях и коррекции ошибок, таких, например, как дрейф масштаба, в LSD-SLAM граф поз аналогичным образом постоянно оптимизируется.
• Обработка дрейфа масштаба (Sim Alignment):
  чтобы явно справляться с неопределенностью масштаба и дрейфом, присущим монокулярному SLAM, LSD-SLAM использует метод выравнивания ключевых кадров с использованием преобразования подобия (similarity transform, Sim) вместо просто преобразования жесткого тела. Преобразование подобия включает вращение, перенос и масштабирование. Оценивая преобразование между ключевыми кадрами в Sim, система может учитывать разницу в масштабе между ними и обнаруживать накопленный дрейф масштаба, особенно при повторном посещении ранее нанесенных на карту областей (замыкания петель — loop closures). Это масштабно-осведомлённое выравнивание использует как фотометрическую информацию (интенсивность изображения), так и информацию о глубине из ключевых кадров.
• Замыкание петель (Loop Closure Detection):
  распознавание того, когда камера вернулась в ранее посещённое место (замыкание петли), имеет решающее значение для коррекции накопленных ошибок и обеспечения согласованности карты. LSD-SLAM обнаруживает потенциальные замыкания петель и затем проверяет преобразование между текущим ключевым кадром и потенциальным ключевым кадром замыкания петли, используя прямое выравнивание изображений в Sim. Успешные замыкания петель добавляют ограничения в граф поз, которые затем используются при оптимизации для коррекции карты.

Учёт неопределенности (Incorporating Uncertainty):
LSD-SLAM также учитывает шум и неопределённость в оценках глубины и включает это в процесс отслеживания. Этот вероятностный подход делает отслеживание более надежным, особенно для монокулярного зрения, где шум глубины значительно варьируется по изображению.

Таким образом, LSD-SLAM предлагает метод для построения крупномасштабных, согласованных 3D карт, используя только одну камеру, путём прямого использования интенсивностей изображения и явной обработки неопределённости масштаба. Полученная карта, представленная в виде облака точек из полуплотных карт глубины, обеспечивает детальную 3D реконструкцию окружения.

Сравнение наиболее представительных визуальных (вверху) и визуально-инерциальных (внизу) систем в хронологическом порядке

Заключение

Оба описанных метода (ORB-SLAM3 и LSD-SLAM) в теории демонстрируют хорошие результаты.
В своём проекте мы планируем использовать монокулярный подход с использованием RGB-камеры. Метод локализации/картографирования планово будет исполняться на бортовом компьютере Raspberry PI 5.

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

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

Список использованных источников

1. https://jakobengel.github.io/pdf/engel14eccv.pdf [LSD SLAM]
2. https://arxiv.org/abs/2007.11898 [ORB SLAM3]
3. https://en.wikipedia.org/wiki/List_of_SLAM_methods
4. https://moluch.ru/archive/213/51977/