Материал: Картографирование при помощи монокулярной камеры

4.   Картографирование на неизвестной карте

4.1 Алгоритм SDVL

Алгоритм SDVL следует философии PTAM и делится на два отдельных потока двумя основными задачами алгоритмов SLAM: с одной стороны, он вычисляет смещение камеры, а с другой-управляет картой среды.

На рис. 4.1 показана общая схема алгоритма; он имеет в качестве входных изображений камеры и в качестве выходных данных 3D-оценку с течением времени.

Рис. 4.1: Схема алгоритма

Поток Mapping отвечает за создание, обслуживание и обновление карты среды робота. Для этого, P точек в 3D хранятся в K изображений (наблюдений), из которых эти точки наблюдаются. Не все наблюдения становятся частью карты, но только кадры, которые соответствуют определенным характеристикам, добавляются на карту (ключевые кадры или Keyframes).

Начальная карта создается на основе двух наблюдений с достаточным смещением между ними, которые становятся первыми двумя ключевыми фреймами карты. После этого новые 3D-точки добавляются в области карты, которые ранее не были изучены. Эти новые 3D-точки не рассчитываются непосредственно триангуляцией, как это происходит в PTAM, но инициализируются с учетом неопределенности в их глубине и сходятся по мере того, как робот перемещается и собирает новые наблюдения.

Поток Tracking отвечает за вычисление смещения камеры между двумя наблюдениями с использованием 3D-карты и самих наблюдений. Для расчета этого смещения было выбрано использование технологии hıbrida, основанной как на характерных pıxeles, так и на прямых методах.

Кроме того, система Tracking имеет систему отбраковки спурий (RDE) для удаления неправильно расположенных 3D-точек или движущихся элементов.

4.1.1 Поиск 3D точек

Самая простая параметризация для представления точки в 3 измерениях-это сохранение ее координат (X, Y, Z). Эта параметризация используется большинством алгоритмов SLAM (например, ранние версии MonoSLAM или PTAM). Однако его основной недостаток заключается в том, что он не позволяет точно представлять неопределенность в позиции точки в пространстве, особенно в ее глубине, когда она получена из изображений.

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

Однако некоторые точки могут быть настолько далеки, что для достаточного параллакса может потребоваться большое смещение или, в худшем случае, могут существовать объекты в “бесконечности”, для которых никогда не будет достаточного параллакса (например, при инициализации точки со звездой).

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

Для реализации SDVL был использован метод, разработанный с адаптацией, предложенной представлять точки в бесконечности.  Таким образом, расчетная глубина каждой точки моделируется с помощью распределения вероятностей, используя каждое новое наблюдение для обновления распределения с помощью байесовского вывода алгоритма SVO. Исходный метод принимает в качестве входных данных набор мер x1· * * * xn, поступающих из датчика глубины. Этот датчик может производить два типа измерений: проверенную меру (inlier), с вероятностью π, обычно распределенную вокруг фактической глубины Z, и неправильную меру (outlier), с вероятностью 1−π в результате равномерного распределения в интервале [Zmin, Zmax]. С помощью этих данных следующая гауссово + равномерная смешанная модель описывает распределение вероятности измерения xn:

Вероятность этой модели приближается с помощью гауссовского распределения для глубины Z и бета-распределения для вероятности:

В реализации алгоритма SDVL вместо прямого использования значений глубины Z и σ2 Z используется обратная глубина ρ = 1 Z и связанная с ней дисперсия σ2 ρ. Эта обратная параметризация облегчает представление точек в бесконечности, которая возникает, когда ρ равно 0.

4.1.2 Расчет 3D позиции

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

Для расчета этой позиции 3D используется уравнение 6.4.

Где ρ-обратная глубина, TF и RF-это положение и ориентация в мире кадра, в котором обнаружена точка 3D, а (vx,vy,vz) - луч обратной проекции в координатах относительно камеры на изображении, где была первоначально обнаружена точка 3D (рис.4.1).

Рис.4.1

4.2 Совпадение точек между изображениями

Одним из фундаментальных элементов алгоритмов SLAM, основанных на характеристиках, является сопряжение точек между двумя наблюдениями. Учитывая два кадра F1 и F2 и точку X в 3D-пространстве, которая видна с обоих кадров, цель спаривания-найти пары p1 и p2, в которых X будет наблюдаться с каждого кадра.

Если вы точно знаете положение кадров и точки 3D, просто проецируйте X на каждый кадр, чтобы получить pıxeles p1 и p2. Тем не менее, из-за ошибок в наблюдениях и неопределенности в позиции 3D X, необходимо выполнить поиск, чтобы попытаться найти спаривание между pıxeles.

В SDVL точка всегда инициализируется с кадра F1, а позиция 2D p1, из которой она была впервые замечена, считается фиксированной, поэтому поиск выполняется на изображениях последовательных кадров (F2), в которых X виден. При инициализации новой 3D-точки глубина точно не известна, поэтому из F1 можно сделать вывод только о обратной проекции p1,которая содержит все точки в пространстве X1, X2,·**, xn, которые проецируются на p1.

Рис 4.2: Эпиполярная прямая, полученная при проецировании каждой точки Xi на плоскость изображения F2

Некоторые из этих точек X1, X2,·**, Xn будут видны из F2, и проекция каждой из этих точек образует прямую линию на плоскости изображение F2, которое принимает имя эпиполярной прямой (рис.4.2 )

Поиск выполняется только на участке эпиполярной прямой, соответствующей глубине в пределах интервала:

В случае, если ρ-2σρ меньше или равно 0, в качестве значения будет установлено достаточно небольшое положительное значение (например, 10-10), чтобы избежать деления на 0.

Поиск 3D-точки в новом наблюдении выполняется вдоль полученной эпиполярной прямой, предполагая погрешность вокруг этого (рис. 4.3 (a)). Погрешность вокруг эпиполярной прямой настраивается, но ее значение сильно влияет на конечное вычислительное время алгоритма.

Рис 4.3: Точки, выбранные для сопряжения 3D-точки: диапазон поиска вокруг эпиполярной прямой (a); обнаруженные характерные точки (зеленые), проверенные (оранжевые) и используемые в сопряжения (красные) (b).

В пределах вычисляемого диапазона поиска можно выполнить тщательный поиск, проверив каждую точку внутри; однако вычислительное время будет очень высоким, а алгоритм не будет работать в режиме реального времени. Поэтому в практике используются только те характеристики, которые обнаруживаются с помощью алгоритма FAST.

Поскольку точки, полученные с помощью алгоритма FAST, сортируются по строкам и столбцам, не нужно проверять все обнаруженные точки, А достаточно их подмножества. На рисунке 6.6 (b) показано, как из всех точек FAST, обнаруженных на изображении (зеленый цвет), необходимо только проверить положение на изображении тех точек, которые разделяют строку и столбец с эпиполярной прямой (оранжевый).

Алгоритм FAST подробно рассмотрен в главе 3.

4.3 Картографирование

Поток Mapping отвечает за управление картой среды, в которой находится робот. Эта карта обновляется с каждым наблюдением и служит основой для расчета отслеживания камеры.

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

Чтобы определить, когда кадр хранится на карте как Keyframe, проверяются два условия:

1. Количество прошедших итераций: в случае создания Keyframe необходимо дождаться числа итераций по умолчанию, прежде чем можно будет создать другой. По умолчанию это значение имеет значение 30 итераций (1 в секунду).

2. Количество потерянных точек: количество видимых 3D точек сравнивается с теми, которые вы видите при создании последнего Keyframe. Если это число меньше, это означает, что очки были потеряны, и новый Keyframe будет создан, когда потеряно не менее 10% очков.

Для создания нового Keyframe эти два условия должны выполняться одновременно. Однако при очень резких перемещениях (особенно при поворотах) точки могут быть быстро потеряны, и эта процедура может привести к потере робота. Таким образом, если количество точек, потерянных после последнего Keyframe, превышает 30%, создается новый Keyframe, даже если с момента последнего не прошло достаточно итераций.

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

3D-точки, из которых состоит карта, - это те, которые были обнаружены одним или несколькими текущими ключевыми фреймами и доступны через каждый Keyframe, следуя схеме на рисунке 6.2.

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

4.3.1 Инициализация на карте

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

Во-первых, с помощью алгоритма FAST вы получаете характерные точки каждого нового наблюдения. Для того, чтобы использовать наблюдение, необходимо, по крайней мере, обнаружить 50 точек на изображении, хотя обычно алгоритм обычно получает более 300.

Первое наблюдение, которое соответствует этому условию, хранится как первый Keyframe алгоритма и служит основой для создания начальной карты. В последующих наблюдениях вычисляется смещение каждой точки, обнаруженной в первом Keyframe, с помощью оптического потока Лукаса-Канада.

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

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

Рис 4.4: Инициализация

После того, как первые два Keyframes доступны, начальная карта рассчитывается с использованием 5-точечного алгоритма. Предполагая, что оба наблюдения содержат доминирующую плоскость и с использованием алгоритма RANSAC, вы получаете H гомографию, который определяет преобразование перспективы между обоими изображениями (рис. 4.4), так что ошибка повторного выбора каждой пары точек минимизируется:

4.3.2 Обновление неопределенности

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

Для точек, наиболее близких к камере, смещение позволит достичь достаточного параллакса, уменьшив неопределенность в глубине и сближая значение обратной глубины с ее фактическим положением. Этап обновления каждой точки также является дорогостоящим и не стоит продолжать выполнять его, как только неопределенность будет достаточно небольшой. Чтобы определить, когда точка сходится в ее реальном 3D-положении и, следовательно, больше не нуждается в обновлении, используется предлагаемое условие конвергенции в, вычисление линейности L с помощью уравнений 6.7 и 6.8:

D расстояние от точки в 3D до текущего положения камеры и α параллакс точки 3D относительно первого и последнего наблюдения.

При таком уравнении, когда параллакс низкий, отклонение будет высоким. Будет близка к одному, получая высокое значение L. В то время как по мере увеличения параллакса оба σρ уменьшаются и, как следствие, значение L будет низким. Точка считается сходившейся, когда значение L меньше 0.1, значение, которое было экспериментально установлено.

Рис. 4.5: Конвергенция 3D точек после итераций.

На рис. 4.5 показан пример того, как 3D-точки сходятся с прохождением итераций. Первоначально все инициализированные точки имеют высокую неопределенность и поэтому их глубина неизвестна (красные прямые), но как только смещение камеры достаточно, некоторые из точек соответствуют условию конвергенции и фиксируют их глубину (синие точки).

Каждый раз при получении нового кадра (независимо от того, является ли он Keyframe) список кандидатов на карте просматривается, и они пытаются обновить свои позиции. Во-первых, проверяется, видна ли точка с текущего положения камеры, и в этом случае выполняется поиск точки по методу. Если точка находится, ее позиция в пространстве треугольника с использованием текущего кадра и исходного Keyframe, в котором этот кандидат впервые обнаружен. Это приводит к новой глубине, которая служит для обновления распределения вероятностей 3D-точки. Кроме того, проверяется, сходилась ли точка, чтобы продолжить в этом случае ее удаление из списка кандидатов.

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

4.4 Локализация

Другой основной поток выполнения алгоритма SDVL отвечает за отслеживание. Этот поток использует информацию, хранящуюся на карте, и наблюдения камеры, чтобы вычислить смещение робота от последней итерации, то есть постепенно. Кроме того, вы должны выполнить эту задачу, выполнив временные ограничения эффективности для работы в режиме реального времени. Исходя из положения и ориентации камеры в моменте t−1 (xt−1), вычисление конечной позиции в моменте t (xt) выполняется в четырех разных шагах: