Как я делал систему оптического трекинга

Дело было в далеком 2015 году. В продаже только появились очки виртуальной реальности Oculus DK2, рынок VR игр быстро набирал популярность.

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

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

Конечно, избавиться от геймпада, от проводов, дать возможность игроку свободно перемещаться в пространстве, видеть свои руки и ноги, взаимодействовать с окружением, другими игроками и реальными интерактивными предметами.

Видел я это так:

1. Берем несколько игроков, надеваем на них VR очки, ноутбук и датчики на руки, ноги и туловище.
2. Берем помещение, состоящее из нескольких комнат, коридоров, дверей, оборудуем его системой трекинга, вешаем датчики и магнитные замки на двери, добавляем несколько интерактивных предметов и создаем игру, в которой геометрия виртуальной локации точно повторяет геометрию реального помещения.
3. Создаем игру. Игра представляет собой многопользовательский квест, в котором несколько игроков надевают на себя оборудование и оказываются в виртуальном мире. В нем они видят себя, видят друг друга, могут ходить по локации, открывать двери и совместно решать игровые задачи.

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

Для реализации заявленного функционала, нужно было создать две основные технологии:

1. костюм, состоящий из датчиков на руках, ногах и торсе, отслеживающий положения частей тела игрока
2. система трекинга, отслеживающая игроков и интерактивные объекты в 3D пространстве.

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

Система трекинга.

Бюджета на все это, конечно, у нас не было, поэтому нужно было сделать все из подручных материалов. Для задачи отслеживания игроков в пространстве я решил использовать оптические камеры и светодиодные маркеры, закрепленные на VR очках. Опыта подобных разработок у меня не было, но я уже что-то слышал про OpenCV, Python, и подумал, что справлюсь.

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



Соответственно, камеры нужно было закрепить на потолке так, чтобы каждая точка пространства просматривалась минимум двумя камерами (лучше больше, чтобы избежать перекрытия обзора телами игроков). Для покрытия трекингом предполагаемого помещения площадью около 100 кв.м., требовалось около 60 камер. Я выбрал первые попавшиеся дешевые на тот момент usb вебки.



Эти вебки нужно к чему-то подключать. Эксперименты показали, что при использовании usb удлинителей (по крайней мере, дешевых), камеры начинали глючить. Поэтому решил разделить вебки на группы по 8 штук и втыкать их в системники, закрепленные на потолке. На моем домашнем компе как раз было 10 usb портов, так что пришло время начинать разработку тестового стенда.

Архитектуру я придумал следующую:
На каждые очки вешается акриловый матовый шарик от гирлянды с вклеенным внутрь RGB светодиодом. Одновременно в игре предполагалось несколько игроков, так что для идентификации решил разделять их по цвету – R, G, B, RG, RB, GB, RB. Вот так это выглядело:



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

Поиск шарика на кадре

Мне нужно было в каждом кадре, пришедшем с камеры, искать координаты центра шарика и его цвет для идентификации. Звучит несложно. Качаю OpenCV под Python, втыкаю камеру в usb, пишу скрипт. Для минимизации влияния лишних объектов на кадре, выставляю экспозицию и выдержку на камере в самый минимум, а яркость светодиода делаю высокой, чтобы получить яркие пятна на темном фоне. В первой версии алгоритм был следующий:

1. Переводим изображение в градации серого
2. Бинаризуем по порогу (если яркость пикселя больше порога, он становится белым, иначе – черным). При этом размытое пятно от шарика превращается в кластер белых пикселей на черном фоне
3. Находим контуры кластеров и их центры. Это и есть координаты шарика на кадре
4. Определяем усредненный цвет пикселей кластера (на исходном цветном изображении) в окрестности его центра для идентификации

Вроде, работает, но есть нюансы.

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

Во-вторых, разрешение камеры всего лишь 640*480, поэтому на некотором расстоянии (не очень большом) шарик виден как пара пикселей на кадре и алгоритм поиска контуров перестает нормально работать.

Пришлось придумывать новый алгоритм. В голову пришла следующая идея:

1. Переводим изображение в градации серого
2. Размываем картинку мощным Gaussian blur –ом так, чтобы изображения светодиодов превратились в размытые пятна с градиентом яркости от центра к периферии
3. Находим самые яркие пиксели на изображении, они должны соответствовать центрам пятен
4. Так же определяем средний цвет кластера в окрестности центра

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

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

Нагрузочный тест

Подключаю 8 камер к своему десктопу, располагаю их так, чтобы каждая видела светящиеся точки и запускаю скрипт, где описанный алгоритм работает в 8-ми независимых процессах (спасибо питонской либе «multiprocessing») и обрабатывает все потоки сразу.

И… сразу натыкаюсь на фейл. Изображения с камер то появляются, то исчезают, framerate скачет от 0 до 100, кошмар. Расследование показало, что часть usb портов на моем компе подключены к одной шине через внутренний хаб, из-за чего скорость шины делится между несколькими портами и ее уже не хватает на битрейт камер. Втыкание камер в разные порты компа в разных комбинациях показало, что у меня всего 4 независимых usb шины. Пришлось найти материнку с 8-ю шинами, что было довольно непростым квестом.


Продолжаю нагрузочный тест. На этот раз все камеры подключились и выдают нормальные потоки, но сразу сталкиваюсь со следующей проблемой – низкий fps. Процессор загружен на 100% и успевает обрабатывать лишь 8-10 кадров в секунду с каждой из восьми вебок.



Похоже, нужно оптимизировать код. Узким местом оказалось Гауссово размытие (оно и не удивительно, ведь нужно на каждый пиксель кадра производить свертку с матрицей 9*9). Уменьшение ядра не спасало ситуацию. Пришлось искать другой метод нахождения центров пятен на кадрах.

Решение удалось найти внезапно во встроенной в OpenCV функции SimpleBlobDetector. Она делает прямо то, что мне нужно и очень быстро. Преимущество достигается благодаря последовательной бинаризации изображения с разными порогами и поиску контуров. Результат – максимальные 30 fps при загрузке процессора меньше 40%. Нагрузочный тест пройден!

Классификация по цвету

Следующая задача – классификация маркера по его цвету. Усредненное значение цвета по пикселям пятна дает RGB компоненты, которые очень нестабильны и сильно меняются в зависимости от расстояния до камеры и яркости светодиода. Но есть отличное решение: перевод из RGB пространства с HSV (hue, saturation, value). В таком представлении пиксель вместо «красный», «синий», «зеленый», раскладывается на компоненты «тон», «насыщенность», «яркость». В этом случае насыщенность и яркость можно просто исключить и классифицировать только по тону.





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

Трекинг

Я использовал pinhole модель камеры, в которой все лучи падают на матрицу через точку, находящуюся на фокусном расстоянии от матрицы.



По этой модели будет происходить преобразование двухмерных координат точки на кадре в трехмерные уравнения прямой в пространстве.

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

Калибровка трекинга

В первом варианте решил сделать калибровку трекинга максимально примитивной.

1. Вешаю первый блок из восьми камер на потолок, подключаю их к системнику, висящему там же, направляю камеры так, чтобы ими покрывался максимальный игровой объем.
2. С помощью лазерного нивелира и дальномера измеряю XYZ координаты всех камер в единой системе координат
3. Для вычисления ориентаций и фокусных расстояний камер, измеряю координаты специальных стикеров. Стикеры вешаю следующим образом:
   
   • В интерфейсе отображения картинки с камеры рисую две точки. Одну в центре кадра, другую в 200 пикселях справа от центра
     
   • Если смотреть на кадр, эти точки падают куда-то на стену, пол или любой другой объект внутри помещения. Вешаю в соответствующие места бумажные наклейки и рисую на них точки маркером
   • Измеряю XYZ координаты этих точек с помощью тех же нивелира и дальномера. Итого для блока из восьми камер нужно измерить координаты самих камер и еще по две точки на каждую. Т.е. 24 тройки координат. А таких блоков должно быть около десяти. Получается долгая муторная работа. Но ничего, позже сделаю калибровку автоматизированной.
   • Запускаю процесс расчета на основе измеренных данных
   
   

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



Идея следующая:

1. Берем исходную матрицу с нулевыми поворотами и смещением.
2. Берем единичный вектор в локальной системе камеры, который смотрит из объектива вперед и преобразуем его в глобальные координаты по исходной матрице.
3. Берем другой вектор в глобальной системе, который из камеры смотрит на центральную точку на стене.
4. С помощью градиентного спуска поворачиваем исходную матрицу так, чтобы после преобразования эти векторы были сонаправлены. Таким образом, мы зафиксировали направление камеры. Осталось зафиксировать вращение вокруг этого направления. Для этого и измерялась вторая точка в 200 пикселях от центра кадра. Поворачиваем матрицу вокруг главной оси, пока два вектора не станут достаточно параллельны.
5. По расстоянию между этими двумя точками вычисляю фокусное расстояния в пикселях (учитывая, что расстояние между проекциями этих точек на кадре составляет 200 пикселей).

Наверняка эту задачу можно было решить аналитически, но для простоты я использовал численное решение на градиентном спуске. Это не страшно, т.к. вычисления будут проводиться один раз после монтажа камер.

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



Тестирование трекинга

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

Вот что вышло:


Похоже на правду, но точность явно могла быть выше. Первая причина несовершенства, которая пришла в голову – искажения в объективах камер. Значит, нужно эти искажения как-то компенсировать.

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



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



А также вектор коэффициентов дисторсии



который позволяет компенсировать искажения объектива с помощью преобразований координат пикселей.

Применяя преобразования с этими коэффициентами к координатам маркера на кадре, можно привести систему к модели идеальной pinhole камеры.

Провожу новый тест трекинга:



Уже гораздо лучше! Выглядит настолько хорошо, что даже вроде будет работать.

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

Вычисление координат маркера

И так, я получил кучу прямых, разбросанных по пространству, на пересечениях которых должны находиться маркеры. Только вот прямые в пространстве на самом деле не пересекаются, а скрещиваются, т.е. проходят на некотором расстоянии друг от друга. Моя задача – найти точку, максимально близкую к обеим прямым. Формально говоря, нужно найти середину отрезка, являющегося перпендикуляром к обеим прямым.



Длина отрезка AB тоже пригодится, т.к. она отражает «качество» полученного результата. Чем он короче, тем ближе друг к другу прямые, тем лучше результат.

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

Параллельно, при разработке костюма с датчиками, я обнаружил странное явление. Все датчики показывали разные значения угла рысканья (направления в горизонтальной плоскости), как будто у каждого был свой север. В первую очередь полез проверять не ошибся ли я в алгоритмах фильтрации данных или в разводке платы, но ничего не нашел. Потом решил посмотреть на сырые данные магнитометра и увидел проблему.
Магнитное поле в нашем помещении было направлено ВЕРТИКАЛЬНО ВНИЗ! Видимо, это связано с железом в конструкции здания.

Но ведь в VR очках тоже используется магнитометр. Почему у них такого эффекта нет? Иду проверять. Оказалось, что в очках он тоже есть… Если сидеть неподвижно, можно заметить, как виртуальный мир медленно, но верно вращается вокруг тебя в рандомную сторону. За минут 10 он уезжает почти на 180 градусов. В нашей игре это неминуемо приведет к рассинхрону виртуальной и реальной реальностей и сломанным об стены очкам.

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



Система в целом работала, хоть и с небольшими косяками. Но было принято решение запустить квест, который как раз был близок к завершению нашим gamedev разработчиком, присоединившимся к нашей миникоманде. Была затречена вся игровая площадь, установлены двери с датчиками и магнитными замками, изготовлено два интерактивных предмета:



Игроки надевали очки, костюмы и рюкзаки-компьютеры и заходили в игровую зону. Координаты трекинга отсылались им по wi-fi и применялись для позиционирования виртуального персонажа. Все работало достаточно неплохо, посетители довольны. Приятнее всего было наблюдать ужас и крики особо впечатлительных посетителей в моменты, когда на них из темноты нападали виртуальные призраки =)

Масштабирование

Внезапно нам прилетел заказ на большой VR шутер на 8 игроков с автоматами в руках. А это 16 объектов, которые нужно тречить. Повезло, что сценарий предполагал возможность разделения трекинга на две зоны по 4 игрока, поэтому я решил, что проблем не будет, можно принимать заказ и ни о чем не волноваться. Протестировать систему в домашних условиях было невозможно, т.к. требовалась большая площадь и много оборудования, которое будет куплено заказчиком, поэтому до монтажа я решил потратить время на автоматизацию калибровки трекинга.

Автокалибровка

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

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

Идея следующая:

1. вешаю камеры и на глаз направляю их в игровую область
2. запускаю режим записи на сервере, в котором все приходящие с камер 2D точки сохраняются в файл
3. хожу по темной игровой локации с маркером в руках
4. останавливаю запись и запускаю расчет калибровочных данных, при котором вычисляются расположения, ориентации и фокусные расстояния всех камер.
5. в результате предыдущего пункта получается единое пространство, наполненное камерами. Т.к. это пространство не привязано к реальным координатам, оно имеет случайное смещение и поворот, которое я вычитаю вручную.

Пришлось перелопатить огромное количество материала по линейной алгебре и написать многие сотни строк питонского кода. Настолько много, что я уже почти не помню как оно работает.





Вот так выглядит напечатанная на принтере специальная палка-калибровалка.

Тестирование большого проекта

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

Когда уровень кортизола в крови превысил теоретический максимум, я решил посмотреть на проблему с другой стороны. Как можно сократить количество разноцветных точек, не сокращая количество маркеров? Сделать трекинг активным. Пускай у каждого игрока, например, левый рог всегда корит красным. А второй иногда загорается зеленым по приходу команды с сервера так, что в один момент времени он горит только у одного игрока. Получается, что зеленая лампочка будет как-будто перепрыгивать с одного игрока на другого, обновляя привязку трекинга к красной лампочке и обнуляя ошибку ориентации магнитометра.

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



Алгоритмы трекинга пришлось заметно усложнить, но в итоге все заработало! Телепортации игроков друг в друга исчезли, нагрузка на процессор упала, засветки перестали мешать.


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

Чем все закончилось?

За 3 года мы открыли множество развлекательных точек по всему миру, но коронавирус внес свои коррективы, что дало нам возможность сменить направление работы в более общественно-полезную сторону. Теперь мы довольно успешно занимаемся разработкой медицинских симуляторов в VR. Команда у нас все еще маленькая и мы активно стремимся расширять штат. Если среди читателей Хабра есть опытные разработчики под UE4, ищущие работу, пожалуйста, напишите мне.

Традиционный забавный момент в конце статьи:

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



Система в прямом смысле работала через задницу.