ПРОГРАММИРОВАНИЕ, 2014, No 4, с. 5-12
КОМПЬЮТЕРНАЯ ГРАФИКА -
УДК 681.3.06
3D SLAM ПО СТЕРЕОИЗОБРАЖЕНИЯМ
© 2014 г. В.А. Бобков, Ю.И. Роньшин, А.П. Кудряшов,
В.Ю. Машенцев
Институт автоматики и процессов управления ДВО РАН 690041 г. Владивосток, ул. Радио, 5 E-mail: {bobkov, ronshin, kudryashovA}@dvo.ru, v.mashentsev@gmail.com
Поступила в редакцию 15.01.2014
Предлагается метод визуальной навигации автономного подводного аппарата по стереоизображениям. Рассмотрены две модификации метода. Первая модификация основана на интеграции данных измерений бортовой навигационной системы с визуальными данными, вторая - на применении 6-облачной схемы вычислений. Приведены сравнительные результаты вычислительных экспериментов с модельными данными.
1. ВВЕДЕНИЕ
Задача SLAM (Simultaneous Localization and Mapping) продолжает оставаться сегодня актуальной в области компьютерного зрения и робототехники, поскольку от эффективности ее решения зависит точность навигации роботов в пространстве и реконструкции окружающей обстановки. Классическое решение SLAM, применяемое во многих работах, основывается на расширенном фильтре Калмана [1]. Применительно к подводным аппаратам (ПА) наряду с сенсорными данными, получаемыми традиционными средствами навигации (гидроакустические сенсоры, компасы, допплеровский лаг и др.) используются и видеоданные (захватываемый видеокамерами поток изображений). Особенно это важно в условиях локального маневрирования, когда традиционно применяемые гидроакустические сонары не всегда обеспечивают требуемую точность позиционирования подводного робота. В последующих работах рассматриваются постановки задач с использованием стереокамер, что позволяет совместно анализировать 2D информацию и получаемые 3D облака точек. Например, такой подход с применением фильтра Калмана развивался в работах [2,3] для навигации и реконструкции подводной обстановки и в работах [4,5] с вычислением геометрических преобразований между локальными системами ко-
ординат ПА по сопоставленным 3D облакам. В настоящем докладе предлагается метод навигации автономного ПА также основанный на сопоставлении точечных особенностей на стереоизображениях и анализе соответствующих 3D облаков точек при движении ПА по траектории. Его отличительными особенностями являются: адаптивная методика отбора видеоданных, применение многоступенчатой фильтрации особенностей, интеграция визуальных данных и показаний навигационной системы ПА, применение 6-облачной схемы вычислений. В разделе 2 дается описание визуального метода навигации и его модификаций. В разделе 3 приведены результаты вычислительных экспериментов и их анализ.
2. ВИЗУАЛЬНЫЙ SLAM 2.1. Описание подхода
В рассматриваемой постановке автономный подводный аппарат (АПА) оснащен двумя видеокамерами (стереопара), которые выполняют синхронную съемку дна при движении аппарата по траектории. Решение задачи SLAM подразумевает точное вычисление траектории автономного робота (его 3D положение и ориентацию) при движении в априори неизвестной среде, с одновременным построением модели среды по получаемым сенсорным данным. Оно сводится к
Рис. 1. Схема вычисления матрицы локального преобразования.
вычислению матрицы преобразования из мировой в .локальную систему координат (ЛСК), привязанную к аппарату в каждой позиции его траектории. Искомая матрица может быть подучена перемножением .локальных матриц всех предшествующих позиций. Предлагаемый метод вычисления матрицы локального преобразования следует уже сложившемуся подходу, основанному на совместном использовании 2D особенностей на изображениях и 3D облаков точек, получаемых на базе стереопар. Вычислительная схема при определении параметров текущей позиции ПА состоит из двух основных этапов (рис. 1). На первом этапе анализируются две стереопары изображений, относящиеся к двум последовательным во времени позициям текущей и предыдущей. Применительно к ним осуществляется поиск и сопоставление единого множества точечных особенностей, одновременно наблюдаемых на всех 4-х изображениях. Для поиска и построения дескрипторов особенностей используется детектор SURF. Для каждой из двух стереопар выполняется построение 3D точек особенностей (3D облако) методом триангуляции, т.к. известна полная калибровка используемых камер. Заметим, что 3D точки в этих облаках сопоставлены в силу выполненного сопоставления их 2D прообразов. На втором этапе оценивается перемещение ПА из предыдущей позиции в текущую позицию. Поскольку мы наблюдаем одно и то же множество 3D точек из двух систем координат (CK) (текущая и предыдущая позиция ПА), то можно решить задачу вычис-
ления геометрического преобразования (перенос и вращение) между СК этих позиций ПА. Для ее решения формулируется оптимизационная задача, где в качестве целевой функции рассматривается минимизация суммарных расхождений по всем точкам 3D облака. Ее решением является локальное (относительное) преобразование между двумя СК камер. Для получения абсолютного преобразования из мировой СК в СК текущей камеры достаточно, как было отмечено выше, объединить матрицы локальных преобразований всех предыдущих позиций ПА, начиная с первой. Вычисленные указанным образом матрицы преобразований позволяют получать в каждой позиции траектории ПА все 6 параметров, характеризующих положение аппарата в пространстве.
В рассматриваемом подходе на получаемую точность визуальной навигации негативно влияют два фактора: а) недостаточное количество точек в 3D облаках и б) ошибочные сопоставления (outliers). Поддержание гарантированного количества точек в 3D облаках обеспечивает адаптивный алгоритм выбора очередной обсчитываемой позиции ПА на траектории. Устранение outliers делается как на этане 2D обработки (пороговое отсечение, кросс-проверка), так и на этапе построения 3D облаков (эпиполярные ограничения в каждой стереопаре, итерационная фильтрация точек на основе структурной когерентности облаков).
Существенное повышение точности навигации дает интеграция в описываемую вычислительную схему измерений бортовой навигационной
системы. Используемые в бортовой навигационной системе ПА приборы (гирокомпасы, магнитные компасы) дают абсолютные значения углов ориентации, что позволяет воспрепятствовать накоплению ошибки со временем - характерному недостатку многих методов. Еще один элемент новизны предлагаемого подхода - рассмотрена и оценена эффективность альтернативной схемы вычислений с учетом обработки 6-ти облаков точек, которые возникают при анализе
3-х стереопар, соответствующих 3-м последовательным позициям ПА. Программная реализация выполнена с использованием CUDA-версии библиотеки OpenCV.
2.2. Сопоставление и фильтрация особенностей на изображениях
Сопоставление точечных особенностей выполняется на 4-х изображениях - 2-х стереопарах (1-2 и 3-4), соответствующих двум последовательным позициям ПА на траектории. Схема сопоставления построена на применении двух алгоритмов (библиотека OpenCV): а) детектора SURF для некалиброванных пар изображений (1-3 и 2-4). В этом случае выполняется построение дескрипторов и вычисление оценки расстояний между ними (с фильтрацией по порогу); и б) алгоритма вычисления диспарантности для калиброванных очищенных пар (1-2 и 3-4). Для каждой пары изображений выполняется сопоставление слева направо и справа налево (cross -checking). Результатом является множество особенностей, сопоставленных по всей цепочке из
4-х изображений. Это позволяет построить на следующем этапе два облака 3D точек, отвечающих двум стереопарам (двум позициям ПА).
Адаптивная мет,одика„ С учетом того, что от количества сопоставленных особенностей зависит точность последующей калибровки камер, была реализована адаптивная стратегия выбора очередной оцениваемой позиции (точки) на траектории ПА. Если для текущей позиции количество сопоставленных особенностей получилось меньше заданного порогового значения, то выбирается более близкая позиция, потенциально обеспечивающая большее число сопоставлений. Соответствующий алгоритм использует для этого переменный шаг во времени для фиксации удовлетворяющей этому условию позиции.
2.3. Вычисление положения и ориентации ПА по ЗВ облакам точек - особенностей
Изменения в положении и ориентации ПА при переходе в очередную позицию можно вычислить, опираясь на сравнение двух сопоставленных (по точкам) ЗО облаков, одно из которых построено в системе координат первой стереопары (предыдущая позиция), а другое в системе координат второй стереопары (текущая позиция). Вычисление ЗБ точек каждого из облаков выполняется обычной триангуляцией (пересечение двух лучей в системе координат стереопары) по сопоставленным особенностям на двух снимках стереопары. Вычислительная задача сводится к решению переопределенной системы уравнений, где данными являются координаты наблюдаемых точек, а неизвестными являются элементы матрицы преобразования. Пусть перемещение ПА из точки Рг-1 траектории в точку Рг определяется некоторым неизвестным преобразованием Иг, состоящим из вектора переноса и кватерниона вращения, г - номер позиции на траектории. Будем искать это преобразование, основываясь на имеющемся взаимнооднозначном сопоставлении двух облаков ЗБ точек, наблюдаемых парой камер, соответственно, в позициях (г-1) и г. Первое облако С1 (х, у, г) задано в СК 1-й стереопары, 2-е облако С2(х, у, г) -в СК 2-й стереопары. Задача нахождения матрицы Иг решается методом нелинейной оптимизации с ограничением (используется библиотека МЛТ1.ЛВ). В качестве параметров оптимизации используются 3 координаты вектора переноса и 4 координаты кватерниона, определяющего вращение Иг. Ограничение задается условием - норма кватерниона = 1. Целевая функция ^ = ^ || с| — ск ■ Иг ||, к - номер точки в облаке. Здесь {с\ } - множество точек в первом облаке и {с|} -множество точек во втором облаке. Тогда с уче-
Иг
Рг =
Рг-1 ■ Иг .В качестве точки, описывающей траекторию ПА, можно рассматривать точку начала СК камеры (0,0,0). Поскольку нам нужны координаты точек траектории в мировой СК, необходимо вычислить преобразование из мировой СК г
ровой СК выбрана СК
Для дальнейшего прочтения статьи необходимо приобрести полный текст. Статьи высылаются в формате PDF на указанную при оплате почту. Время доставки составляет менее 10 минут. Стоимость одной статьи — 150 рублей.