Опубликовано 11 февраля 2009 в 13:15:00 | Coding4Fun

В этой статье я расскажу о системе виртуальной реальности с двумя устройствами Nintendo Wii Remotes (Wiimotes) и небольшим количеством дополнительного оборудования стоимостью менее $10.

Тимо Флейш (Timo Fleisch (EN)).

Сложность: высокая.

Необходимое время: 3-4 часа.

Цена: : $80 на два Wiimotes, ~$20 на индикаторы, подставки, держатель батареек.

ПО: Visual C# Express Edition 2008, XNA Games Studio 3.0, Managed Library for Nintendo's Wiimote, OpenCV Computer Vision Library и EmguCV C# Wrapper.

Оборудование: два Nintendo Wii Remote (Wiimote), очки Anaglyph 3D Glasses (красно-зеленые очки) или монитор Zalman 3D, PC-совместимый адаптер Bluetooth и полка.

Исполняемая программа: Загрузить

Исходные тексты: Загрузить

Введение

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

  • возможностью полного погружения;
  • интерактивностью.

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

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

В этой статье я расскажу о системе виртуальной реальности с двумя устройствами Nintendo Wii Remote (Wiimote) и небольшим количеством дополнительного оборудования стоимостью менее $10. Тем самым ВР становится доступной каждому. Демонстрационную программу и исходные коды можно загрузить с моего веб-сайта:

http://www.vrhome.de.

Настольная система виртуальной реальности

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

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

Второй Wiimote прикреплен над монитором и направлен сверху вниз на область перед экраном. Это устройство будет использовано для взаимодействия с 3D-сценой путем манипулирования преобразованиями 3D-объекта. Поскольку для данного взаимодействия используется рука, я назвал это отслеживанием руки. Настольная система виртуальной реальности показана на рис. 1.

Отслеживание 6СС Wiimote

Одна из основных идей описываемой конфигурации ВР — применение в качестве следящей камеры устройства Nintendo Wiimote Controller. В Wiimote есть инфракрасная камера, способная распознавать до четырех инфракрасных сигналов. Теперь главная задача — сделать устройство с четырьмя ИК-светодиодами, которые могут быть распознаны Wiimote. Я назвал это устройство инфракрасным светодиодным маяком. Используя значения, получаемые Wiimote от четырех светодиодов, я вычисляю исходное положение и ориентацию маяка с помощью некоторого алгоритма. Полученные смещения по всем трем осям и углы поворота относительно трех осей я называю шестью отслеживаемыми степенями свободы устройства или сокращенно 6СС.

ИК-маяк

 

Рис. 1. Настольная система ВР на базе Wiimote

 

Поскольку в нашей системе ведется как отслеживание головы, так и отслеживание манипуляций руками, я сделал два маяка. Они показаны на рис. 2 и 3.

Рис. 2. Маяк слежения за головой

Рис. 3. Маяк слежения за рукой

 

Как видите, в состав маяков входит по четыре ИК-светодиода, держатели батареек и провода. Для облегчения монтажа и уменьшения числа проводов я использовал монтажную плату. При выборе ИК-светодиодов важно убедиться, что у них широкий угол излучения. У обычных светодиодов угол довольно малый, нам же требуется более 120° (поищите в digikey.com). Три из четырех светодиодов монтируются на одной линии, с небольшой разницей по высоте. Четвертый — над этой линией на большей высоте. Чтобы алгоритм мог устанавливать соответствие между ИК-сигналами, распознанными Wiimote, и светодиодами маяка, необходим особый порядок расположения светодиодов. Важно также расположить четвертый излучатель на высоте, отличающейся от других светодиодов, чтобы они были не в одной плоскости. Схема маяка показана на рис. 4. В качестве источника питания я использовал батарейку AAA, к которой подключил параллельно все светодиоды. Для удобства я использовал держатель батарейки, который можно найти в любом магазине радиодеталей.

Собрав все вместе, надо аккуратно разместить светодиоды, поскольку их расположение очень важно для определения трехмерных координат. Если данные этих координат будут неточные, результаты слежения будут плохими. Значения должны быть в миллиметрах.

Рис. 4. Схема маяка с ИК-светодиодами

Мои светодиоды, например, имеют следующие координаты:

   1: <point3d value="1, 0, 8.5"/>
   2: <point3d value="29, 0, 11"/>
   3: <point3d value="56, 0, 6"/>
   4: <point3d value="29, 45, 21"/>

 

Настройка ПО

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

   1: Settings.xml
   2: <Settings> 
   3:     <Stereo
   4:         eyeDistance = "0.02"
   5:         switchLeftRight = "False"
   6:         fieldOfView = "60"
   7:         antiAlias = "False"
   8:         stereoMode = "lineInterlaced"
   9:         fullscreen = "False"
  10:         resolution = "1280,1024"
  11:         displayDevice = "Screen"
  12:         anaglyph = "True"
  13:         windowPosition = "0,0" />
  14: </Settings>

 

Если у вас есть 3D-монитор Zalman, измените значение anaglyph на False. Если вам трудно сфокусировать взгляд, чтобы наблюдать стереоизображение, попробуйте изменить значение eyeDistance. Уменьшение этого значения упростит фокусировку, но уменьшит эффект трехмерности. Назначение остальных параметров должно быть понятно из их названий.

Tracker.xml

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

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

   1: <TrackerCam 
   2:     id="2" cameramodel="Wiimote" 
   3:     translation="0,200,50" 
   4:     rotation="0,0,0" scale="0.001" 
   5:     xAxis="x" yAxis="y" zAxis="z"> 
   6: </TrackerCam>
   7: <TrackerCam
   8:     id="1" cameramodel="Wiimote"
   9:     translation="0,350,350" rotation="0,0,0" scale="0.001"
  10:     xAxis="x" yAxis="z" zAxis="y">
  11: </TrackerCam>

 

Wiimote с id=2 — это устройство, расположенное сверху на мониторе, которое служит для слежения за головой. В поле translation введите расстояние от переднего края этого Wiimote до центра экрана. Параметры по умолчанию: 200 мм по вертикали (y-координата) и 50 мм вглубь (z-координата) от центра. То же проделайте для Wiimote с id=1. Здесь вы видите, что по оси y измеряется z-координата, а по оси z — y-координата. Это сделано потому, что данный Wiimote установлен в вертикальном положении. Если ПО распознает устройства Wiimote не в том порядке, поменяйте блоки определения их параметров местами.

Еще вам надо внести изменения в настройки маяков:

   1: <MarkerBody>
   2:     <WiiMarkerBody id="0" name="WiiMote Head Beacon" nearClip="20"
   3:         farClip="1500" translation="0,0,0" rotation="0,0,0">
   4:         <point3d value="0, 4, 7"/>
   5:         <point3d value="40, 4.5, 10.5"/>
   6:         <point3d value="83.5, 5, 7.5"/>
   7:         <point3d value="38, 45, 18"/>
   8:     </WiiMarkerBody>
   9:     <WiiMarkerBody id="4" name="WiiMote Hand Beacon" nearClip="20" 
  10:         farClip="1500" translation="0,0,0" rotation="0,0,0">
  11:         <point3d value="1, 0, 8.5"/>
  12:         <point3d value="29, 0, 11"/>
  13:         <point3d value="56, 0, 6"/>
  14:         <point3d value="29, 45, 21"/>
  15:     </WiiMarkerBody>
  16: </MarkerBody>

 

Здесь надо поменять координаты маяков в миллиметрах в соответствии с вашими измерениями. Имейте в виду, что важен правильный порядок этих координат. Они идут от LED1 до LED4 в соответствии со схемой на рис. 4.

Есть и описание отслеживаемого устройства:

   1: <TrackedDevice id="4" type="WiiMote" rotation="True" translation="True">
   2:     <MarkerBodyId>0</MarkerBodyId>
   3:     <TrackerCamId>2</TrackerCamId>
   4:     <ReverseTranslation>False,True,True</ReverseTranslation>
   5:     <ReverseRotation>False,False,False</ReverseRotation>
   6:     <LocalTranslation>-45,25,0</LocalTranslation>
   7:     <LocalRotation>0,0,0</LocalRotation>
   8:     <WorldTranslation>0,0,0</WorldTranslation>
   9:     <WorldRotation>0,0,0 </WorldRotation>
  10:     <RotationFilterId>4</RotationFilterId>
  11:     <TranslationFilterId>4</TranslationFilterId>
  12: </TrackedDevice>

 

Здесь вы устанавливаете соответствие между Wiimote (TrackedCamId) и маяком (MarkerBodyId). Единственное значение, подлежащее настройке, это LocalTranslation. Для вычисления значений смещения (translation) и поворота (rotation) требуется точка отсчета на маяке. По умолчанию за нее принимается первый светодиод. Вам может быть удобней другая точка, например, центр маяка. В этом случае измерьте расстояние от первого светодиода до предпочитаемого вами начала координат в тех единицах измерения, что применяются в маяке, присвойте полученное значение параметру LocalTranslation.

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

Работа программ

Перед запуском программ надо подключить Wiimote к компьютеру. Для этого компьютер должен иметь адаптер Bluetooth. Подробности см. в статье Брайана Пика (Brian Peek) о библиотеке Wiimote Library:

http://blogs.msdn.com/coding4fun/archive/2007/03/14/1879033.aspx

После того как вы внесли свои значения в конфигурационные файлы и подключили два Wiimote к компьютеру, можно запустить установку исполняемой программы, щелкнув VRDesktoDemo в меню Пуск. Если вы хотите использовать исходный код, перед запуском приложения скопируйте dll-библиотеки OpenCV из (VRDesktopSrc)\ExtLibs\OpenCV\opencvlib в целевой каталог для двоичных файлов компилируемого проекта, например: (VRDesktopSrc)\VRDesktopDemo\bin\x86\Release.

Использование библиотеки

Использовать VRDesktop в XNA-приложении не составляет труда. Я для пример�� опишу действия, требуемые в приложении VRDesktopDemo.

Чтобы начать с нуля, надо создать новый проект XNA Windows Game Project. Сначала добавьте ссылки на две библиотеки — Tgex и Tvrx. Затем откройте класс Game и добавьте вначале пространства имен для этих библиотек:

   1: using Tgex.Graphics;
   2: using Tgex;
   3: using Tvrx;

Затем необходимо изменить родительский класс с Game на VRGame:

   1: public class VRDesktop : VRGame

 

VRGame является частью библиотеки Tgex и реализует поддержку стереодисплея. Он создает стереокамеру и окно приложения в соответствии с параметрами конфигурационного файла. Использовать этот класс можно примерно так же, как исходный класс Game. Основное отличие в том, что вы не должны переопределять метод Draw(GameTime time), а должны добавить новый метод DrawScene(GameTime time), поскольку метод Draw класса VRGame обеспечивает стереоскопическую визуализацию.

Для хранения матрицы преобразований слежения за рукой мы определяем такую переменную:

   1: Matrix modelTransform = Matrix.Identity;

 

А также в этом простом примере мы определяем переменную для модели:

   1: Model model;

 

В функции Initialize надо проинициализировать TrackerManager:

   1: protected override void Initialize()
   2: {
   3:     // Диспетчер отслеживателя является одноэлементным, но один раз должен быть проинициализирован.
   4:     TrackerManager.Instance.Initialize();
   5:  
   6:     // инициализация базового класса
   7:     base.Initialize();
   8: }

 

В методе LoadContent загружается модель и начинается слежение:

   1: protected override void LoadContent()
   2: {
   3:     // в данном примере загружаем координатные оси
   4:     model = Content.Load<Model>("coordinate");
   5:     modelTransforms = new Matrix[model.Bones.Count];
   6:  
   7:     // начинаем слежение
   8:     TrackerManager.Instance.StartTracking();
   9: }

 

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

   1: // Стандартный выход из игры в Xbox 360 и Windows
   2: if ((GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
   3:     || (Keyboard.GetState().IsKeyDown(Keys.Escape)))
   4: {
   5:     TrackerManager.Instance.StopTracking();
   6:     this.Exit();
   7: }

 

Перед получением последних данных слежения надо обновить TrackerManager:

   1: TrackerManager.Instance.Update();

 

Для получения данных смещения вызывается метод GetProxyTransform(indexNumber) диспетчера. Прокси определяются в файле tracking.xml . В нашем примере вызов такой:

   1: // id 1 соответствует слежению за рукой
   2: modelTransform = TrackerManager.Instance.GetProxyTransform(1);
   3:  
   4: // id 0 соответствует слежению за головой, т. е. за изменением положения глаз.
   5: m_camera.EyePosition = 
   6:     TrackerManager.Instance.GetProxyTransform(0).Translation;

 

m_camera определен в родительском классе VRGame. Класс камеры также выполняет преобразования, необходимые для реализации перспективы.

В завершение в DrawScene прорисовывается модельная сетка. Нам надо передать в этот эффект матрицу преобразования модели и матрицы камеры:

   1: // Рисуем модель в цикле, т. к. она может иметь несколько сеток
   2: foreach (ModelMesh mesh in model.Meshes)
   3: {
   4:     // Здесь устанавливается ориентация сетки, а также 
   5:     // камера и проекция
   6:     foreach (BasicEffect effect in mesh.Effects)
   7:     {
   8:         effect.EnableDefaultLighting();
   9:         effect.World = modelTransforms[mesh.ParentBone.Index]
  10:                        * Matrix.CreateScale(0.01f) // coordinate scale
  11:                        * modelTransform;
  12:         effect.View = m_camera.ViewMatrix;
  13:         effect.Projection = m_camera.ProjectionMatrix;
  14:     }
  15:  
  16:     // рисуем сетку, к которой будет применен ранее установленный эффект.
  17:     mesh.Draw();
  18: }

 

Это все, что надо сделать в вашем собственном приложении для работы с настольной системой ВР.

Принцип работы

Для особо любопытных подробно опишу работу системы слежения на базе Wiimote. Математические выкладки для базового алгоритма я все же приводить не буду, а лишь дам вам необходимые ссылки. Я остановлюсь лишь на той части, которая касается слежения с помощью Wiimote, и не буду подробно рассматривать ни библиотеку Tvrx, ни игровую библиотеку Tgex.

Определение положения

Выражаясь техническим языком, слежение с помощью Wiimote — это система слежения, основанная на оптических маркерах. Она оптическая, потому что используется камера Wiimote, маркерная, поскольку мы не используем все изображение с камеры. Wiimote возвращает местоположение до четырех инфракрасных светодиодов, которые и являются нашими маркерами.

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

Главная задача — вычислить позицию и наклон ИК-светодиодного маяка в реальном пространстве в соответствии с точками изображения светодиодов, измеренными Wiimote. Эта задача называется «определение положения» (Pose Estimation); она изучается учеными многие годы. Одно из приложений определения положения — машинное зрение в робототехнике. Алгоритм определения положения, используемый в слежении с помощью Wiimote, был опубликован еще в 1995 г. и для наших нужд вполне подходит. Если вы хотите узнать все подробности работы алгоритма определения положения, обратитесь к первоисточнику.

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

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

Фокусное расстояние в пикселах = 1380.

   1: // предполагаем, что матрица 1/4"
   2: pixel size in mm = 0.0035
   3: chip resolution = 1024x768
   4:  
   5: // центр Wiimote center (приблизительно)
   6: principal point = 512x384

 

Значение разрешающей способности, возвращаемое Wiimote, равно1024×768. Очевидно, что это не физическая разрешающая способность, поскольку подобные камеры должны стоить более 1000 долларов. В Wiimote стоит матрица от PixArt Imaging Inc. (http://www.pixart.com.tw) и, вероятно, ее разрешающая способность равна 352×288 или 164×124. Между тем, значения для матриц PixArt не дают корректного результата, так что я остановился на приведенных выше значениях. Важно, что в совокупности эти значения дают хороший результат, хотя каждое из них в отдельности может быть и неверно. Главная точка — это действительная точка начала координат поверхности изображения. В идеале эту точку необходимо измерить, я же просто предположил, что она находится в середине светочувствительной матрицы.

Обзор алгоритма слежения

Алгоритм слежения может быть разбит на следующие этапы:

  • получение от Wiimote точек изображения ИК-светодиодов;
  • связывание этих точек изображения со светодиодами маяка;
  • выполнение алгоритма определения положения для вычисления поворота и смещения маяка;
  • фильтрация вычисленных значений поворота и смещения;
  • построение матрицы преобразования и пересчет результатов в соответствии с конфигурационным файлом;
  • (положение и ориентация Wiimote и локальные преобразования для маяка).

Далее я опишу эти этапы подробней и приведу примеры программ.

Получение точек изображения

Подключение к Wiimote и получение данных из этого устройства осуществляется с помощью класса WiiMoteTracker. В этом классе реализован интерфейс IMarkerTracker, определяющий взаимодействие с устройством слежения на базе оптических маркеров. Инициализируется Wiimote функцией Initialize(), а подключается при вызове StartTracking():

   1: public void Initialize()
   2: {
   3:     // проверка статической переменной при первом обращении
   4:     if (m_wiimoteCount == 0)
   5:         m_wiimoteCollection.FindAllWiimotes();
   6:  
   7:     if (m_wiimoteCollection.Count <= m_wiimoteCount)
   8:     {
   9:         ErrorHandler.Report("Invalid WiimoteTracker count, only "
  10:         + m_wiimoteCollection.Count.ToString() + " Wiimotes found");
  11:         return;
  12:     }
  13:  
  14:     wm = m_wiimoteCollection.ElementAt(m_wiimoteCount);
  15:     m_wiimoteId = m_wiimoteCount;
  16:     m_wiimoteCount++;
  17:  
  18:     // установка обработчика события для обработки изменений состояния
  19:     wm.WiimoteChanged += wm_WiimoteChanged;
  20:  
  21:     // установка обработчика события для обработки добавления/удаления расширений
  22:     wm.WiimoteExtensionChanged += wm_WiimoteExtensionChanged;
  23:  
  24:     // создание фильтра для значений акселератора
  25:     AverageFilterDesc filterDesc = new AverageFilterDesc();
  26:     filterDesc.numOfValues = 1000;
  27:  
  28:     for (int i = 0; i < 3; i++)
  29:     {
  30:         m_acceleratorFilter[i] = new AverageFilter();
  31:         m_acceleratorFilter[i].SetFilterDesc(filterDesc);
  32:     }
  33:  
  34:     // создание фильтра для точек изображения
  35:     filterDesc = new AverageFilterDesc();
  36:     filterDesc.numOfValues = 5;
  37:  
  38:     for (int i = 0; i < 8; i++)
  39:     {
  40:         m_imagePointsFilter[i] = new AverageFilter();
  41:         m_imagePointsFilter[i].SetFilterDesc(filterDesc);
  42:     }
  43: }
  44:  
  45: public void StartTracking()
  46: {
  47:     if (!m_isTracking)
  48:     {
  49:         m_isTracking = true;
  50:  
  51:         // подключение к Wiimote
  52:         try
  53:         {
  54:             wm.Connect();
  55:  
  56:             // установить тип отчета для возврата данных ИК-датчика и акселерометра 
  57:             // (от кнопок данные всегда возвращаются)
  58:             wm.SetReportType(InputReport.IRAccel, true);
  59:             wm.SetLEDs(m_wiimoteId);
  60:         }
  61:         catch
  62:         {
  63:             ErrorHandler.Report("Cannot connect to Wiimote");
  64:             m_isTracking = false;
  65:         }
  66:     }
  67: }

 

Для получения данных Wiimote регистрируется функция обратного вызова wm_WiimoteChanged. Затем эта функция вызывается при каждом изменении значений Wiimote. Эта функция считывает значения, соответствующие светодиодам, и устанавливает соответствия для маяка.

Затем создается Vector2 с точками изображения:

   1: // записать список
   2: irList.Clear();
   3:  
   4: for (int i = 0; i < 4; i++)
   5:     irList.Add(new Vector2((float)
   6:            (ws.IRState.IRSensors[i].Position.X * m_resolution.X) - m_principalPoint.X,
   7:            (float)(ws.IRState.IRSensors[i].Position.Y * m_resolution.Y) - m_principalPoint.Y));

 

Определение точек изображения

Теперь необходимо присвоить значения для ИК-светодиодов, поместив их в правильном порядке. Это делается простым распознаванием геометрических объектов. Идея заключается в том, чтобы иметь геометрическую модель, инвариантную к двух- и трехмерным проекциям. Как видно на рис. 4, три светодиода маяка расположены более или менее по прямой, а четвертый — над этой прямой. В двухмерных данных изображения Wiimote, три светодиода также находятся примерно по прямой. Следовательно, первый шаг алгоритма присваивания — найти три наиболее «выровненные» точки изображения. Этот анализ выполняется следующей функцией:

   1: void TestPoints(Vector2 lineStartPoint, Vector2 lineEndPoint,
   2:                 Vector2 onLinePoint, Vector2 freePoint)
   3: {
   4:     float lambda;
   5:     float dist = onLinePoint.DistanceToLine(lineStartPoint, lineEndPoint, out lambda);
   6:  
   7:     // проверить, находится ли проецируемая точка между начальной и конечной точками отрезка прямой
   8:     if ((lambda > 0) && (lambda < 1))
   9:     {
  10:         // если расстояние маленькое, считать эту комбинацию искомым результатом
  11:         if (dist < m_pointLineDist)
  12:         {
  13:             m_pointLineDist = dist;
  14:             m_lineStartPoint = lineStartPoint;
  15:             m_lineEndPoint = lineEndPoint;
  16:             m_onLinePoint = onLinePoint;
  17:             m_freePoint = freePoint;
  18:         }
  19:     }
  20: }

 

Входными параметрами этой функции являются четыре точки изображения. Предполагается, что первая точка является началом отрезка прямой, а вторая — его окончанием. Затем вычисляется расстояние третьей точки от этого отрезка. Это делается функцией C# 3.0 Vector2:

   1: public static float DistanceToLine(this Vector2 point,
   2:                                    Vector2 startLinePoint,
   3:                                    Vector2 endLinePoint,
   4:                                    out float lambda)
   5: {
   6:     Vector2 rv = endLinePoint - startLinePoint;
   7:     Vector2 p_ap = point - startLinePoint;
   8:     float dot_rv = Vector2.Dot(rv, rv);
   9:     lambda = Vector2.Dot(p_ap, (rv / dot_rv));
  10:     Vector2 distVec = point - (startLinePoint + lambda * rv);
  11:     return distVec.Length();
  12: }

 

Вычисление расстояния до прямой – это распространенный алгоритм, описанный здесь: http://mathenexus.zum.de/html/geometrie/abstaende/AbstandPG.htm (DE).

Он возвращает расстояние до прямой и лямбда-значение, определяющее положение проекции точки на линию. Если проекция точки находится за пределами отрезка, определенного начальной и конечной точками, лямбда будет меньше 0 или больше 1.

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

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

   1: m_pointLineDist = float.MaxValue;
   2:  
   3: // Явно вызываем все проверки
   4: // В результате должен получиться отрезок из трех точек
   5: TestPoints(irList[0], irList[1], irList[2], irList[3]);
   6: TestPoints(irList[0], irList[1], irList[3], irList[2]);
   7: TestPoints(irList[0], irList[2], irList[1], irList[3]);
   8: TestPoints(irList[0], irList[2], irList[3], irList[1]);
   9: TestPoints(irList[0], irList[3], irList[1], irList[2]);
  10: TestPoints(irList[0], irList[3], irList[2], irList[1]);
  11: TestPoints(irList[1], irList[2], irList[0], irList[3]);
  12: TestPoints(irList[1], irList[2], irList[3], irList[0]);
  13: TestPoints(irList[1], irList[3], irList[0], irList[2]);
  14: TestPoints(irList[1], irList[3], irList[2], irList[0]);
  15: TestPoints(irList[2], irList[3], irList[0], irList[1]);
  16: TestPoints(irList[2], irList[3], irList[1], irList[0]);

 

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

   1: // осталось проверить, в правильном ли 
   2: // порядке представлены начальная и конечная точки
   3: // сравним их положение с произвольной точкой
   4: // если направление по часовой стрелке, значит все в порядке, иначе 
   5: // поменяем местами начальную и конечную точки
   6:  
   7: Vector2 E1 = m_lineStartPoint - m_freePoint; // P1-P2 
   8: Vector2 E2 = m_lineEndPoint - m_freePoint; // P3-P2 
   9: bool clockwise;
  10: if ((E1.X * E2.Y - E1.Y * E2.X) >= 0)
  11:     clockwise = true;
  12: else
  13:     clockwise = false;
  14:  
  15: if (!clockwise)
  16: {
  17:     Vector2 tmp = m_lineEndPoint;
  18:     m_lineEndPoint = m_lineStartPoint;
  19:     m_lineStartPoint = tmp;
  20: }

 

Этот алгоритм взят с сайта http://www.geocities.com/siliconvalley/2151/math2d.html (EN).

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

   1: // сохраняем окончательные координаты
   2: m_imagePoints[0].X = m_imagePointsFilter[0].Filter(m_lineStartPoint.X);
   3: m_imagePoints[0].Y = m_imagePointsFilter[1].Filter(m_lineStartPoint.Y);
   4: m_imagePoints[1].X = m_imagePointsFilter[2].Filter(m_onLinePoint.X);
   5: m_imagePoints[1].Y = m_imagePointsFilter[3].Filter(m_onLinePoint.Y);
   6: m_imagePoints[2].X = m_imagePointsFilter[4].Filter(m_lineEndPoint.X);
   7: m_imagePoints[2].Y = m_imagePointsFilter[5].Filter(m_lineEndPoint.Y);
   8: m_imagePoints[3].X = m_imagePointsFilter[6].Filter(m_freePoint.X);
   9: m_imagePoints[3].Y = m_imagePointsFilter[7].Filter(m_freePoint.Y);
  10:  
  11: for (int i = 0; i < 4; i++)
  12: {
  13:     m_imagePoints[i] *= m_pixelSize;
  14: }
  15:  
  16: // передаем координаты для определения положения
  17: m_poseEstimate.UpdateImagePoints(m_imagePoints);
  18:  

 

Определение положения

Определение положения выполняется в классе Posit, в котором реализован интерфейс IPoseEstimate:

   1: public interface IPoseEstimate
   2: {
   3:     void InitializeCameraParameter(double focalLengthMM, bool flipImage,
   4:                         float scale, int[] assignAxis, int[] assignAxisSign);
   5:     void InitializeMarkerBody(Vector3[] markerPoints);
   6:     void UpdateImagePoints(Vector2[] imagePoints);
   7:     void GetTransform(out Vector3 position, out Vector3 rotation);
   8:     void StartEstimation();
   9:     void StopEstimation();
  10: }

 

При инициализации указывается фокусное расстояние камеры слежения. 3D-координаты реального устройства (в нашем случае маяка) передаются функции InitializateMarkerBody. Измеренные координаты передаются UpdateImagePoints и полученный результат может быть получен из функции GetTransform. Поскольку сам алгоритм вычисления положения выполняется асинхронно в собственном потоке, он должен запускаться и останавливаться методами StartEstimation и StopEstimation. Применение интерфейса позволяет легко подключать различные алгоритмы определения положения.

Как я уже говорил, применяемый здесь алгоритм определения положения, это алгоритм PosIt, опубликованный Д. ДеМентоном (D. DeMenthon). Я использовал его реализацию из библиотеки машинного зрения OpenCV. Поскольку эта библиотека написана на C, для нее необходима оболочка управляемого кода. Я использовал свободно распространяемую оболочку EmguCV. Перед определением положения необходимо создать объект положения. Это делается при передаче координат маркеров:

   1: public void InitializeMarkerBody(Vector3[] markerPoints)
   2: {
   3:     m_numOfMarker = markerPoints.Length;
   4:     MCvPoint3D32f[] worldMarker = new MCvPoint3D32f[m_numOfMarker];
   5:     for (int i = 0; i < m_numOfMarker; i++)
   6:     {
   7:         worldMarker[i].x = markerPoints[i].X;
   8:         worldMarker[i].y = markerPoints[i].Y;
   9:         worldMarker[i].z = markerPoints[i].Z;
  10:     }
  11:     m_positObject = CvInvoke.cvCreatePOSITObject(
  12:     worldMarker, m_numOfMarker);
  13:     m_imagePoints = new MCvPoint2D32f[m_numOfMarker];
  14:     m_imagePointsBuffer = new Vector2[m_numOfMarker];
  15: }

 

MCvPoint32f — это управляемая структура для соответствующей структуры из OpenCV, аналогичная Vector3f. Класс CvInvoke из оболочки EmguCV является набором статических функций, вызывающих исходные функции OpenCV. Поскольку алгоритм определения положения не был включен в этот класс, я добавил следующие функции:

   1: /// <summary>
   2: /// Создание объекта определения положения
   3: /// </summary>
   4: [DllImport(CV_LIBRARY)]
   5: public static extern IntPtr cvCreatePOSITObject(MCvPoint3D32f[] points, 
   6:                                                 int point_count);
   7: /// <summary>
   8: /// Определение положения
   9: /// </summary>
  10: [DllImport(CV_LIBRARY)]
  11: public static extern void cvPOSIT(IntPtr posit_object, 
  12:                                   MCvPoint2D32f[] image_points,
  13:                                   double focal_length,
  14:                                   MCvTermCriteria criteria,
  15:                                   float[] rotation_matrix,
  16:                                   float[] translation_vector);
  17: /// <summary>
  18: /// Освобождение объекта определения положения
  19: /// </summary>
  20: [DllImport(CV_LIBRARY)]
  21: public static extern void cvReleasePOSITObject(IntPtr posit_object);

 

Функция CvInvoke.cvCreatePOSITObject возвращает обычный IntPtr, который в дальнейшем используется в функции определения положения PoseEstimate(), которая выполняется в собственном потоке.

Сначала анализируются новые координаты. Если обновления не было, мы ожидаем новых значений. Это делается методами Monitor.Wait и Monitor.Pulse:

   1: // копировать координаты изображения
   2: Monitor.Enter(m_imagePointsLock);
   3: if (!m_imagePointsUpdate)
   4:     Monitor.Wait(m_imagePointsLock);
   5:  
   6: for (int i = 0; i < m_numOfMarker; i++)
   7: {
   8:     m_imagePoints[i].x = m_imagePointsBuffer[i].X;
   9:     m_imagePoints[i].y = m_imagePointsBuffer[i].Y;
  10: }
  11: m_imagePointsUpdate = false;
  12: Monitor.Exit(m_imagePointsLock);

 

После получения новых координат изображения вызывается функция cvPOSIT:

   1: MCvTermCriteria criteria;
   2: criteria.type = TERMCRIT.CV_TERMCRIT_EPS | TERMCRIT.CV_TERMCRIT_ITER;
   3: criteria.epsilon = 0.00001;
   4: criteria.max_iter = 500;
   5: CvInvoke.cvPOSIT(m_positObject,m_imagePoints, m_focalLengthMM,
   6:                  criteria, POSITRot, POSITTrans);

 

Поскольку алгоритм является итеративным, необходим критерий выхода из него, для чего используется MCvTermCriteria. В данном случае я определил, что его выполнение прерывается после 500 итераций или когда разница вычисленных значений в двух соседних итерациях менее 0.00001. Вы можете поэкспериментировать с этими значениями и посмотреть, как это скажется на точности слежения. Кроме критерия прекращения, вы должны передать функции cvPOSIT IntPtr на объект определения положения, координаты изображения и фокусное расстояние камеры в миллиметрах. В качестве результата вы получаете массив значений с плавающей точкой для матрицы поворотов и массив значений с плавающей точкой для смещений.

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

Фильтрация результатов определения положения

Поскольку разрешающая способность камеры Wiimote не слишком высока и оптическое слежение вообще характеризуется наличием флуктуаций, результаты преобразований содержат приличные шумы. Для их снижения результирующие значения необходимо подвергнуть фильтрации. Побочным эффектом жесткой фильтрации является появления шлейфа за движущимися объектами. Хорошим компромиссом между уменьшением флуктуаций и прямой передачей является применение фильтров Калмана. Они построены на математической модели, в соответствии с которой выполняется прогнозирование изменения значений, а затем измеренные значения применяются для корректировки данного прогноза. Хороший ознакомительный курс по фильтрам Калмана Siggraph 2001 Course Грега Уэлша (Greg Welch) (см. ссылки на ресурсы в конце статьи). В любом случае, определение наилучших параметров фильтрации для не-математика является сложным делом. Хорошим источником информации о применении фильтров при слежении является диссертация Рональда Азума (Ronald Azuma) «Прогнозирующее слежение для расширенной реальности» («Predictive Tracking for Augmented Reality»). Ознакомьтесь с этой работой, если хотите разобраться в упомянутых параметрах. Для системы виртуальной реальности на базе Wiimote параметры фильтра Калмана определены в файле tracking.xml следующим образом:

   1: <Kalman class="KalmanFilter" id="4" A="1, 0.005, 0, 1" 
   2:     measurement_noise_cov="1.0" process_noise_cov_1="0.0000001"
   3:     process_noise_cov_2="0.0000001"/>

 

Реализация этого фильтра, опять же, является частью библиотеки OpenCV. Классы-оболочки содержатся в EmguCV. В моей программе для фильтров данных используется интерфейс IDataFilter. Реализован этот интерфейс классом KalmanFilter. Кроме фильтра Калмана в библиотеке есть также простой фильтр AvarageFilter. Вот как инициализируется фильтр Калмана:

   1: public void SetFilterDesc(DataFilterDesc desc)
   2: {
   3:     m_kalman = new Kalman(2, 1, 0);
   4:     filterDesc = (KalmanFilterDesc)desc;
   5:     // устанавливаем A, второй параметр – число кадров в сек.
   6:     m_kalman.TransitionMatrix.Data.SetValue(filterDesc.A[0], 0, 0);
   7:     m_kalman.TransitionMatrix.Data.SetValue(filterDesc.A[1], 0, 1);
   8:     m_kalman.TransitionMatrix.Data.SetValue(filterDesc.A[2], 1, 0);
   9:     m_kalman.TransitionMatrix.Data.SetValue(filterDesc.A[3], 1, 1);
  10:     // устанавливаем H
  11:     m_kalman.MeasurementMatrix.Data.SetValue(1.0f, 0, 0);
  12:     m_kalman.MeasurementMatrix.Data.SetValue(0.0f, 0, 1);
  13:     // устанавливаем Q
  14:     CvInvoke.cvSetIdentity(m_kalman.ProcessNoiseCovariance.Ptr, new MCvScalar(1));
  15:     m_kalman.ProcessNoiseCovariance.Data.SetValue(filterDesc.process_noise_cov_1, 0, 0);
  16:     m_kalman.ProcessNoiseCovariance.Data.SetValue(filterDesc.process_noise_cov_2, 1, 0);
  17:     // устанавливаем R
  18:     CvInvoke.cvSetIdentity(m_kalman.MeasurementNoiseCovariance.Ptr, new MCvScalar(1e-5));
  19:     m_kalman.MeasurementNoiseCovariance.Data.SetValue(filterDesc.measurement_noise_cov, 0, 0);
  20:     CvInvoke.cvSetIdentity(m_kalman.ErrorCovariancePost.Ptr, new 
  21:     MCvScalar(500));
  22:     m_kalman.ErrorCovariancePost.Data.SetValue(2, 0, 0);
  23: }

 

После инициализации значение с плавающей точкой просто передается функции Filter:
   1: public float Filter(float inData)
   2: {
   3:     // координата Z 
   4:     data.Data[0,0] = inData;
   5:     m_kalman.Predict(predict);
   6:     m_kalman.Correct(data);
   7:     return m_kalman.CorrectedState[0, 0];
   8: }

 

Поскольку в результирующем преобразовании есть три значения смещения и три значения поворота, требуется 6 отдельных экземпляров фильтра. В библиотеке Tvrx фильтрация выполняется в классе TrackedDevice, порождаемого из TrackedWiimote, и являющегося родительски для отслеживаемых устройств:

   1: public virtual void Filter() 
   2: {
   3:     m_rawTranslation.X = m_translationFilter[0].Filter(m_rawTranslation.X);
   4:     m_rawTranslation.Y = m_translationFilter[1].Filter(m_rawTranslation.Y);
   5:     m_rawTranslation.Z = m_translationFilter[2].Filter(m_rawTranslation.Z);
   6:     m_rawRotation.X = m_rotationFilter[0].Filter(m_rawRotation.X);
   7:     m_rawRotation.Y = m_rotationFilter[1].Filter(m_rawRotation.Y);
   8:     m_rawRotation.Z = m_rotationFilter[2].Filter(m_rawRotation.Z);
   9: }

 

Окончательные преобразования

В классе TrackedDevice также выполняется преобразование значений из системы координат камеры в систему координат игры:

   1: public virtual void TransformToVirtualSpace() 
   2: {
   3:     Matrix bodyTransformMatrix =
   4:     Matrix.CreateFromYawPitchRoll(m_rawRotation.Y, m_rawRotation.X, m_rawRotation.Z)
   5:                                   * Matrix.CreateTranslation(m_rawTranslation);
   6:     Matrix result = m_TrackerWorldTransform * m_DeviceWorldTransform;
   7:     result = bodyTransformMatrix * result;
   8:     result = m_DeviceLocalTransform * result;
   9:     Vector3 scale;
  10:     result.Decompose(out scale, out m_Rotation, out m_Translation);
  11: }

 

Сначала из матрицы с эйлеровскими углами и вектора смещений формируется Body-Transform. Из конфигурационного файла tracking.xml мы получаем также матрицы Tracker-World-Transform, Device-World-Transform и Device-Local-Transform.

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

В завершение, для получения корректного результирующего преобразования, матрицы необходимо перемножить в правильном порядке:

Device-Local-Transform * Body-Transform * Tracker-World-Transform * Device-World-Transform

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

Ограничения и заключительные замечания

Я продемонстрировал возможность создания недорогой настольной системы виртуальной реальности с использованием двух устройств Wiimote и стереоскопических очков. Низкая разрешающая способность камер Wiimote не позволяет обеспечить качество, сравнимое с профессиональными монокулярными следящими системами. Между тем, качество представленной системы все же можно улучшить при точном измерении параметров, получаемых от камер Wiimote. Известно несколько алгоритмов измерения таких параметров, использующих совокупность замеров объекта с неизменяемой известной геометрией. Для стандартных камер обычно применяется модель шахматной конфигурации. Алгоритм для этой модели имеется в OpenCV, так что использовать его с Wiimote не составит проблем.

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

Ссылки
  • D. DeMenthon and L.S. Davis, «Model-Based Object Pose in 25 Lines of Code», International Journal of Computer Vision, 15, pp. 123-141, June 1995.
  • Заметки Дэниела Дементона (Daniel DeMenthon) об алгоритме определения положения на его сайте (EN).
  • OpenCV: библиотека алгоритмов машинного зрения с открытым кодом. Включает реализацию алгоритма определения положения и фильтра Калмана.
  • EmguCV: C#-оболочка OpenCV.
  • WiimoteLib: библиотека управляемого кода для Wiimote, написанная Брайаном Пиком (Brian Peek (EN)).
  • XNAnimation: интересная библиотека для анимации в XNA. Я не применял ее в самой программе слежения для Wiimote tracking, но задействовал ее анимационное демо-приложение в своем демонстрационном видео.
  • XNA: библиотека разработки игр на C#, которую я использую в качестве основы для своих приложений.
  • Знакомство с фильтром Калмана (EN). Грег Уэлш (Greg Welch) и Гэри Бишоп (Gary Bishop). Siggraph 2001 Course 8.
  • Predictive Tracking for Augmented Reality. Ronald Tadao Azuma (EN). Dissertation. University of North Carolina. February 1995.