![](https://habrastorage.org/files/5c1/8ce/2c1/5c18ce2c110e45d69715197ac0ccce03.png)
Если посмотреть на последовательность кадров, в которых движется камера, то мозг легко воспринимает геометрическую структуру содержимого. Однако, в компьютерном зрении это не тривиальная проблема. В этой статье я постараюсь описать возможное решение этой задачи.
Перед прочтением этой статьи не помешает внимательно прочитать статью «Основы стереозрения».
Итак, перед нами стоит задача превращения последовательности двумерных изображений в трехмерную структуру. Задача это не простая, и нужно ее упрощать.
Во-первых, предположим, что кадра у нас только два. Обозначим их как A и B.
Во-вторых, будем работать с конечным множеством точек, соответствующих друг другу на кадрах A и B. Точки на изображении обозначим как
![](https://habrastorage.org/files/339/966/3b6/3399663b658044479efe99c956d38b05.png)
![](https://habrastorage.org/files/0d2/fb8/eee/0d2fb8eeeb7f43a98bff60032a1495e4.png)
![](https://habrastorage.org/files/da4/dde/afc/da4ddeafcf7041f5b3dc25af437c88d1.png)
![](https://habrastorage.org/files/da0/98c/7cf/da098c7cf1a54a0abc1ce6ac5ec60ab3.png)
Теперь необходимо определиться как искать
![](https://habrastorage.org/files/0d2/fb8/eee/0d2fb8eeeb7f43a98bff60032a1495e4.png)
![](https://habrastorage.org/files/da4/dde/afc/da4ddeafcf7041f5b3dc25af437c88d1.png)
![](https://habrastorage.org/files/0d2/fb8/eee/0d2fb8eeeb7f43a98bff60032a1495e4.png)
![](https://habrastorage.org/files/da4/dde/afc/da4ddeafcf7041f5b3dc25af437c88d1.png)
А теперь мы подошли к центральному вопросу этой статьи: как из точек
![](https://habrastorage.org/files/0d2/fb8/eee/0d2fb8eeeb7f43a98bff60032a1495e4.png)
![](https://habrastorage.org/files/da4/dde/afc/da4ddeafcf7041f5b3dc25af437c88d1.png)
Модель проективной камеры
Так как вы, вероятно, уже прочитали статью «Основы стереозрения», эта формула должна быть знакома:
![](https://habrastorage.org/files/1a1/6c2/bb4/1a16c2bb4da448dbbf3546bedca24617.png)
Или если описать более полно:
![](https://habrastorage.org/files/8e4/02d/303/8e402d3031264874bb84077a9f5aac93.png)
Здесь X — это трехмерная точка в пространстве.
x — это координата точки на изображении в однородных координатах, и
![](https://habrastorage.org/files/3ea/2c2/f60/3ea2c2f608084aa394acc5c29be01dbe.png)
![](https://habrastorage.org/files/20a/509/762/20a509762c8c4369a02c9f7df9db87d0.png)
Процесс перевода точки в пространстве в координаты изображения можно разбить на два этапа, реализуемыми двумя матрицами в формуле:
- [R|t] — R и t представляют собой положение камеры в пространстве. На этом этапе координаты точек переводятся в локальные координаты камеры. R — матрица поворота размером 3x3, t — трехмерный вектор смещения — вместе они составляют матрицу перехода [R|t] (размером 3x4), она определяет положение камеры в кадре. [R|t] — эта то же, что и видовая матрица в трехмерной графике (если не брать в расчет, что она имеет размер не 4x4).
— это матрица поворота камеры,
— трехмерные координаты точки расположения камеры в пространстве. R и t называют внешними параметрами камеры.
- K — матрица камеры. Локальные координаты точек переводятся в однородные координаты изображения. fx, fy — фокальное расстояние в пикселях, cx, cy — оптический центр камеры (обычно это координаты центра изображения). Эти параметры называют внутренними параметрами камеры.
Важным свойством этой модели является то, что точки лежащие на одной прямой в пространстве будут также лежать на одной прямой на изображении.
На самом деле, описываемая модель может быть очень неточной. В реальных камерах вступают в игру линзовые искажения, из-за которых прямые линии становятся кривыми. Это искажение называются дисторсией. Существуют разные модели, исправляющие эти искажения. Здесь есть некоторые их реализации. Параметры этой модели также входит в понятие внутренних параметров камеры.
С учетом дисторсии наша формула усложняется:
![](https://habrastorage.org/files/a4d/d84/731/a4dd847311ea4fac8395de1b4c6d7569.png)
Внутренние параметры камеры должны быть известны заранее. Выяснение этих параметров — это отдельная тема, будем считать, что они уже есть.
Искажения дисторсии не зависит от глубины видимых точек, а только координат на изображении. А значит «исправить» изображение (получив прямые линии там, где они и должны быть) можно не зная внешних параметров камеры и координат точек в пространстве. Дальше тогда можно использовать модель камеры без функции D.
![](https://habrastorage.org/files/4e1/49b/411/4e149b4115aa4023b20633c12e30c774.jpg)
Изображение с дисторсией слева, и справа — «исправленное» от линзовых искажений изображение. Видно, что линии стали прямыми.
Нормализация точек
Мы договорились, что внутренние параметры нам известны, известны и координаты точек на изображении, а значит, остается найти [R|t] и Xi (положения камеры и точек в пространстве).
Наша формула выходит уже немного сложной, надо ее упрощать. Для начала сделаем так:
![](https://habrastorage.org/files/f92/a6e/d4e/f92a6ed4e2cb4446aa6fe7cd452873bc.png)
Выражение остается справедливым. Продолжим:
![](https://habrastorage.org/files/523/c97/666/523c976666a849708f53734318d32aca.png)
Обозначим
![](https://habrastorage.org/files/eca/c92/f5f/ecac92f5f5f846d1b97c5d895fb7f415.png)
![](https://habrastorage.org/files/ba3/407/647/ba3407647102427db6b0b37ced951c81.png)
![](https://habrastorage.org/files/00f/f7b/825/00ff7b825e9346f0843d2d22d9134544.png)
nxi — это нормализованные точки изображения.
Фундаментальная и сущностная матрицы
Итак, предположим, у нас есть два изображения, полученные от одной камеры. Нам неизвестны положения камер и координаты точек в пространстве. Договоримся ввести расчеты относительно первого кадра. Так получается, что RA = I (I — единичная матрица), tA = (0, 0, 0). Положение камеры в кадре B обозначим просто как R и t (т. е. RB = R, tB = t). [R|t] — это матрица координат второго кадра, и оно же — матрица смещения положения камеры от кадра A к кадру B. В итоге имеем получаем такую систему (без учета дисторсии!):
![](https://habrastorage.org/files/551/c82/84b/551c8284b2504978961a3cda8dc4235c.png)
Используя фундаментальную матрицу F (fubdamental matrix), получим такое уравнение:
![](https://habrastorage.org/files/15b/bd2/895/15bbd28958fe476cb7afe0e144dc7e6a.png)
Также заметим, что F имеет размер 3х3 и должна иметь ранг равный 2.
Из фундаментальной матрицы F уже можно получить необходимые нам R и t. Однако дисторсия все портит, с ее учетом зависимость точек между кадрами будет нелинейная, и это уже не будет работать.
Но перейдем к нормализованным точкам и используем сущностную матрицу E (essential matrix). Все будет почти тем же, но проще:
![](https://habrastorage.org/files/7e9/8cb/9e6/7e98cb9e670f41439aaf87526b485f9b.png)
![](https://habrastorage.org/files/55e/1d8/758/55e1d8758df84f8da2df82d16dbd7f9f.png)
А тут мы можем спокойно брать в расчет дисторсию.
Фундаментальная и сущностная матрицы связаны таким образом:
![](https://habrastorage.org/files/9c7/8f4/25e/9c78f425e4a249cfbe037d7ac28e9361.png)
Теперь перед нами встала задача нахождения либо фундаментальной матрицы F, либо сущностной матрицы E, из которой позже сможем получить на R и t.
Вычисление сущностной матрицы (8-ми точечный алгоритм)
Вернемся к уравнению:
![](https://habrastorage.org/files/55e/1d8/758/55e1d8758df84f8da2df82d16dbd7f9f.png)
Эту же формулу можно переписать в таком виде (вспоминаем, что
![](https://habrastorage.org/files/300/ef0/ad2/300ef0ad22904f35a398839059fe9685.png)
![](https://habrastorage.org/files/5d1/c21/fb7/5d1c21fb7eec4524a1781a37d8e1a01b.png)
![](https://habrastorage.org/files/184/7f2/e57/1847f2e5728247f1a96dd00af6283cce.gif)
Введем вектор e и матрицу M:
![](https://habrastorage.org/files/cc0/30b/8a3/cc030b8a32d1457793b1c3abd75804e4.png)
![](https://habrastorage.org/files/503/02c/f81/50302cf81280475a8ec60f650f457c26.png)
Тогда всю систему уравнений можно представить в виде:
![](https://habrastorage.org/files/72e/91c/ebc/72e91cebc4574904934837c7c8ba666e.png)
Получаем однородную систему уравнений, решив которую, получим E из е. Очевидным решением здесь является нулевой вектор, но нас явно интересует не он. Для решения необходимо минимум 8 точек.
Решение систем однородных уравнений при помощи сингулярного разложения
Сингулярное разложение — это декомпозиция матрицы, приводящее ее к такому виду:
![](https://habrastorage.org/files/471/778/cae/471778cae5184089ba0c4cec732e450d.png)
Итак, было дано уравнение вида:
![](https://habrastorage.org/files/72e/91c/ebc/72e91cebc4574904934837c7c8ba666e.png)
![](https://habrastorage.org/files/471/778/cae/471778cae5184089ba0c4cec732e450d.png)
Строки VT, которым соответствует нулевой диагональный элемент W на этой же строке, являются нуль-пространствами матрицы M, т. е. в данном случае являются линейно-независимыми решениями нашей системы. А так как элементы W располагаются в порядке убывания, то смотреть нужно последний элемент матрицы W. И решением будет последняя строка
![](https://habrastorage.org/files/450/d96/00e/450d9600e5f74ccf8f351749112e9b77.png)
При расчете сущностной матрицы, используя 8 точек, последний элемент матрицы W должен быть равен нулю — W99=0, но на практике, в следствии ошибок, там будет какое-то ненулевое значение, и по величине этого значения можно оценить величину этой ошибки. При этом мы получим лучшее решение.
Тем не менее, найденное нами решение — не единственное, более того, решений будет бесконечно много. Если умножить найденное решение на какой-либо коэффициент, оно все-равно останется решением. Таким образом в уравнении спрятался коэффициент s (который может быть любым):
![](https://habrastorage.org/files/1b0/49f/510/1b049f510d3944a7a5a4a1700ddb9319.png)
Правда, все эти решения будут линейно зависимыми, а интересовать нас будет только одно из них.
Отсюда и матрица E может также масштабироваться. Вот только расчеты ведутся в однородном пространстве и, как следствие, от масштабирования (т. е. от коэффициента s) не зависят.
Наверное, стоит масштабировать получившуюся матрицу E так, чтобы E33 = 1.
Вычисление сущностной матрицы (7-ми точечный алгоритм)
Можно обойтись и 7-ю точками.
Если мы возьмем только 7 точек, то M будет матрицей размером 7x9.
Вернемся к выражению:
![](https://habrastorage.org/files/471/778/cae/471778cae5184089ba0c4cec732e450d.png)
W — будет также матрицей размером 9x9, как и раньше, но теперь не только W99 будет равно нулю (ну опять же без учета ошибок вычислений), но и W88. Это значит, что имеем два линейно-независимых решения уравнения
![](https://habrastorage.org/files/72e/91c/ebc/72e91cebc4574904934837c7c8ba666e.png)
![](https://habrastorage.org/files/7d7/0eb/7b1/7d70eb7b17a44b61a67a2e4379c420f3.png)
Сущностная матрица, как и фундаментальная, должна иметь ранг равный двум, а так как она имеем размер 3x3, то значит определитель матрицы равен 0 —
![](https://habrastorage.org/files/787/703/51f/78770351fd2240bcacb040a5d3e0d0d3.png)
![](https://habrastorage.org/files/507/972/de9/507972de925145a6b855b1b2278a94af.png)
![](https://habrastorage.org/files/b16/f78/148/b16f78148b5c4e648c7060e4f9d27b6a.png)
Расписывать решение этого уравнения я не буду (оно объемное, ну и считайте это домашним заданием). В крайнем случае можете просто посмотреть сразу реализацию в opencv.
Уточнение сущностной (фундаментальной) матрицы
Так как все в этом мире несовершенно, то мы будем постоянно получать ошибки, с которыми нам необходимо бороться. Так сущностная матрица должна иметь ранг равный 2 и следовательно
![](https://habrastorage.org/files/787/703/51f/78770351fd2240bcacb040a5d3e0d0d3.png)
Чтобы увидеть в чем это выражается, возьмем фундаментальную матрицу. Сущностная матрица / фундаментальная матрица — разница лишь в том, с какими точками мы работаем (нормализованными или точками на изображении).
Луч, выпущенный из точки кадра A, ляжет в кадр B как прямая линия (или не совсем из-за дисторсии, но забудем про нее). Допустим матрица F — это фундаментальная матрица кадров A и B (
![](https://habrastorage.org/files/15b/bd2/895/15bbd28958fe476cb7afe0e144dc7e6a.png)
Тогда если выпустить луч из точки
![](https://habrastorage.org/files/9cf/8a2/6ae/9cf8a26ae2274b3eb83884b0ce9ab685.png)
![](https://habrastorage.org/files/c35/3e7/b29/c353e7b290d641378228ce1c549d5dd2.png)
![](https://habrastorage.org/files/d83/832/54f/d8383254fa204aa181e00154f5b811ee.png)
![](https://habrastorage.org/files/404/51e/d6c/40451ed6c73147f48c24e998aef1e1f5.png)
![](https://habrastorage.org/files/536/ea8/b02/536ea8b023064804a7108d0127b0f323.png)
![](https://habrastorage.org/files/9ed/3fa/8ba/9ed3fa8ba5ef4e91bfa0ff1cf91b8015.png)
![](https://habrastorage.org/files/15b/bd2/895/15bbd28958fe476cb7afe0e144dc7e6a.png)
![](https://habrastorage.org/files/dc1/2a4/54b/dc12a454b44544099b391f813bf4a520.jpg)
На картинке изображен пример эпиполярных линий, полученных из правильной фундаментальной матрицы (ранг которой равен 2, картинка справа) и неправильной (слева).
Чтобы получить правильную фундаментальную матрицу, воспользуемся свойством сингулярного разложения — приближать матрицу к заданному рангу:
![](https://habrastorage.org/files/e1c/bb3/680/e1cbb368005e40378d03fc76973b718b.png)
Тогда исправленный вариант:
![](https://habrastorage.org/files/db0/6ae/c20/db06aec201a547cf9bceed6ad41d5a67.png)
Ровно тот же принцип работает и для сущностной матрицы.
Нормализованная версия алгоритма
Чтобы уменьшить ошибку, получаемую при расчетах, точки трансформируют к определенному ввиду. Выбираются такие матрицы TA и TB, которые (каждый независимо и на своем кадре) смещают среднюю координату точек в точку (0, 0) и масштабируют так, чтобы средняя дистанция дистанция до центра была равна
![](https://habrastorage.org/files/a10/80c/9c7/a1080c9c7560449a8fe36977db4b5d69.gif)
![](https://habrastorage.org/files/1ad/c0f/f35/1adc0ff3561f478e99b011aeb3aef62f.png)
А матрицы TA, B имеют вид:
![](https://habrastorage.org/files/165/1bd/0ff/1651bd0ff60944c78d4024276172941f.png)
После этого вычисляем сущностную матрицу как обычно. После необходимо ее уточнить, как было описано выше. Обозначим полученную матрицу как Et.
Итоговая сущностная матрица —
![](https://habrastorage.org/files/90f/6fd/601/90f6fd6018244fdc832e69cd115be783.png)
В итоге:
![](https://habrastorage.org/files/48a/9e9/cbe/48a9e9cbe3f841c9ae9464205fc3058a.png)
Опять же, если необходимо найти фундаментальную матрицу, все принципы сохраняются.
Получение положения камеры из сущностной матрицы
Введем матрицу H:
![](https://habrastorage.org/files/50f/6ff/1ba/50f6ff1ba3b946b994295b5db4be97b2.png)
Используем сингулярное разложение на сущностной матрице:
![](https://habrastorage.org/files/108/53a/ab3/10853aab3ae84d9fb69ee2716539bfc0.png)
Тогда получаем такие решения:
![](https://habrastorage.org/files/76b/577/170/76b57717001e40a0bd99765189c13967.png)
![](https://habrastorage.org/files/99b/f8e/08c/99bf8e08ca6a45ec948379198be62ef6.png)
![](https://habrastorage.org/files/747/a8c/73b/747a8c73b7db4e648f6f11d64ab92e6e.png)
![](https://habrastorage.org/files/b83/931/9eb/b839319eba544d1dac2542769205dc23.png)
![](https://habrastorage.org/files/ca2/946/4eb/ca29464ebad44019a69cf193b94bd93f.png)
![](https://habrastorage.org/files/99a/c67/425/99ac674259c94e38a47f64d770b3e4bd.png)
Нам же необходимо положение камеры в локальных координатах самой камеры:
![](https://habrastorage.org/files/26a/2df/e22/26a2dfe22dd2467c9f26a40ec85dba64.png)
Выходит четыре решения:
![](https://habrastorage.org/files/9e9/bbd/8b4/9e9bbd8b436b41ab8d14f83ce971bce5.png)
В случае 8-ми точечного алгоритма, выбираем из 4-ёх решений. В случае 7-ми точечного алгоритма, выйдет три сущностные матрицы, из которых получится 12 решений. Выбрать нужно только одно, то, которое будет давать меньше ошибок.
Вырожденные случаи
Снова вернемся к вычислению сущностной матрицы. У нас было уравнение:
![](https://habrastorage.org/files/72e/91c/ebc/72e91cebc4574904934837c7c8ba666e.png)
Далее мы его решали при помощи сингулярного разложения:
![](https://habrastorage.org/files/471/778/cae/471778cae5184089ba0c4cec732e450d.png)
Решения данного уравнения зависит от ранга матрицы W, ну или от количества нулей в диагонали этой матрицы (мы же помним, что это отражает ранг матрицы). Вот только с учетом погрешности, считаем нулем в данном случае число, достаточно близкое к нулю.
Имеем такие варианты:
- Нулей нет. Нет решений, вероятно ошибка вышла слишком большой.
- Один нуль. Одно решение, случай при котором число точек больше, либо равно восьми.
- Два нуля. Одно или три решения. Использовалось семь или более точек.
- Три нуля. Тогда должно быть верно условие
. Такое возможно, если камера не смещалась от кадра к кадру, был только поворот, т. е. t = (0, 0, 0). Либо все точки лежат на одной плоскости. Во втором случае еще есть возможность найти координаты этих точек и положение камеры, но уже другими способами.
Вычисление координат точек в пространстве
Допустим сейчас у нас есть больше, чем два кадра — A, B, C, …
![](https://habrastorage.org/files/2c6/3c0/4ed/2c63c04ed5fb4cdeb2f9271515a0c7c3.png)
![](https://habrastorage.org/files/f7f/757/479/f7f7574795b34d60b7b6f16aa76f55b6.png)
Необходимо найти точку
![](https://habrastorage.org/files/81d/5c0/549/81d5c05490534e44a9908d69f4f04d7a.png)
![](https://habrastorage.org/files/69f/c16/76e/69fc1676ec834224a87ef258fc4f4d38.png)
Представим эту систему так:
![](https://habrastorage.org/files/b9f/e05/3e9/b9fe053e9ef84c0783e1861ee8c03364.png)
![](https://habrastorage.org/files/d16/89d/4e7/d1689d4e7895473f9e12120c6419267c.png)
В матричном виде:
![](https://habrastorage.org/files/abb/741/1ea/abb7411ea73544038571aba0956da887.png)
![](https://habrastorage.org/files/e37/a0c/9a7/e37a0c9a7b4b475aadeb612278e0c86d.png)
С помощью сингулярного разложения находим вектор
![](https://habrastorage.org/files/afa/7dd/99c/afa7dd99cb944c1cb3e69afb84f20d8b.png)
![](https://habrastorage.org/files/ac6/2b4/82f/ac62b482f95844f3bd896d0c8c7b2836.png)
![](https://habrastorage.org/files/4fe/dfc/652/4fedfc652e454c0c8564ed3ef8d955d3.png)
![](https://habrastorage.org/files/0c1/d1b/088/0c1d1b088f75459e9807da4c09e57a0e.png)
Оценочная функция
Оценочные функции (cost functions) необходимы, чтобы получив какие-то результаты, оценить насколько достоверными они являются, или сравнить их с другими.
Возьмем нашу модель:
![](https://habrastorage.org/files/ce6/d10/0b5/ce6d100b5e634d49a2d57ffa45566037.png)
![](https://habrastorage.org/files/9cf/8cb/ce2/9cf8cbce2b0d4c3a9e149f61b4323eb9.png)
Отсюда квадрат ошибки для i-ой точки будет:
![](https://habrastorage.org/files/fb6/c42/691/fb6c426919164e4fab9966a243351caa.png)
На практике одни точки будут давать более достоверные результаты, чем другие. А некоторые вообще явно будут давать неправильные. В результате возникает необходимость выбрать из общего массива точек только те точки, которым можно доверять, а остальные просто выбросить из расчетов.
Самый простой способ выбрать “достоверные” точки — выбрать какой-то лимит (допустим, 5 пикселей), и брать только те точки, которые дают ошибку меньше этого лимита (
![](https://habrastorage.org/files/97a/b97/680/97ab97680c2d41418716e1f24a84bb85.png)
Таким образом, можно ввести оценочную функцию — количество достоверных точек. И при сравнении, выбирать тот результат, который дает большее количество “достоверных” точек.
А можно воспользоваться другим, более “тонкой” функцией:
![](https://habrastorage.org/files/9a5/f7c/876/9a5f7c8764e641958b3d8bb2c6863180.png)
Лучшим будет тот вариант, который будет давать меньшее значение. Понятно, что и здесь следует убирать “недостоверные” точки для будущих расчетов.
Метод RANSAC
- При вычислении сущностной матрицы необходимо отбрасывать “недостоверные” точки, так как они приводят в существенным ошибкам в расчетах. Определить набор подходящих точек можно при помощи алгоритма RANSAC.
Повторяем цикл заданное количество раз (например, 100, 400):
- Выбираем случайным образом минимальный набор точек для расчетов (у нас это 7);
- Вычисляем сущностные матрицы из этого набора (напоминаю, может получится либо одна матрица, либо три)
- Оценочной функцией вычисляем достоверность каждой матрицы
- Из предыдущего цикла выбираем сущностную матрицу, которая дает лучший результат;
- Выбираем точки для расчетов, которые дают ошибку при полученной сущностной матрице ошибку меньше заданного порога;
- Из полученного набора точек вычисляем итоговую сущностную матрицу.
Общий алгоритм
- Находим «особенные» точки на первом кадре
- Определяем точки-соответствия между двумя изображениями.
- Находим сущностную (или все-же фундаментальную) матрицу, соответствующую этим двум изображениям при помощи RANSAC.
- У нас будет одно или три решения, из которых получим 4 или 12 возможных матриц [R|t]. Имея положение камер в обоих кадрах, рассчитываем координаты точек в пространстве для каждой возможной матрицы. Из них выбираем лучшую, используя оценочную функцию.
Что дальше?
Изначально мы исходили из предположения, что кадра у нас было всего два.
Чтобы работать с последовательностью кадров, нужно просто разбить последовательность на последовательные пары кадров. Обрабатывая пары кадров, мы получаем смещение камеры от одного кадра к другому. Из этого можно получить координаты положения камеры в остальных кадрах.
Получив главное — положения камер, можно действовать по-разному:
- По точкам-соответствиям получить трехмерные координаты точек в пространстве, выйдет облако точек, которое можно превратить в трехмерную модель
- Использовать фундаментальную матрицу для расчета карты глубины.
- При помощи двух кадров инициализировать карту для SLAM, используя рассчитанные координаты точек в пространстве, можно проще и быстрее получить координаты положения в следующих кадрах.
- ну и другое
В общем, действовать можно по-разному, применяя разные методы, в том числе и те алгоритмы, которые были описаны — не единственные.
Литература
Fundamental matrix, Essential matrix, Eight-point algorithm — больше информации в википедии
Hartley, Zisserman — Multiple View Geometry in Computer Vision — спонсор этой статьи