Привет, Хабр.  По мере написания библиотеки в этой статье я хочу продолжить объяснять математику, лежащей в основе работы дополненной реальности. Результатом будет пример на игровом движке Unity, распознающий маркер и накладывающий на него трехмерную модельку. Библиотека пишется на C++ под Android, но фокус статьи будет направлен на математику. Эта статья, в отличии от предыдущей, будет ближе к практике, но если необходимо разобраться с основами векторной математики, то можно начать с нее.  В предыдущей статье мы рассматривали преобразования в евклидовом пространстве — , а сейчас ведем понятие проективного пространства —  (projective space).  Чтобы перевести координаты точки из двумерного евклидова пространства в проективное, нужно умножить вектор координат на любой ненулевой скаляр , а затем добавить  в качестве последней компоненты: . Получаем однородные координаты точки (homogeneous coordinates). Так как  — свободная переменная, то одной точке евклидова пространства принадлежит бесконечное множество точек проективного пространства. Перевод обратно выполняется делением на последнюю компоненту: . Удобно в качестве значения  брать 1: . Перевод обратно в таком случае также упрощается: .  Можно заметить, что умножение однородных координат на скаляр не меняет координат соответствующей точки в евклидовом пространстве: .  Также, если последняя компонента равна 0, то такую точку мы не сможем перевести в евклидово пространство:  – такая точка называется точкой на бесконечности (point at infinity или ideal point).  Трансформации, которые мы делали в евклидовом пространстве, можно применять и для однородных координат.  Для примера возьмем поворот точек матрицей поворота: .
В однородных координатах это принимает такую форму:   А смещение выполнялось следующим образом: . Мы не могли преобразовать смещение объекта в матричную операцию, так как она нелинейна в евклидовом пространстве. Но уже в проективном пространстве она становится линейной : .  Из предыдущей статьи мы помним, что матричные операции, можно объединять. Объединим поворот и смещение:
    Прежде чем двигаться дальше, вооружимся новым инструментом — методом наименьших квадратов (МНК).  Возьмем для примера такую систему линейных уравнений:

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


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

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

От OpenCV мы получаем координаты 4х углов маркера в кадре:   Для наложении изображения на маркер нам необходимо описать преобразование из локальных координат накладываемого изображения в координаты кадра с камеры. Для этого возьмем два вектора  и , описывающих базовые оси ,  и вектор смещения от начала координат — :

Можем упростить формулу, вычеркнув последнюю строку:


Одна пара точек задает два линейных уравнений, при этом имеем 6 неизвестных. Значит нужно 3 пары точек, чтобы система была определена. Мы имеем 4, а значит она переопределена. Воспользуемся МНК для нахождения нашего преобразования:  Пусть искомый вектор  будет собран из наших неизвестных следующим образом . Исходя из такого вектора , матрица , задающая нашу линейную систему уравнений будет иметь следующий вид:

А вектор  будет равен:

Тогда, используя МНК, можем получить решение нашей системы: . Отсюда получаем ,  и .  Пробуем!    Красным рисуются координаты маркера, полученного от OpenCV. В целом работает, но если взять низкий угол по отношению к маркеру, то видно, что искажения работают неправильно. А так получилось, потому что для моделирования пространственных искажений маркера мы использовали аффинную матрицу. Важное свойство аффинной матрицы — линии, полученные после преобразования, остаются параллельными. Однако обычно, смотря на параллельные линии на плоскости, мы видим такую картину:    Параллельные линии при проецировании сходятся в одной точке, т.е. становятся не параллельными. Значит аффинной матрицы нам недостаточно. Получить лучший результат нам поможет алгоритм, который называется Direct linear transformation.  А описать перспективные искажения поможет перспективная матрица размером 3x3: .  Перевод из евклидовых координат в однородные обозначим как  и распишем полученное преобразование для наших  точек (, а  в нашем случае):

Представим наши уравнения в виде системы уравнений:
      Представим матрицу  как вектор .
Теперь систему уравнений можно перевести в матричный вид: 

Получили однородную систему уравнений в которой нам неизвестен вектор  (т.е. матрица ). Однородную систему уравнений мы не сможем решить при помощи метода наименьших квадратов. Однако одно решение найти несложно — это нулевой вектор — . Но такое решение нас не интересует. Если же система имеет какое-то ненулевое решение, то мы получаем сразу бесконечное множество решений. Для примера пусть мы уже имеем ненулевое решение — вектор : . Теперь умножим вектор  на любой ненулевой скаляр , система уравнений все равно останется справедливой, а  — также будет еще одним решением системы: . Так как мы работаем с однородными координатами, то умножение на скаляр ничего не меняет. Тем не менее, чтобы не работать сразу со множеством решений, будем искать только одно — пусть длина вектора будет равна единицы .  Решить однородную систему уравнений можно при помощи сингулярного разложения матрицы (singular value decomposition). Сингулярное разложение — это разложение вида: , где  и  — ортонормальные матрицы, а  — диагональная, при этом диагональные элементы больше либо равны нулю и располагаются в порядке убывания (сверху вниз). Если воспринимать матрицу как операцию трансформации векторов, то это разложение будет декомпозицией этой трансформации на три последовательных: поворот, масштабирование по осям, второй поворот. Матрицы  и  — это ортонормальные матрицы, а значит можно назвать их матрицами поворота. Только следует учитывать, что  и , а значит, например, если  — имеет размер 3x3, то тройка базисных векторов и этих матриц поворота могут быть левосторонними, а не правосторонними как обычно. Не будем останавливаться на том, как вычислять это разложение. В более-менее полноценных математических фреймворках оно будет реализовано. Например — Eigen.  Воспользуемся этим разложением для полученной выше матрицы : . Лучшее решение для нашей системы уравнений  — это последняя строка матрицы : . А так как матрица  — ортонормальная, то длина вектора, составленного из любого его столбца, будет равна как раз единице.  В процессе вычислений у нас всегда есть погрешность, а это значит после  мы можем получить не нулевой вектор, хотя его компоненты должны быть близки к нулю. Нужно минимизировать получаемую погрешность, а для ее оценки воспользуемся обычной для таких вещей суммой квадратов: .

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

Обозначим , заметим, что  — не сохраняет длину вектора , значит :
.
Представим диагональ матрицы  как вектор . Тогда 
Теперь подумаем чему должен быть равен , чтобы  стало минимальным. Так как значения в . Судя по , наибольший вклад должен вносить последняя компонента вектора . Так как , то выходит .
.
Из описанного выше делаем вывод, что  — это последний столбец матрицы . Возвращаемся к нашей системе уравнений . Решаем ее описанным выше способом, получаем вектор . Затем из этого уже получаем искомую матрицу . Пробуем:    Как мы видим стало заметно лучше и искажения уже похожи на правду.
Соответствующий код для аффинных и перспективных искажений можно найти в тестах проекта — sonar/tests/test_marker_transform Хорошо, мы получили какую-то матрицу для преобразования изображений, но в идеале хотелось бы получить координаты камеры в пространстве. Чтобы их найти, построим математическую модель, которая будет описывать как точки мирового пространства проецируются на изображение камеры.  В качестве модели формирования изображения возьмем центральную проекцию. Суть центральной проекции состоит в том, что все точки, которые формируют выходное изображение, формируют лучи, сходящиеся в одной точке — центре проекции. Примерно так лучи себя и ведут в модели глаза или в цифровой камере.  Проецировать точки будем на плоскость, которая и будет формировать наше изображение. В качестве такой плоскости возьмем плоскость, заданную таким условием . В таком случае наша камера будет сонаправлена с осью :    Зададим параметрически формулу луча, формирующего точку на проекции: , где  — координаты точки луча,  — координаты проецируемой точки, а  — является свободным параметром. Если луч не лежит на нашей плоскости и не параллелен ей, то он будет пересекать ее только в одной точке. При этом мы знаем, что .


Получаем координаты спроецированной на плоскость точки: . Ничего не напоминает? Это формула перевода из однородных координат в двумерные евклидовы. Получается, что трехмерные координаты можно воспринимать как однородные координаты, соответствующие двумерным евклидовым координатам на плоскости. А перевод из однородных координат в двумерные евклидовы координаты выполняет центральную проекцию. Можно сказать, что трехмерные евклидово пространство эквивалентно двумерному проекционному. После проецирования точки на плоскость можно переводить ее в координаты изображения. Обычно координаты изображения определены следующим образом, где  и  — высота и ширина изображения:    Пусть у нас для камеры задан угол обзора по горизонтали — .
Масштабируем координату  точки  так, чтобы расстояние от крайней левой видимой точки камеры до крайней правой было равно ширине изображения. При проецировании имеем такую картину:    Здесь  — левая и правые крайние точки,  — точка между ними, у которой координата , а . Угол . При этом имеем треугольник , у которого угол , а угол , так как  делит угол  пополам. Длина  равна 1. Отсюда через тангенс угла  найдем длину  — . А уже отсюда .
После проецирования сводим диапазон координаты  —  к диапазону  умножением на коэффициент:

Чтобы сохранить пропорции изображения, то координату  мы должны масштабировать на ту же величину. Но на выходном изображении ось  направлена в обратную сторону (вниз). Чтобы отразить ось, умножим коэффициент масштаба по оси  на . Получаем такое преобразование:


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

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

Матрицу  — называют внутренними параметрами камеры (intrinsic camera parameters). Эта матрица может в себе иметь еще параметр :
.
Эффект от его применения чем схож эффектом от rolling shutter. На практике обычно этот параметр не учитывается и остается равно нулю.  Следует учитывать, что формулы вычисления параметров  справедливы, если мы хотим перевести именно в пиксельные координаты изображения. Но других случаях нам может быть нужно перевести в другую координатную систему, например, в текстурные координаты, где значения координат  и  меняются в диапазоне . Тогда эти параметрами будут уже другими.  Погрешности на производстве при сборки камер немного сказываются и на получаемых с них изображениях, оптический центр смещается немного от центра изображения, а каждая камера имеет слегка отличающиеся внутренние параметры. Поэтому каждая камера калибруется индивидуально. Реализацию калибровки можно найти в OpenCV.  В API Android OS и iOS можно найти методы получения внутренних параметров камеры, заданные производителем.  Сейчас же предлагаю просто считать оптическим центром — центр изображения, а параметры  подбирать "на глаз". Для начала хватит и такого варианта.  Все относительно — и хотя наблюдатель перемещается в пространстве, а мир стоит на месте, мы также можем сказать, что движется мир, а наблюдатель остается на месте. Меняется только система координат, в которой происходит движение. Значит выберем ту, которая для нас удобнее. А удобнее нам оставить локальную систему координат на месте, и все остальное пусть движется вокруг нее. Движение у нас определяется поворотом и смещением — матрицей поворота  и вектором смещения . Также считаем, что у нас есть внутренние параметры камеры, заданные матрицей . Таким образом мы можем разбить процесс вычисления координат на изображении из мировых координат на два этапа:  Как мы уже знаем, матрицу  и вектор  можно объединить в одну матрицу, только для того, чтобы ее применить придется перевести  в однородные координаты:

Мы знаем, что  и нам необязательно ее вычислять, поэтому можем чуть сократить формулу:

Более короткая версия этой же формулы:

Теперь мы можем объединить два этапа в одну формулу:

К этому моменту можно нас поздравить — мы построили математическую модель проецирования изображения камеры. Опираясь на нее далее мы сможем понять, что происходит в видеопотоке. А точнее нашей основной задачей станет поиск параметров  и . Эти параметры называют внешними параметрами камеры (extrinsic camera parameters). Так как мы предположили, что камера остается на месте, а двигается окружающий мир, то  — это не матрица поворота самой камеры, а вектор  — не координаты позиции камеры. Матрица  — это поворот из мировых координат в локальные координаты камеры, а когда говорят о повороте объекта, то имеют ввиду поворот из локальных координат в мировые. Поэтому поворот камеры — это . Вектор  — вектор смещения камеры. Позицию камеры в мировых координатах можем найти следуя такой логике: при переводе в локальные координаты позиция камеры должна стать нулевым вектором — .  Возвращаемся к предыдущему примеру. В нем мы нашли перспективную матрицу , описывающую преобразования наших точек:

Попробуем разобраться как эта формула соотноситься с нашей моделью:

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

Пусть , тогда получаем:

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





Итак, мы получили координаты камеры в пространстве. Теперь можем передать эти координаты трехмерному движку, чтобы он вывел трехмерные модели поверх реального изображения. Что и было реализовано в плагине для Unity. Видео, записанное с android-сматфона:  Точки маркера, на которых мы основываем нашу мировую систему координат в наших вычислениях и которая одновременно с этим является нашим полом, находятся в плоскости . У Unity же пол является плоскостью . Чтобы повернуть пол в нужные нам координаты, я воспользовался встроенными в Unity средствами — создал пустой объект, назначил его "родителем" камеры, и повернул так, как мне нужно. Таким образом, мне самому не пришлось делать дополнительных вычислений. Также важно сопоставить характеристики виртуальной камеры и реальной. Пример этого можно увидеть в коде плагина. Нужно еще отметить, что описать преобразование точек при помощи перспективной матрицы размером 3x3 можно только в случае, когда идет отображение точек с одной плоскости на другую. Такая матрица называется матрицей гомографии (homography matrix). В нашем случае шло отображение с плоскости маркера на плоскость проекции. Но при работе трехмерными координатами алгоритм direct linear transformation задействовать уже не получится и нужно будет искать другие способы. Код, реализующий все примеры выше можно найти в проекте на гитхаб в ветке article2. Точность пока оставляет желать лучшего. Более того, маркеры являются не самом удобным способ взаимодействия с дополненной реальностью, поэтому далее будем пытаться отойти от их использования и улучшать точность работы.