Алгоритмическое решение пятиточечной задачи оценки положения камер, основанное на представлении Кэли матриц вращения Текст научной статьи по специальности «Математика»

АЛГОРИТМИЧЕСКОЕ РЕШЕНИЕ ПЯТИТОЧЕЧНОЙ ЗАДАЧИ ОЦЕНКИ ПОЛОЖЕНИЯ КАМЕР,

ОСНОВАННОЕ НА ПРЕДСТАВЛЕНИИ КЭЛИ МАТРИЦ ВРАЩЕНИЯ

Е.В. Мартюшев'

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

Ключевые слова: 5-точечная задача оценки положения камер, калиброванная камера, эпиполярные ограничения, представление Кэли.

1. Введение

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

Задача 1. Пусть даны две калиброванные камеры-обскуры с центрами 0Х, 02 и пять точек сцены Q1,..., Q5, лежащие перед камерами в 3-мерном евклидовом пространстве, рис. 1. В системе координат каждой камеры известны только однородные координаты точек Qi. Задача состоит в определении относительного положения и ориентации второй камеры относительно первой.

Пятиточечная задача является ключевой для более общей задачи трехмерной реконструкции сцены, которая в свою очередь используется во многих приложениях компьютерного зрения, таких как дополненная реальность, системы автопарковки, навигация и планировка маршрута для роботов и т.д. Как известно, 5-точечные алгоритмы показывают значительно лучшие результаты с точки зрения точности и надежности, чем 6-, 7- и 8-точечные алгоритмы оценки положения камер. Более того, для плоских и почти плоских сцен только 5точечные методы позволяют надежно получать решение без каких-либо дополнительных модификаций алгоритма.

Как впервые показал Круппа [1] в 1913 году, задача 1 может иметь самое большее 11 решений. Используя методы проективной геометрии, он предложил алгоритм решения 5-точечной задачи, который, однако, не был численно устойчивым и потому не мог быть реализован программно. Позднее Демазур [2], Фогерас и Мэйбанк [3], Хейден и Спарр [4] уточнили результат Круппы и доказали, что точное количество решений (включая комплексные) равно 10.

Впервые численно устойчивое решение задачи 1 было предложено Филипом [5] в 1996. Его метод требовал нахождения корней полинома 13-й степени. В 2004 Нистер [6] улучшил алгоритм Филипа и выразил решение через корни полинома 10-й степени. После этого было предложено множество модификаций алгоритма Нистера, одни из которых упрощали его программную реализацию [7], а другие - повышали его численную устойчивость [8, 9].

Рис. 1. К формулировке 5-точечной задачи

1 Мартюшев Евгений Владимирович - кандидат физико-математических наук, кафедра математического анализа, Южно-Уральский государственный университет.

E-mail: [email protected]

В настоящей статье мы предлагаем еще одно алгоритмическое численно устойчивое решение задачи 1, использующее известное представление Кэли для матриц вращения [10]. В нашем подходе не смешиваются ротационные и трансляционные параметры существенной матрицы и, тем не менее, решение выражается через корни полинома 10-й степени. Эксперименты на синтетических данных показывают, что наш метод сравним по точности и надежности с существующими 5-точечными решениями.

1.1. Обозначения. Мы используем обозначение а,Ь,... для вектор-столбцов и А,В,... для

матриц. Элементы матрицы А обозначаются Ау, Ат - транспонированная к А матрица, det А

- определитель матрицы А . Для двух векторов а и Ь мы обозначаем а X Ь и атЬ их векторное и скалярное произведения соответственно. Для вектора а мы обозначаем [а]х кососимметрическую матрицу такую, что [а]х Ь = а X Ь для всех Ь . Символ I обозначает единичную матрицу, 0

- нулевую матрицу или вектор, • - норму Фробениуса.

2. Описание алгоритма

2.1 Преобразование начальных данных. Начальными данными для нашего алгоритма являются однородные координаты , у^, г^ точек Qi в системе координат у -й камеры, у = 1, 2,

1 = 1, „.,5 (рис. 1).

Без потери общности МОЖНО ПОЛОЖИТЬ Ху1 = Уу1 = Ху2 = 0 ДЛЯ у = 1, 2 . Численно устойчивый

способ добиться этого состоит в следующем. Мы объединяем начальные данные в две матрицы размера 3 х 5 :

А у =

Ху1

Уу1

2Л

Ху 5 У у 5 ^ 5

и вычисляем матрицы

А = н у 2 А = Н у 2 Н ЛА у

(1)

(2)

где Ну1 и Ну2 - матрицы Хаусхолдера, обращающие в нуль ху1, уу1 и Ху2 соответственно. Соответствующие векторы Хаусхолдера имеют вид:

Ху1 " Ху2

Ь у1 = Уу1 , Ь у 2 = У у' 2 + 81§п( У} 2 }\) Х% + У у'2

гу1 + ^П( гу1 )^1 Ху1 + Уу1 + 4 0

В дальнейшем мы увидим, что преобразование (2), будучи очень простым, существенно упрощает дальнейшие вычисления. В частности, это преобразование позволяет легко преобразовать полином 20-й степени (12) в полином 10-й степени (13).

2.2. Эпиполярные ограничения и существенная матрица. Напомним сначала некоторые определения из многопроекционной геометрии [11-13]. Камера-обскура - это тройка (О, п, Р), где п - плоскость изображения, Р - центральная проекция точек 3-мерного евклидова пространства на п, и О - центр камеры (центр проекции Р). Фокусное расстояние - это расстояние между

О и п, ортогональная проекция О на п называется принципиальной точкой. Камера-обскура называется калиброванной, если все ее внутренние параметры (такие как фокусное расстояние и координаты принципиальной точки) известны.

Пусть даны две калиброванные камеры-обскуры (Оу, пу, Ру-) для у = 1, 2 . Без потери общности можно положить Р1 = [1 0], Р2 = [ 1] , где Я е 80(3) - матрица вращения и

Ь = [ ^2 ^3 ]Т - вектор сдвига, подчиненный условию нормализации ||Ц = 1.

Поскольку лучи O1Qi и O2Qi (рис. 1) должны пересекаться в М3, то это накладывает некоторые ограничения на Я и 1 . Эти ограничения называются эпиполярными и имеют вид [12]:

Уіі

= 0.

(3)

где і = 1,..5, Е = [і]х Я называется существенной матрицей.

2.3. Десять полиномов четвертой степени. Наш алгоритм основан на следующем хорошо известном результате.

Теорема 1 ([10]). Если матрица Я є 80(3) не является вращением вокруг некоторой оси на угол к + 2пк, k є Ъ, то Я может быть представлена в виде:

Я =

Ґ - - Л Ґ - - л

и и

I - V I + V

w

V - X/ V - X

-1

(4)

где u, v, w є

Предложение 1. Если

u =

v =

w =

и, V, W, 1) = [ ]х

-1 ■ - Уґ3 + Wt2

5

-2 - wt1 + иґ3

5

. -3 - иґ2 + Уґ1

(5)

5

где 8 = и^ + vt2 + ^3, то Е(и', v/, w' , Ь) = -Е(и, v, w, Ь).

Доказательство. Рассмотрим матрицу Я' = -ИЬК_еБО(3), где матрица Хаусхолдера НЬ = I - 2ЬЬт . Тогда Е' = [Ь] Я' = -Е . Записывая матрицу Я' в форме (4) с параметрами u' , v' , w',

получаем следующую систему уравнении относительно переменных

.V , w

ґ „, 1 л ( - - ґ - п „, 1 л

и и и и

I - V1 I + V = -Н( I - V I + V1

w' w w'

V - -1 XJ V - X У - -1 - -1 X )

которая, как можно убедиться прямым вычислением, имеет единственное решение (5). □

Поскольку эпиполярные ограничения (3) линейны и однородны по Ь , то их можно переписать так:

8Ь = 0, (6)

где і -я строка 5 X 3 матрицы 8 есть [ у1і 21і ]1

'■и

У2і

2і

Представим матрицу вращения Я в форме (4) и найдем определители всех 3 X 3 подматриц матрицы 8 . В результате получим систему из десяти полиномиальных уравнений:

= [0]и 4 + [0]и\ + [0]и 2у 2 + [0]^ + [0]у4 + [1]и3 + [1]и 2у

+ [!]ну2 + [1]у3 + [2]и 2 + [2]му + [2]у2 + [3]и + [3]у + [4] = 0,

(7)

где i = 1, ..., 10 , [п] обозначает полином степени п по переменной w , [0] - константа. Таким образом, мы получили десять полиномов , каждый из которых имеет полную степень 4.

Замечание 1. На самом деле определители 3 X 3 подматриц матрицы S имеют вид ¥і / А , где А = 1 + и2 + V2 + м2 и ¥і - полином 6-й полной степени. Однако можно убедиться в том, что ¥і факторизуется как ^ = /і А , и коэффициенты полинома / легко выражаются через коэффициенты полинома ^.

2.4. Многочлен десятой степени от одной переменной. Перепишем систему (7) в виде:

Bm = 0, (8)

где B - матрица коэффициентов размера 10X 35 и m =

4 3 3

и и V и м

V м 1

вектор

мономов.

Расширим систему (8) еще двадцатью полиномами и/і, vfi для і = 1, ...,5, и м/і для

і = 1,..., 10 . Таким образом, мы получим:

Б'

пі

m

= 0,

(9)

где Б7 - новая матрица коэффициентов размера 30X 50 и

т' =

4 3 3 1

им и ^ им

2 3 V м

м"

- вектор мономов пятой степени. Ясно, что система (9) эквивалентна системе (8).

Переставим столбцы в матрице Б7 и выполним преобразование Гаусса-Жордана с выбором ведущего элемента столбца (для лучшей численной устойчивости). Тогда последние шесть строк

323 3323 3 им ими V м V м V ЫУ и V 1

& 1 [3] [4] [4] [5]

8 2 1 [3] [4] [4] [5]

83 1 [3] [4] [4] [5]

8 4 1 [3] [4] [4] [5]

85 1 [3] [4] [4] [5]

8 6 1 [3] [4] [4] [5]

Мы опустили здесь первые 28 нулевых столбцов. Из шести соответствующих полиномов g1,..., g6 можно получить следующие четыре полинома:

К 81 8 2

82 83 и

= - м = С(м)

К 84 85 V

К4 85 8б 1

= 0

(10)

С( м) =

(11)

где матрица С(^) имеет вид:

[4] [5] [5] [6]

[4] [5] [5] [6]

[4] [5] [5] [6]

[4] [5] [5] [6]

Замечание 2. Поскольку мы используем только последние шесть строк матрицы Б7, то нет необходимости выполнять «полное» преобразование Гаусса-Жордана матрицы Б7. Для первых 24 строк матрицы Б7 только поддиагональные элементы должны быть обнулены. Это существенно уменьшает объем вычислений и повышает эффективность алгоритма.

Из (10) следует, что для существования решения системы (8) необходимо, чтобы detС(У) = 0 . Обозначим Ж = detС(У). В общем случае это полином 20-й степени по переменной м.

Предложение 2. Полином 7^ имеет специальную симметричную форму:

* = £ Рк [м10+к + (—м)10—к ] , (12)

к=0

где рк е К .

Доказательство. Из условий х;1 = у г = 0 следует, что Е33 = 0 . Как следствие,

Л23 ум + и

t2 = и—23 = и-------.

Я13 им — у

Подставляя это в последнее равенство из (5), получаем м' = — м_1. Таким образом, числа м и (—м_1) одновременно являются (или не являются) корнями полинома IV . Поэтому,

10 10 г

* = Р10П(м — мг )(м + м—1) = £ Рк м10+к + (—м)10—к 1=1 к=0

Введем новую переменную w = w — w 1 и преобразуем IV к полиному 10-й степени:

10

W = X pkw, (13)

k=0

k k Л

где рк могут быть получены С ПОМОЩЬЮ формулы Мк = ^ (— 1)г мъ к . Результат имеет вид:

i=0

V i У

10 f : pk = ^ k

i=k k

(i+k — i^ 2 i — k

Pi, (i4)

2

где штрихованная сумма берется по всем i от к до 10, для которых i — к mod 2 = 0 . Заметим, что

10 f

в случае к = 0 правая часть равенства (14) принимает вид ^ 2 Pi .

i=0

2.5. Восстановление структуры. Комплексный корень полинома 1У дает комплексный корень полинома и, согласно (4), - комплексную матрицу вращения, не имеющую геометриче-

ской интерпретации. Поэтому имеет смысл находить только вещественные корни полинома УУ. Для этого можно использовать, например, метод последовательностей Штурма [14] для изоляции корней и метод Риддерса [15] для их последующего уточнения. После этого матрица второй камеры может быть восстановлена следующим образом.

Пусть w0 - вещественный корень полинома W . Тогда м0 = w0/2 + sign(w0)^(w0 / 2)2 +1 есть корень полинома W, подчиненный условию | м0 |> 1. Далее, и - и v -компоненты решения находятся с помощью метода исключения Гаусса с выбором ведущего элемента столбца на матрице C(w0) в (11).

После этого, по формуле (4) находятся элементы матрицы R. Зная R, вектор сдвига t может быть найден с помощью метода исключения Гаусса с выбором ведущего элемента столбца на матрице S(u0, v0, w0) в (6). Здесь необходимо учесть условие нормализации ||t|| = 1.

Пусть Ht = I — 2ttT и R7 = —HtR . Известно [12, 6], что существуют четыре возможности для матрицы второй камеры: PA = [R t], PB =[R —t], Pc =[R' t]и PD =[R' —t] . Все эти матрицы удовлетворяют эпиполярным ограничениям (3). Однако только одна из них верна, все остальные соответствуют нефизическим конфигурациям.

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

R

13

R

c2 = C1R33 + ^3 •

(15)

23

Тогда.

если c1 > 0 и c2 > 0 , то P2 = PA ; иначе если c1 < 0 и c2 < 0, то P2 = PB ; иначе если cl > 0 и c2 > 0 , то P2 = Pc ;

P2ni = (H22H2l)T P2

• иначе Р2 = Рд .

Здесь величины с( и с2 вычисляются в точности также как с1 и с2 в (15) с заменой Я на Я7. Наконец, исходная матрица второй камеры получается по формуле

Н12Н11 0“

0 1

где матрицы Хаусхолдера Н;1 и Н^2 определены в подразделе 2.1.

3. Эксперименты на синтетических данных

В этом разделе мы сравниваем наш алгоритм с 5-точечным алгоритмом Нистера [6]. Для этого оба алгоритма были программно реализованы на С/С++. Все вычисления выполнялись с двойной точностью. Параметры синтетических данных были такими же как в [6], см. таблицу.

Расстояние до сцены 1

Глубина сцены 0,5

Длина базы 0,1

Размеры изображения 352 X 288

Угол обзора камеры 45°

Средняя скорость из 10 тестов для нового алгоритма составила примерно 35 микросекунд/вызов, для алгоритма Нистера - 24 микросекунды/вызов на процессоре Intel Core i5 2,3 Ghz.

В качестве численной ошибки измерялась величина £ = |P — P^|, где P2 - истинное значение матрицы второй камеры.

Распределения численной ошибки приведены на рис. 2. Полное число тестов в каждом эксперименте было 10б . Мы сравнили алгоритмы, во-первых, для случая условий по умолчанию -сцены глубины 0,5 и движения камеры общего вида, и, во-вторых, для случая, наиболее проблематичного с точки зрения численной устойчивости, - плоской сцены и движения камеры по направлению к сцене. Из приведенных графиков видно, что оба алгоритма имеют высокую численную устойчивость, хотя необходимо отметить, что алгоритм Нистера существенно превосходит новый алгоритм для первого случая.

Рис. 2. Распределение численной ошибки. Слева: условия по умолчанию, ошибка медианы есть 1,56х 10-13 для алгоритма Нистера и 2,94х 10 10 для нового алгоритма. Справа: плоская сцена и движение по направлению к сцене, ошибка медианы есть 1,52х10 2 для алгоритма Нистера и 7,17х 10 3 для нового алгоритма

Мартюшев Е.В.

Рис. 3. Трансляционная (слева) и ротационная (справа) ошибки относительно шума,

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

4. Обсуждение

Представлен новый алгоритм решения 5-точечной задачи оценки положения камер. Тестирование на синтетических данных подтверждает его численную устойчивость и надежность. В целом, представленный алгоритм является хорошей альтернативой к существующим 5-точечным решениям. Его преимущество состоит в том, что он позволяет восстанавливать относительные положения и ориентации камер напрямую без вычисления существенной матрицы. Такой подход является более гибким, когда имеется некоторая дополнительная информация о векторе сдвига и/или о матрице вращения. Например, если известно, что углы Эйлера (ф, в, /), представляющие матрицу R, лежат в некоторых известных пределах, то пределы изменения величины w = —2ctg(^ + /) также известны. Это позволяет сразу отбрасывать некоторые корни полинома

10-й степени W без вычисления матриц камеры.

Литература

1. Kruppa, E. Zur Ermittlung eines Objektes aus zwei Perspektiven mit Innerer Orientierung / E. Kruppa // Sitz.-Ber. Akad. Wiss., Wien, Math. Naturw. Kl. Abt. Ila. - 1913. - Vol. 122. - P. 1939— 1948.

2. Demazure, M. Sur Deux Problemes de Reconstruction / M. Demazure // INRIA. - 1988. -№ RR-0882. - P. 1-29.

3. Faugeras, O. Motion from Point Matches: Multiplicity of Solutions / O. Faugeras, S. Maybank // International Journal of Computer Vision. - 1990. - Vol. 4. - P. 225-246.

4. Heyden, A. Reconstruction from Calibrated Camera - A New Proof of the Kruppa-Demazure Theorem / A. Heyden, G. Sparr // Journal of Mathematical Imaging and Vision. - 1999. - Vol. 10. -P. 1-20.

5. Philip, J. A Non-Iterative Algorithm for Determining all Essential Matrices Corresponding to Five Point Pairs / J. Philip // Photogrammetric Record. - 1996. - Vol. 15. - P. 589-599.

6. Nister, D. An Efficient Solution to the Five-Point Relative Pose Problem / D. Nister // IEEE Transactions on Pattern Analysis and Machine Intelligence. - 2004. - Vol. 26. - P. 756-777.

7. Li, H. Five-Point Motion Estimation Made Easy / H. Li, R. Hartley // IEEE 18th Int. Conf. of Pattern Recognition. - 2006. - P. 630-633.

8. Kukelova, Z. Polynomial Eigenvalue Solutions to the 5-pt and 6-pt Relative Pose Problems / Z. Kukelova, M. Bujnak, T. Pajdla // Proceedings of the British Machine Conference. - 2008. - P. 56.156.10.

9. Stewenius, H. Recent Developments on Direct Relative Orientation / H. Stewenius, C. Engels,

D. Nister // ISPRS Journal of Photogrammetry and Remote Sensing. - 2006. - Vol. 60. - P. 284-294.

10. Cayley, A. Sur Quelques Proprietes des Determinants Gauches / A. Cayley // J. Reine Angew. Math. - 1846. - Vol. 32. - P. 119-123.

11. Faugeras, O. Three-Dimensional Computer Vision: A Geometric Viewpoint / O. Faugeras // MIT Press, 1993. - 663 p.

12. Hartley, R. Multiple View Geometry in Computer Vision. / R. Hartley, A. Zisserman. -Cambridge University Press, 2004. - 655 p.

13. Maybank, S. Theory of Reconstruction from Image Motion / S. Maybank // Springer-Verlag, 1993. - 261 p.

14. Hook, D.G. Using Sturm Sequences to Bracket Real Roots of Polynomial Equations /

D.G. Hook, P.R. McAree // Graphic Gems I : сб. науч. тр. - Academic Press, 1990. - P. 416-422.

15. Numerical Recipes in C. Second Edition / W. Press, S. Teukolsky, W. Vetterling, B. Flannery. -Cambridge University Press, 1993. - 1020 p.

ALGORITHMIC SOLUTION OF THE FIVE-POINT POSE PROBLEM BASED ON THE CAYLEY REPRESENTATION OF ROTATION MATRICES

E.V. Martyushev

A new algorithmic solution to the five-point relative pose problem is introduced. Our approach is not connected with or based on the famous cubic constraint on an essential matrix. Instead, we use the Cayley representation of rotation matrices in order to obtain a polynomial system of equations from epi-polar constraints. Solving that system, we directly obtain positional relationships and orientations of the cameras through the roots for a 10th degree polynomial.

Keywords: five-point pose problem, calibrated camera, epipolar constraints, Cayley representation.

References

1. Kruppa E. Zur Ermittlung eines Objektes aus zwei Perspektiven mit Innerer Orientierung. Sitz.-Ber. Akad. Wiss., Wien, Math. Naturw. Kl. Abt. IIa. 1913. Vol. 122. pp. 1939-1948.

2. Demazure M. Sur Deux Problemes de Reconstruction. INRIA. 1988. no. RR-0882. pp. 1-29.

3. Faugeras O., Maybank S. Motion from Point Matches: Multiplicity of Solutions. International Journal of Computer Vision. 1990. Vol. 4. pp. 225-246.

4. Heyden A., Sparr G. Reconstruction from Calibrated Camera - A New Proof of the Kruppa-Demazure Theorem. Journal of Mathematical Imaging and Vision. 1999. Vol. 10. pp. 1-20.

5. Philip J. A Non-Iterative Algorithm for Determining all Essential Matrices Corresponding to Five Point Pairs. Photogrammetric Record. 1996. Vol. 15. pp. 589-599.

6. Nister D. An Efficient Solution to the Five-Point Relative Pose Problem. IEEE Transactions on Pattern Analysis and Machine Intelligence. 2004. Vol. 26. pp. 756-777.

7. Li H., Hartley R. Five-Point Motion Estimation Made Easy. IEEE 18th Int. Conf of Pattern Recognition. 2006. pp. 630-633.

8. Kukelova Z., Bujnak M., Pajdla T. Polynomial Eigenvalue Solutions to the 5-pt and 6-pt Relative Pose Problems. Proceedings of the British Machine Conference. 2008. pp. 56.1-56.10.

9. Stewenius H., Engels C., Nister D. Recent Developments on Direct Relative Orientation. ISPRS Journal of Photogrammetry and Remote Sensing. 2006. Vol. 60. pp. 284-294.

10. Cayley A. Sur Quelques Proprietes des Determinants Gauches. J. Reine Angew. Math. 1846. Vol. 32. pp. 119-123.

11. Faugeras O. Three-Dimensional Computer Vision: A Geometric Viewpoint. MIT Press, 1993. 663 p.

12. Hartley R., Zisserman A. Multiple View Geometry in Computer Vision (Second Edition). Cambridge University Press, 2004. 655 p.

13. Maybank S. Theory of Reconstruction from Image Motion. Springer-Verlag, 1993. 261 p.

14. Hook D.G., McAree P.R. Using Sturm Sequences To Bracket Real Roots of Polynomial Equations. Graphic Gems I (A. Glassner ed.). Academic Press, 1990. pp. 416-422.

15. Press W., Teukolsky S., Vetterling W., Flannery B. Numerical Recipes in C (Second Edition). Cambridge University Press, 1993. 1020 p.

Поступила в редакцию 18 февраля 2013 г.

1 Martyushev Evgeniy Vladimirovich is Cand.Sc. (Physics and Mathematics), Mathematical analysis department, South Ural State University. E-mail: [email protected]