Что такое SLAM?

SLAM – акроним для Simultaneous Location and Mapping

Эта заметка – небольшая памятка на тему что такое SLAM. Здесь описаны основные принципы наиболее популярных методов SLAM (EKF, iSam, TORO и др). В отдельном разделе интересующиеся могут найти ссылки на свободные(!) реализации различных методов SLAM в виде готовых библиотек. А также перечислены блоги и проекты посвященные задаче SLAM. Если вы ищите готовую реализацию SLAM – собрал->запустил->получил карту => листайте к последнему разделу.

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

Представьте себе робота, оснащенного некоторым набором сенсоров (например обычной и дальномерной камерой) находящегося в некоторой неизвестной среде – например лабиринте Минотавра. Робот не обладает информацией об окружающем его пространстве равно как и своем положением в нем. Все что у него есть – показания сенсоров и возможность сохранять информацию о предыдущих измерениях. Его задача – слоняясь по лабиринту построить полную карту, дабы будущие Тесеи не теряли времени зазря в поисках супостата. На практике это означает что робот ищет ответы на вопросы “Где я?!” и “Как выглядит мир вокруг?!”.

В каждый момент времени в мозг робота передаются показания с сенсоров. С помощью методов визуальной одометрии или на основе анализа дальномерных данных – робот может определять свое смещение относительно предыдущего положения. В идеальном случае – когда его вычисления точны и безукоризнены – по одним этим данным возможно воссоздать карту местности, где он уже побывал и полностью описать траекторию его движения. В печальной реальности – на каждом шаге возникает небольшая погрешность вычислений (ошибка замеров/помехи/ограничения, накладываемые алгоритмами и т.п.). С течением времени общая, аккумулятивная ошибка продолжает нарастать таким образом, что не смотря на приемлимую точность определения локального смещения – общая глобальная карта положений робота будет полна искажений. И вот тут-то на помощь роботу и приходят методы Simultaneous Localization And Mapping или просто SLAM.

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

Первый принципиальный момент – SLAM не предполагает каких-либо знаний о среде – ни меток на местности/ни предварительной карты нет – все решения строятся только на результатах измерений датчиков (обычно это rgb- и дальномерная камеры, но, бывает, и дополнительное оборудование навроде гироскопа-IMU/gps-датчика и т.д.).

Второй не менее принципиальный момент – среда считается статичной. Это значит, что если робот проехал мимо фикуса, никто за его спиной этот же самый фикус двигать не станет. В кадре – в обозреваемом сенсорами пространстве – никакого постороннего движения нету – флюгера не крутятся, никто в кадре не прогуливается. Освещение радикально не изменяется – т.е. тени по стенам не скачут. Такие условия сложно гарантировать в естественной среде – приятно шелестящая листва или любопытная кошка запросто может дезориентировать робота в пространстве (ну, не то чтобы прям запросто – и отсекать динамическую часть можно, и делать поправку на нее). Однако, если пофантазировать на тему марсоходов – то даже в настоящем виде методы SLAM могут найти свое применение. В частности, робот-пылесос – яркий пример применения данной методики в 2D-пространстве. Существуют экспериментальные реализации данной задачи в 3D (квадроптер шныряющий по коридорам и беспилотный батискаф).

SLAM – карта мира и траектория движения робота в нем

В общем случае SLAM можно описать как повторяющая последовательность шагов:
1) сканирование окружающего пространства
2) определение смещение на основе сравнения текущего кадра с предыдущим
3) выделение на текущем кадре особенностей-меток
4) сопоставление меток текущего кадра с метками полученными за всю историю наблюдений
5.1) обновление на основе этой информации положение робота за всю историю наблюдений
5.2) проверка на петли – не проходим ли мы повторно по одной и той же местности
5.3) выравнивание общей карты мира (отталкиваясь от положения меток и робота за всю историю наблюдений)

На каждом шаге алгоритма мы располагаем предположениями о структуре мира (у нас есть карта) и историей движения робота в нем (траектория движения относительно карты с разбивкой по времени).

Представленные реализации разделяются на две большие группы – SLAM frontend и SLAM backend.

Задача SLAM-fronted‘а сводится к оценке пространственных отношений между особенностями сцены и позами робота с некоторым значением вероятности. SLAM-fronted – набор методов для преобразования полученных данных от сенсоров к унифицированному представлению (структуре специального вида – см. далее) подаваемому на вход непосредственно SLAM-ядру. В настоящее время, для этих целей используются графы специального вида (варьируются от взвешенного, основанного на результатах измерений, до динамической сети Байеса, вовсю отталкивающейся от вероятностной модели положения робота в мире). Данный компонент специфичен для робота (какие там у него сенсоры, как организовано преобразование входной информации и т.п.), а потому какой-то универсальной библиотеки, реализующей весь функционал я не встречал. Хотя список поддерживаемых сенсоров того же ROS – robot operaring systems впечатляет. Принципы выбора особенностей на каждом кадре здесь описаны не будут – интересующимся гуглить по SIFT, SURF, NARF, FAST и т.д.

SLAM-backend – SLAM-ядро – непосредственная реализация решения задачи SLAM – оптимизация полученных от фронтенда пространственных отношений с целью максимизации вероятности. Данная задача абстрагированна от того, каким образом получены входные данные – главное, чтобы формат представления (граф, сеть, матрица, список и т.д.) информации о среде и положении робота в нем совпадали. Например, в каких-то случаях необходима высокая точность и не принципиально время вычислений, в других случаях критичен быстрый результат. Если мы храним все наши перемещения и замеры в виде специального графа то мы в первом случае скормим этот граф на оптимизацию iSam’у, а во втором отдадим TORO’у.

SLAM в деталях

Задачу SLAM можно подразбить на несколько связных частей:

SLAM frontend:

1) Анализ и интеграция новых данных (data association). На этом этапе производится выделение особенностей (Feature/landmarks extraction) на вновь полученных данных (на основе RGB-информации, карты глубины, их комбинации, показаний дополнительных датчиков – гироскопа, GPS-приемника и др.) Особенности – это такие характеристики, которые легко могут быть выделены в среде и использоваться в дальнейшем для ориентации в пространстве. Желательно чтобы их можно было распознать под разным “углом зрения” с точки зрения используемых сенсоров. Критично чтобы они были стационарны. Желательно чтобы их можно было различать друг от друга – для того чтобы понять – не встречали ли мы их ранее. Самый простой пример особенностей – геометрические – углы, прямые.

2) Вычисление сдвига (Local Motion Estimation). На основе сопоставления нового набора особенностей с особенностями, полученными на предыдущем шаге можно определить как изменилось их расположение на сцене. Так как особенности сами по себе стационарны – очевидно что этот сдвиг – результат изменения положения камеры (робота). На основе этой информации можно выразить координаты камеры через систему линейных уравнений для решения которой используются различные методы и их комбинации (ICP, visual odometry – FOVIS, RGB-D, etc).

3) Обновление структуры (Features Integration) хранящей историю перемещений, где каждое состояние представляет собой глобальное положение робота и взаимо-расположение обозреваемых особенностей на определенном промежутке времени. В последнее время кроме непосредственно данных о положении, хранятся также и вероятностные оценки для каждого значения. На данном этапе производится анализ и добавление полученных ранее данных в общую структуру для хранения информации о мире за все время исследования. Как правило, данные добавляются не на каждом шаге а при условии, что позиции робота(камеры) существенно изменилась – например, сдвиг или поворот по сравнению с предыдущей запомненной позицией составил более некоего граничного значения.

SLAM backend:

1) Сравнение данных локального сдвига и особенностей
Теперь у нас с одной стороны есть результаты локального сдвига, на основе которого мы можем вычислить новое положение робота. С другой стороны – мы так же можем определить позицию робота относительно особенностей сцены. В силу погрешностей сенсоров и используемых алгоритмов данные позиции не будут совпадать между собой (и, скорее всего, также и с реальным местоположением робота). Полученная разница должна быть отражена во всех предыдущих замерах особенностей как корректирующий коэффициент – вероятность.

2) Обновление общего представления о карте мира и траектории робота (Global Optimisation)
На основе вновь полученных данных производится уточнение текущих представлений о мире – пересчет позиций робота (state estimation), особенностей и их вероятностей. Дополнительно производится поиск замыканий (loop closure – ситуаций, когда робот возвращается в те места где уже побывал). После всех вычислений обновляется общее представление мира – позы робота (state update) и координаты особенностей (landmark update).

На основе глобальной структуры, хранящей все перемещения робота и представление мира в любой момент времени возможно воссоздать карту миру с траекторией движения робота. Для визуализации (Reconstructing world map) используются сторонние средства к методам SLAM особого отношения не имеющие.

Методы SLAM

Популярнейшим методом для решения был EKM – Extended Kalman Filter.
На каждом шаге у нас есть набор ранее полученных особенностей (landmark) и только что поступившие данные (дальномерные и RGB). На основании новых и предыдущих кадров мы можем определить смещение робота (используя методы визуальной одометрии) и предсказать новую позицию робота. С другой стороны, из нового кадра, мы можем выделить местоположение особенностей и вычислить положение робота относительно их. На основе разницы между двумя этими оценками позиций робота обновляются вероятности/веса для всех особенностей и корректируются позы-траектория движения робота. Детальное описание – по ссылкам ниже. В качестве структуры для хранения информации о мире и траектории движения робота – используется разрастающаяся со временем матрица ковариации, содержащая на каждом шаге исчерпывающую информацию о нашем текущем представлении мира. В случае большего числа поз – лучше подойдет particle filter, видео – отличный пример из курса ai-class от Sebastian Thrun.

Loop closure – обнаружение петлей. Обособлено от задач SLAM стоит вопрос как отслеживать ситуации когда робот возвращается туда, где уже побывал (в литературе это называется loop closure – замыкание, петля). Одно из решений – так называемая корзина “слов” (англ. Bags of Binary Words). Каждому кадру ставится в соответствие дескриптор (BRIEF descriptor), вычисляемый на основе визуальных особенностей изображения. Для хранения информации о изображениях, на основе данных для обучения, формируется словарь в виде дерева, содержащий “слова” для представления дескрипторов и их веса (отражающих насколько часто они встречались в наборе изображений для обучения). Для формирования “слова” производится поиск визуальных особенностей на основе данных для тренинга и их последующая группировка (с помощью метода k-среднего). Каждый последующий уровень дерева получается путем повторения данной операции с дескриптором родительского узла. В результате, при анализе ново-поступивших данных производится быстрое (см дистанция Хэмминга) разложение идентификатора текущего кадра в линейную комбинацию узлов дерева, где в роли коэффициентов выступают их веса – вероятности.

Как уже упоминалось выше, в настоящее время наиболее популярно представление проблемы SLAM в виде графа, где вершины и ребра представляют позицию робота и месторасположение особенностей сцены. iSam – одна из открытых реализаций, построенная по этому принципу. В iSam используется двудольный граф состоящий из узлов-поз, содержащих результаты вычислений координат робота и узлов-факторов, содержащих результаты оценок визуальной одометрии, отражающей сдвиг между двумя последовательными позами. Дополнительно используется множество узлов-особенностей, содержащей вычисленные координаты особенностей представленных на сцене. Узлы-позы и узлы-особенности не могут быть связаны друг с другом напрямую, а только через узлы-факторы. В качестве расширения могут быть использованы узлы-якоря – особенности наблюдаемые разными роботами (одновременно) или особенности которые видит один и тот же робот в течении нескольких независимых путешествий. Ядро системы производит оптимизацию данного графа выравнивая все узлы на основе новых данных. Возможна работа и без особенностей и без узлов-якорей – только на основе результатов визуальной одометрии.

В настоящее время наиболее быстрые результаты дает TORO (ценою точности вычисления).
Данный метод определяет конфигурация графа при которой вероятность наблюдаемых особенностей будет максимальна. Подходов к решению подобных задач – видимо не видимо (гуглить по “Sparse Bundle Adjustments”, “Structure from motion”, “Visual SLAM”), в основу TORO лег метод стохастического градиентного спуска – (SGD – stochastic gradient descent). Суть данного метода в последовательной оптимизации всего графа для каждого ограничения  (взаимного расположения особенностей и робота). Для объединения данных оптимизации с нескольких ограничений используются специальные коэффициенты для корректировки значений остатка. Ключевое достижение данного метода – представление данных для SGD – в TORO используется дерево параметров в качестве индексов для линейного уравнения поз робота.

Хочу попробовать поиграться со SLAM – есть ли что-нибудь готовое?

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

Желающим поиграться с какой-нибудь готовой реализацией можно посоветовать RGBDSLAM. На момент написания – инструкция по сборке устарела и требуется как следует прошерстить раздел http://answers.ros.org/ на предмет каждой ошибки. Гипотетически, если следовать уточнениям в ссылках ниже – и использовать эталонную систему авторов – т.е. 64 разрядную убунту – должно работать. Лично я собрать то ее собрал (на убунте 32), убив пару дней, однако спорадически возникающие сегфолты и пропадающий gui задушил желание поиграться на корню.

Желающим попытать счастья настоятельно рекомендую ознакомиться:

1. Ошибки вида can’t locate node, can’t find node – лечатся с помощью http://www.ros.org/wiki/fuerte/Installation/Overlays
2. Ошибки rosmake:
   1. Во-первых не надо лениться заглядывать в лог файл сборки.
   2. Во-вторых перед сборкой желательно ознакомиться с инструкцией – http://answers.ros.org/question/40059/ros-rgbdslam-fuerte-support/
   3. если не помогает – читать до просветления:
      http://answers.ros.org/question/12708/rgbdslam-installation-problem/
3. Если при сборке ноет что не находится findg2o.cmake – надо его найти в пакете g2o и либо смейк-скрипт подправить для rgbdslam либо сам файл переименовать.
4. Ошибки запуска (вида can not locate launch file) – см тут.
5. И, на посошок, репозиторий ежели что для всего проекта хранится тут – http://alufr-ros-pkg.googlecode.com/svn/trunk/

Возможный результат можно посмотреть тут.

Еще есть интересная библиотека Phovo. Скачать ее можно оттуда – http://code.google.com/p/photoconsistency-visual-odometry/. В своем составе, кроме всего прочего имеет приложения использующее SLAM-реализацию из mrpt, и реализованные автором алгоритмы визуальной одометрии (на основе http://vision.in.tum.de/_media/spezial/bib/steinbruecker_sturm_cremers_iccv11.pdf)

Из минусов – солидный список зависимостей, не все из которых собираются простым make install (одна unstable версия PCL чего стоит – внимательнее смотрите в cmake, какие компоненты вам действительно нужны).

Также учтите что для того чтобы завести NVidea toolkit под линуксом – придется немного попотеть: как установить cuda toolkit под ubuntu – http://sn0v.wordpress.com/2012/05/11/installing-cuda-on-ubuntu-12-04/

Ссылки на реализацию SLAM и связанных с ними компонент

DBow: Hierarchical bag-of-word library for C++ – описание
последняя версия DBoW – http://webdiis.unizar.es/~dorian/index.php?p=32

iSAM: Incremental Smoothing and Mapping
http://people.csail.mit.edu/kaess/isam/
по мотивам – http://www.cc.gatech.edu/~kaess/pub/Kaess08tro.pdf
страничка автора iSam:
http://www.cc.gatech.edu/~dellaert/FrankDellaert/Frank_Dellaert/Frank_Dellaert.html

Еще одна из вариаций SLAM-backend (на этапе оптимизации позволяет изменять структуру графа, ускоряя процес оптимизации)
http://www.tu-chemnitz.de/etit/proaut/forschung/robustSLAM.html.en

Список api связанных со SLAM имплементированных в библиотеке MRPT – Mobile Robot Programming Toolkit:
http://www.mrpt.org/List_of_SLAM_algorithms

Занимательное обсуждение различных алгоритмов SLAM:
http://stackoverflow.com/questions/1080533/slam-algorithm

ну и вика:
http://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping

UPDATE от 11.01.2012
Наткнулся в запасниках еще на некоторые материалы. Добавляю.

Вопрос новичка – что такое SLAM – и развернутые ответы “для начинающих”:
http://www.mrpt.org/node/441
Описание свеженькой (от 2012 года) книжки про SLAM
Оглавление интересное.
http://www.mrpt.org/SLAM_book_2012

Karto – коммерческая версия SLAM (есть триалка и опенсурсный вариант с последним коммитом от 2010 года)
в описании проскальзывали утверждения о том что работает лучше чем iSam
http://www.kartorobotics.com
http://www.willowgarage.com/blog/2010/04/19/karto-mapping-now-open-source-and-coderosorg

Описание еще одного метода loop closing:
http://jochen.sprickerhof.de/software/elch/
по ссылке на странице можно зайти на сайт опенсурсного продукта – The 3D Toolkit – в котором кроме SLAM есть средства для просмотра облака точек, определения поверхностей и т.п.

update 03102013
Презентация с roboconf.ru:
“SLAM – путеводные крошки в мире людей” SLAM – Intro with algo and libs

зы уточнения/поправки рьяно приветствуются