Одна из ключевых особенностей Kintinious – стабильная работа методов позиционирования камеры в пространстве. Многие ждут когда же они выложат свой код (дада, они собирались это сделать) но мало кто знает что часть кода вобщем-то и так доступна. В частности, одна из ключевых компонент используемых в их работе – набор методов визуальной одометрии – вполне себе открыт для всех, кого интересуют как же работает Kintinious, как ориентируется в пространстве вертолет из ролика – http://www.youtube.com/watch?v=aiNX-vpDhMo.
В данной статье речь пойдет о библиотеке fovis – Fast Odometry from VISion – предназначенной для визуальной одометрии – т.е. для определения изометрии (движения) камеры на основе анализа последовательных фреймов. Смещение камеры однозначно определяется двумя характеристиками – матрицей поворота и вектором сдвига. Альтернативые методы описания смещения – кватернионы, с учетом масштабирования и т.п. здесь не будут рассматриваться. Путем анализа последовательных кадров получаемых с кинекта – кадров глубины и rgb-изображений – можно вычислить изометрию для каждой пары кадров. Располагая данными о том как камера перемещалась в пространстве относительно любой пары последовательных кадров можно во-первых отследить весь ее путь, и, во-вторых, сконструировать сформировать общую сцену, в рамках которой происходило движение камеры.
Библиотека содержит набор методов визуальной одометрии, скомпонованных таким образом чтобы обеспечить разумный компромисс между точностью и вычислительной эффективностью. Для расчетов используются CPU (хотя было бы эффективней распараллелить SIMD операции на GPU).
Общий алгоритм визуальной одометрии из библиотеки FOVIS выглядит так:
- Подготовка данных (фреймов rgb и глубины), далее, когда упоминается текущий фрейм – имеется в виду данные с кадра глубины и rgb-фрейма, полученные на текущей итерации.
- Поиск характерных особенностей на текущем кадре (FAST) – характерные особенности это, например, углы
- Сегментация – изображение разбивается на прямоугольные участки, в каждом из которых отбирается заданное число самых ярко выраженных характерных особенностей
- Определяем смещение текущего фрейма относительно предыдущего
- На его основе найденного смещения производим поиск и сравнение характерных особенностей не сразу во всем изображении, а по соответствующим другу-другу участкам Для сравнения и выравнивания используется несколько методов:
- Значение SAD (sum of absolute difference) – абсолютная сумма различий между двумя особенностями + выравнивание на основе метода наименьших квадратов
- Конструируется граф, где каждая вершина соответствует совпавшей паре особенностей. Ребра проводятся между вершинами в случае если расстояние между особенностями не превосходит заданного порогового значения, на протяжении анализа всей последовательности. (Т.е. на двух кадрах мы нашли несколько особенностей – например углы здания – евклидово расстояние между ними будет постоянным от кадра к кадру). В этом графе ищется максимальный связный подграф – т.е. все вершины которого соединены ребрами. (как альтернатива RANSAC)
- Финальное выравнение результатов:
- Минимизации евклидовой дистанции между совпавшими особенностями
- Минимизации ошибки проектирования (с предыдущего кадра на текущий и наоборот)
- Минимизация масштабного отклонения с помощью ключевых кадров
- Дополнение общей сцены данными из текущего сцены
В результате шагов 1-7 – мы получаем финальную изометрию, достаточно точную для дальнейшего конструирования глобальной сцены и позиционирования камеры в нем.
Описание алгоритмы FOVIS подробно:
I Этап – визуальная одометрия
1. Подготовка данных
RGB изображение упрощается в градации серого – grayscale – на основе следующего округления – финальный цвет пикселя равен округленному значению суммы (0.2125 * R + 0.7154 * G + 0.0721 * B).
Cоздается пирамида для того чтобы упростить поиск фич при различном масштабе
Число уровней можно задать (VisualOdometryOptions значение max-pyramid-level) – по умолчанию – 3 первый уровень – исходная картинка, последующие – вдвое меньше предыдущей.
Каждый уровень фильтруется с помощью Гауса (для того чтобы влияние соседних пикселей на текущий уменьшалось с увеличением расстояния до него). В сопутствующей работе указано что сигма должна быть 0.85, однако в коде библиотеки используется ядро 1/16*[ 1 4 6 4 1 ] что соответствует почти 1.0.
2. Поиск характерных особенностей (FAST)
Детальное описание FAST – можно найти здесь(ССЫЛКА)
Изначально исходный кадр разбивается на окружности с диаметров 7 пикселей (длина окружности 16), для каждой проверяется наличие особенностей и их значение. Для этого изучается яркость соседених пикселей в окружности пикселя-кандидата, где длина окружности 16. На основании наиболее распространных описаний конфигурации углов – не всех так как всех для 16 будет 3^6 – а для наиболее характерных. т.е. Фактически это дерево вариантов по которому производится бинарный поиск. Если данный регион изображения удовлетворяет условию поиска – для него вычисляется значение особенности на основе значений яркости соседних пикселей. Каждой особенности ставится в соответствие данные из кадра глубины, если данных по ней не находится – то данная особенность отбрасывается.
3. Сегментация
Необходимо отобрать наиболее сильные, характерные особенности – для этого применяется простенький фильтр: на все изображение, фигурально выражаясь, набрасывается сетка, каждая ячейка которой рассматривается отдельно. Среди всех особенностей, что попадали в эту ячейку, отбираются наиболее ярковыраженные (по их значению) в количестве регулируемом параметром max_keypoints_per_bucket (здесь и далее, данный параметр – значение набора типа VisualOdometryOptions содержащему набор значений, набор по умолчанию состоит из рекомендованных значений и формируется с помощью вызова функции getDefaultOptions).
По умолчанию размер ячейки сетки 80х80 пикселей, и требуется 25 наиболее сильных особенностей в каждой ячейке.
Необходимое число фич может изменяться в процессе работы алгоритма, с использованием простого выравнивания числа найденных фич и параметров _fast_threshold_min, _fast_threshold_max если активирован флаг _use_adaptive_threshold.
4. Определение изометрии – смещения
Так как мы рассматриваем последовательные кадры, движение, которое можно наблюдать по изменению положения особенностей от кадра к кадру вызвано 3d-вращением. Определив это 3d-вращение мы можем сузить окно поиска – ячейку сетки из предыдущего пункта – для сравнения особенностей разных фреймов.
Для определения вращения сначала вычисляем проективное преобразование от предыдущего фрейма к текущему, а затем, с помощью матрицы Якоби – определяем изометрию.
5. Поиск и сравнение характерных особенностей
Каждой особенности ставится в соответствие 80-байтовый дескриптор содержащий усредненные значения яркости пикселей в окрестности особенности размером 9 на 9, за исключением верхнего правого пикселя.
При сравнении двух особенностей вычисляется абсолютная сумма различий их дескрипторов (SAD – sum-of-absolute difference) – SIMD инструкции. Здесь кроется потенциальная возможность для оптимизации – данные вычисление эффективней было бы выполнять на GPU.
Особенности признаются одинаковыми в случае если значение их SAD наименьшее и они лежат в одном и том же окне (ячейке сетки) определяемым начальным движением. После чего, может быть произведено попиксельное выравнивание (активируется параметром use-subpixel-refinement)
В качестве дополнительного метода избавления от ошибок используется следующий прием как альтернатива для RANSAC. Конструируем граф так – если пара особенностей соответствуют друг-другу во всех фреймах с минимальным уровнем расхождения – добавляем в граф новую вершину. Добавляем ребро между двумя вершинами графа, в случае если евклидово расстояние между особенностями, соответствующими этим вершинам, изменяется не больше указанного граничного значения (параметр _clique_inlier_threshold).
Определяем максимально полный подграф с помощью “жадного” алгоритма и обновляем на его основе наш список особенностей. Оставшиеся после этого особенности принято называть “inliers” но я продолжаю использовать термин “особенности”.
6. Финальная стадия выравнивания
1) Выравнивание полученного движения минимизацией ошибок проектирования особенностей. Проектирование используется двустороннее – с текущего на предыдущий кадр и с предыдущего на текущий – за счет этого мы еще раз уточняем наше значение изометрии при этом из набора особенностей удаляются особенности с высокими показателями ошибок. ( оригинальный метод гуглить по названию “Improving vision-based control using efficient second order minimization techniques”)
2) Для уменьшения небольшого масштабного отклонения – используется техника ключевых участков-фреймов: движение определяется сравнением предыдущего кадра-ссылки с текущим. Если движение камеры по предыдущему фрейму вычисляется не успешно и на новом кадре обнаружено достаточно число особенностей (ref_frame_change_threshold) – ссылочный кадр меняется. Иначе производится попытка сравнения текущего кадра не с ссылочным, а с предыдущим в последовательности, в случае неудачи с пред-предыдущим и т.д.. Такой прием используется для избавления от колебаний в случае если точка обзора не меняется значительно.
II Этап – SLAM
Визуальная одометрия дает достаточно точные результаты, но нужна общая целостность. При совмещении данных от нее может возникнуть наложение и перекрытие данных в случае повторного перемещения камеры над одним и тем же местом. Для корректной обработки подобной ситуации нужны алгоритмы SLAM умеющие обрабатывать – loop closure – повторные посещения одной и той же области. В библиотеку FOVIS методы SLAM не включены, однако их описание присутствует в предыдущих работах, потому, считаю логичным привести здесь дальнейшие шаги общего алгоритма воссоздания общей сцены.
Задачи SLAM решаются на отдельной машине – данные на сервер SLAM подаются в общем виде – карты глубины уменьшены до 128×96 так, что расстояние между соседними пикселями 5 см. Для разрешения замыканий сцены (этим термином я буду оперировать рассуждая о loop closure) используется сравнения набора ключевых фреймов с новым фреймом. Новые ключевые фреймы добавляются когда общее движение по сравнению с текущим фреймом превышает 25 см и 10 градусов.
Когда создается новый ключевой фрейм, производится сравнение, с помощью процедуры RANSAC на ключевых точках (определяемых все тем же FAST) текущего фрейме и фрейма полученного 4 секунды назад. Процедура RANSAC – это просто случайная выборка среди набора особенностей. Так как замыкание сцены требует сравнения не обязательно последовательных кадров, в RANSAC предполагаемые совпадение ключевых точек определяются используя дескриптор Калондра (гуглить по фразе “Calonder randomized tree descriptor – Keypoint signatures for fats learning and recognition”. In europian conference on computer vision 58-71). Ключевая точка считается похожей на точку из предыдущего фрейма, если расстояние к наиболее подходящей ключевой точке предыдущего фрейма имеет соотношение меньше чем 0.6 по сравнению со следующей наиболее подходящей точке.
RANSAC совпадение устанавливается между фреймами если на них совпало минимум 10 особенностей.
Далее аналогично FOVIS производится выравнивание на основе минимизации ошибок обратной проекции между эталонным (полученным 4 секунды назад) и текущим кадром. Финальное выравнивание относительной позиции между двумя ключевыми кадрами получается путем решения системы выравнивания двух ключевых фреймов минимизирующую общую ошибку репроекции. (гуглить по словам “sparse bundle-adjustment” или “ams05-visualodometry.pdf”)
III Этап выравнивание
Показания скорости и направления из систем I и II суммируются с показаниями юнита IMU и передаются на вход в расширенный фильтр кальмана (extended kalman filter). Фильтр сглаживает их показания и выдает управляющему модулю.
В процессе работы II корректировки положения передаются клиенту, в FOVIS (с ощутимой задержкой) – потому они заносятся в историю положений источника съемки с учетом времени. После чего все последующие оценки положения пересчитываются с учетом этих обновлений.
Используемая литература:
исходный код библиотеки FOVIS – http://code.google.com/p/fovis/
Closed-form solution of absolute orientation using unit quaternions
Visual Odometry and Mapping for Autonomous Flight Using an RGB-D Camera
RGB-D Mapping: Using Depth Cameras for Dense 3D Modeling of Indoor Environments – SLAM система упомянутая во втором этапе детально описана там