Введение
В рамках своей научной работы в области роботехники я занимаюсь разработкой собственной SLAM-системы (Simultaneous Localization and Mapping), реализованной полностью на языке Python. Этот проект представляет собой попытку осмысленно пройти весь путь построения визуального SLAM — от базовых идей и архитектурных решений до рабочей, пусть и экспериментальной, системы, без опоры на готовые компилируемые C++-фреймворки.
Проект находится в стадии активной разработки и на данный момент является, скорее, промежуточным итогом проведённого исследования. Этой статьёй я хочу поделиться своим опытом разработки такой системы, рассказать о принятых решениях, возникавших сложностях и достигнутых результатах, а также получить обратную связь от сообщества. Может кто подскажет что то дельное.
Концепция
Изначально я не планировал разрабатывать собственную SLAM-систему с нуля. Моей первой идеей было использовать уже существующие решения, такие как RTAB-Map, ORB-SLAM3 или pySLAM, и встроить их в более общий контекст задач мобильной робототехники. Я последовательно пробовал разные фреймворки, пытался развернуть их, запустить на собственных данных и адаптировать под нужный сценарий. На практике это довольно быстро привело к большому количеству трудностей — начиная от сложного процесса установки и сборки, заканчивая высокой сложностью внутренней архитектуры и документации. На мой взгляд высокий порог въождения.
Даже несмотря на то, что подобные системы объективно более зрелые и качественные с точки зрения алгоритмов и производительности, их интеграция в более сложные программные комплексы оказывается нетривиальной задачей.
Было принято решение реализовать собственную SLAM-систему — упрощённую, во многом повторяющую классические подходы, но при этом полностью контролируемую и расширяемую.
Классические SLAM-системы, как правило, строятся вокруг графовой оптимизации с использованием библиотек вроде g2o (и другие). Это проверенный и эффективный подход, однако на данный момент он показался мне концептуально несколько устаревшим.
Субъективно мне кажется, что будущее оптимизации в SLAM связано с тензорными вычислениями и активным использованием GPU.
С архитектурной точки зрения система была разделена на несколько независимых подмодулей. Основной поток обработки включает инициализацию карты и основной трекинг кадров. Параллельно с ним предполагается работа локальной оптимизации и глобальной оптимизации, которые не блокируют основной процесс и не ожидают его завершения. Карта при этом постоянно пополняется, локально уточняется и, в перспективе, глобально оптимизируется с учётом замыканий петель.
В качестве основного языка разработки используется Python, для оптимизации — PyTorch, а для визуализации структуры карты и траектории движения камеры — Open3D.
Долго не буду расписывать как и что делал, лучше сразу обартиться к исходному коду. Если кратко то сначала обработал первые 2 кадра, получил матчи, затем триангулляция, формирвоание первоначальной карты. Потом основной трекинг с помощью PnP определиял положение камеры последующих кадров, когда выполнялось условие на добавление кадра выполнял матчинг, триангуляцию и наполнял карту. Параллельно выполнялась локальная оптимизация. До глоальной оптимизации руки покаа что не дошли.
Результаты
В качестве первичного эксперимента я использовал данные из датасета KITTI, ограничив последовательность всего двадцатью кадрами. Такое ограничение было сделано сознательно, чтобы упростить анализ поведения системы и визуально отслеживать, на каком этапе начинают проявляться ошибки и нестабильность алгоритма.
На этапе инициализации система ведёт себя достаточно стабильно. Корректно формируется начальное облако 3D-точек, и визуально хорошо видно, что структура сцены соответствует изображению: скопления точек располагаются примерно в тех местах, где на кадре присутствуют реальные объекты. Репроекционная ошибка на этом этапе относительно небольшая, и трекинг камеры выглядит согласованным.
Однако по мере добавления новых кадров и быстрого накопления 3D-точек ситуация начинает заметно ухудшаться. Я предполагаю, что основная причина здесь связана с качеством оптимизации карты. Либо в текущей реализации допущены ошибки в самой постановке задачи оптимизации, либо оптимизация выполняется недостаточно эффективно и не успевает компенсировать накопленные искажения. Не исключаю и того, что использование PyTorch для данной задачи в текущем виде может быть не самым удачным решением — по крайней мере без более аккуратной постановки вычислений и контроля численной стабильности.
На визуализациях хорошо видно текущее состояние системы. На верхних изображениях показаны отфильтрованные матчинги между кадрами: каждая 2D-точка на изображении соответствует своей 3D-точке в пространстве. Это позволяет интуитивно оценить, насколько корректно система связывает наблюдения с элементами карты. Ниже представлены облака точек и их проекции в различных плоскостях, что делает структуру сцены более наглядной и позволяет лучше понять геометрию восстановленного пространства.
В 3d облако точек выглядит лучше чем на картинка :)
Несмотря на нестабильность при длительном трекинге, сам факт формирования связанного облака 3D-точек и согласованной карты уже можно рассматривать как результат. После завершения работы алгоритма к этой карте теоретически можно обращаться для решения прикладных задач — например, для построения маршрутов, анализа сцены, классификации объектов или дальнейшей обработки в рамках более сложной системы. На данном этапе это скорее демонстрация потенциала подхода.
Итоги
В текущей реализации удалось собрать базовый пайплайн: инициализацию карты, основной трекинг, локальную оптимизацию, а также работу с картой и её визуализацию. Это позволило получить рабочий прототип и на практике увидеть, как ведёт себя визуальный SLAM при накоплении данных.
При этом многие компоненты требуют серьёзной доработки. В первую очередь это касается трекинга и локальной оптимизации — их необходимо перепроверить, возможно, реализовать альтернативные варианты и лучше контролировать накопление ошибок. Отдельной проблемой остаётся масштабирование: при росте числа точек система заметно замедляется, особенно на этапе оптимизации. Это ожидаемо для Python-реализации, но явно указывает на узкие места в текущем подходе.
В дальнейшем я планирую глубже разобраться с оптимизацией — либо дорабатывать реализацию на PyTorch, либо рассмотреть использование классических графовых оптимизаторов, таких как g2o. Кроме того, в системе пока отсутствуют глобальная оптимизация и замыкание петель.
Главным преимуществом текущего решения на мой взгляд это простота развёртывания: отсутствие сложной компиляции и внешних зависимостей позволяет быстро менять код и проверять идеи. Однако за это приходится платить производительностью. Я считаю, что эти проблемы решаемы, но требуют более детальной и аккуратной проработки.
Буду рад комментариям и конструктивной критике — они помогут мне скорректировать дальнейшее развитие проекта и избежать возможных концептуальных ошибок.
Для связи, советов и критики
Буду рад ответить на ваши вопросы или замечания.
-
Имя: Anton
-
Email: anton42@yandex.ru
-
Telegram: antonSHBK
Автор: anton_shbk
