Как найти положение камеры по трем точкам в пространстве?

Question

[Роман Володин]

Уже некоторое время в моей голове сидит идея скрипта для Блендера, который бы находил положение камеры в прострастве по известным точкам.

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

Подскажите, в каком направлении копать?
Скрипт будет писаться на третьем питоне, может есть какие-то полезные пакеты?

Comments

[386DX]

На хабре 2 недели назад что-то такое было

Answer 1

[Mrrl]

Непростая задача.
Сначала по фотографии измеряете углы между лучами, а по модели - расстояния между точками. Потом обозначаете через x,y,z неизвестные расстояния от камеры до точек (длину каждого луча), и записываете уравнения из теоремы косинусов:
x^2-2*a*x*y+y^2=P^2
x^2-2*b*x*z+z^2=Q^2
y^2-2*c*y*z+z^2=R^2
Здесь a,b,c - косинусы углов между лучами, а P,Q,R - расстояния.
Дальше надо решить эту систему. Теоретически, она сводится к уравнению 8-й степени от одной переменной z. MAPLE смог найти этот многочлен, он занимает чуть больше экрана. Не знаю, хороший ли это вариант - может быть, и да. Можно попытаться решить систему численно - перебрать разные стартовые значения x,y,z и искать корень методом Ньютона. Но учтите, что система плохая - у неё вполне могут найтись 4 близких корня с положительными x,y,z. А могут и не найтись - тогда будут локальные минимумы. Можно перебрать расстояние x с мелким шагом. Для каждого x найти два варианта y, два варианта z, подставить их в третье уравнение, и из самой лучшей тройки начать искать решение - можно методом Ньютона, но можно и делением отрезка пополам.
После того, как x,y,z найдены, находите положение центра камеры - как точку пересечения трёх сфер. И дальше надо найти правильную ориентацию. Это довольно противно, но должно быть не очень сложно (по сравнению со всеми предыдущими шагами).

Comments

[eugenebartosh]

Спасибо за содержательный ответ по теме. Скажите, как вы получили в MAPLE полином размером на экран? При использовании solve({P1^2 = X1^2-2*cos(a1)*X1*X2+X2^2, P2^2 = X1^2-2*cos(a2)*X1*X3+X3^2, P3^2 = X2^2-2*cos(a3)*X2*X3+X3^2}, {X1, X2, X3}) получается невообразимой длины полином. Как-то по другому? Спасибо!

[Mrrl]

eugenebartosh: Я решал так:

with(Groebner);
f:=[x^2-2*a*x*y+y^2-P^2,x^2-2*b*x*z+z^2-Q^2,y^2-2*c*y*z+z^2-R^2];
B:=gbasis(f,plex(x,y,z));
res:=collect(B[1],z);

В многочлене только чётные степени z, коэффициент при z^8 равен (a^2+b^2+c^2-2*a*b*c-1)^2. Интересно, что это такое.

[eugenebartosh]

Mrrl: спасибо за ответ и за ликбез по Mapple! Дорожка оказывается уже проторена - computeroptics.smr.ru/KO/PDF/KO39-3/390317.pdf например, у них получается полином 4й степени от одной переменной с известной структурой коэффициентов, что сводит задачу к 4-м уравнениям (нужны координаты 4-х точек) и в конечном счете решению линейной системы.

[eugenebartosh]

Mrrl: Получил многочлен, у меня он достаточно компактный, менее половины экрана (наверно шрифт просто поменьше:), если только четные степени - для преобразовании к линейной системе нужно также 4 уравнения и следовательно координаты 4х точек

[eugenebartosh]

eugenebartosh: В общем есть решение, получилось запрограммировать и отладить сравнительно легко - благо Maple сгенерил более 50% C-кода. Однако понадобилось 5 точек. 4, тем более 3 - мало. Хотя теоретически чисто достаточно 3-х.

[eugenebartosh]

Mrrl: Сказать что все гладко пока нельзя, работает не очень стабильно. Может погрешность вычислений накапливается, может сказывается дуализм решений полинома четной степени... из пяти точек обычно 3 определяются примерно нормально, 2 плавают - более близкая из двух точек может оказаться более далекой, или вообще точка вылетает за пределы сцены...

Решение выполняется заменой переменных полинома a1*z^8 + a2*z^6 + a3*z^4 + a4*z^2 + a5 = 0 на z1 = z^8, z2 = z^6, z3 = z^4, z4 = z^2 и последующему вычислению обратной матрицы линейной системы, составленной из данных 5-ти точек.
Для решения выполняются условия z3 ~= z4 * z4, z2 ~= z4*z4*z4, z1 != z4*z4*z4*z4 с погрешностью 5-20%. Редко бывает z1 < 0, еще реже - z2 < 0.
В большинстве случаев z4 содержит довольно реалистичное значение, система ведет себя стабильно и реалистично при удалении-приближении, поворотах камеры и т.п. - расстояния больше-меньше, либо пропорции меняются при поворотах.

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

[Mrrl]

eugenebartosh: Сначала уточним. Z у вас - расстояние от некоторой точки (одной конкретной из тех трёх, для которых строилась система) до камеры. Верно ли, что когда вы строите разные полиномы, то во всех случаях это одна и та же точка, т.е. вы выбираете какую-то точку A и берёте 5 из 6 возможных троек (A,X,Y), где X,Y - это какие-то две точки из оставшихся четырёх?
"Дуализм решения" вряд ли проявляется. Но я бы эту задачу продолжил решать так:
- у нас уже есть 6 полиномов 4-й степени от u=z^2. Найдём сумму S(u) их квадратов - это будет какой-то полином степени 8. Скорее всего, корней у него не будет, но есть хороший шанс, что правильному значению z будет соответствовать его глобальный минимум в области u > 0. Можно, на всякий случай, посчитать такие же полиномы для остальных точек, и выбрать ту, у которой минимум меньше. Минимум искать сначала перебором u с мелким шагом, а потом уточнить методом Ньютона для уравнения S'(u)=0.
И обязательно попробуйте нарисовать графики всех 6 полиномов для тестового примера! Возможно, станет понятнее, что там происходит в действительности.

[eugenebartosh]

Mrrl: Расстоянию Z было уделено внимание в том ключе, как вы упомянули - иначе наверно не удалось бы получить и эти скромные пока но надеюсь обещающие результаты.

Множественность не дает мне покоя тем не менее - она сидит в уравнениях теоремы косинусов, геометрически можно вот такую картинку сразу представить:

Нетрудно заметить, что точки А3 и A3' удовлетворяют любым условиям теоремы косинусов, не просто в отдельной паре точек, но и в более сложной их совокупности (в данном случае 3 точки).

[Mrrl]

eugenebartosh: А, под "дуализмом" вы имеете в виду большое число положительных корней. Да, это проблема, особенно, если корни близкие. Остаётся надеяться на то, что у многочленов, полученных из разных троек, все корни, кроме одного, будут достаточно сильно различаться. И тогда поиск глобального минимума суммы квадратов сработает. А будет ли достаточно устойчиво решение через систему уравнений - не уверен: там не используется важная информация о том, что компоненты решения - степени одного и того же z^2. По хорошему, там надо брать ещё больше троек, и искать вектор методом наименьших квадратов (поиск оптимального решения переопределённой несовместной системы).

[eugenebartosh]

Mrrl: Мне представляется - корни скорей всего будут близкими, что следует и из геометрической иллюстрации задачи - расположена ли точка n на dZ ближе или дальше чем ее соседи по совокупности?

Брать большое множество точек не хочется - все-таки вычислительная сложность алгоритма заметная, вычисление всех коэффициентов одного уравнения - это около тысячи операций сложения и умножения, система из 4-х ур для одной точки вместе с решением обратной матрицы - уже более 5 тыс операций, а должно работать в реальном времени на iPad / iPhone. Да и реперные точки еще надо где-то взять, особенно в первых итерациях.

С другой стороны, в модели не используется важнейшее правило - то что центр камеры, точка в пространстве и проекция точки, находятся на одной прямой.
Проекция точки может быть выражена в условных реальных координатах через пиксельные в соответствии с матрицами поворота камеры:
C + (cos(Rz)*cos(Ry)*Ux - sin(Rx)*sin(Ry)*cos(Rz)-Uy*cos(Rx)*sin(Rz),
sin(Rz)*cos(Ry)*Ux-sin(Rx)*sin(Ry)*sin(Rz)+Uy*cos(Rx)*cos(Rz),
-Ux*sin(Ry)-cos(Ry)*sin(Rx) )
где Rx,Ry,Rz - искомые значения углов поворота камеры (от положения по оси Z),
С - искомый центр камеры,
U вычисляется из пиксельных координат точки как (tg(uv.x-W/2)/W*fov_x,tg(uv.y-H/2)/H*fov_y))

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

[eugenebartosh]

Mrrl: в итоге метод не дал приемлемого результата в начальной итерации, так как все реперные точки начальной итерации расположены ко-планарно, то есть на 2D-метке, напечатанной на листе бумаги и положенной на пол. От этих точек и по положению камеры уже должны генерироваться другие точки методом триангуляции, но это уже другая история...
Ко-планарное расположение точек провоцирует описанные degenerate cases. Даже в развитых пакетах типа OpenCV все degenerate cases данного метода не реализованы.
Короче, я взял готовый пакет OpenCV, тут реализованы все 5 алгоритмов, в том числе рассмотренный нами P3P.
Интересующихся отправляю к OpenCV, а также первоисточниками:
EPNP - F.Moreno-Noguer, V.Lepetit and P.Fua "EPnP: Efficient Perspective-n-Point Camera Pose Estimation"
P3P - X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem"
DLS - Joel A. Hesch and Stergios I. Roumeliotis. "A Direct Least-Squares (DLS) Method for PnP"
UPNP - A.Penate-Sanchez, J.Andrade-Cetto, F.Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation"

[eugenebartosh]

Вот забрел случайно сюда опять ))
Если исходить из того, что ориентация камеры соответствует данным гироскопа мобильного устройства на момент фиксации кадра, то задача становится даже скучной и не требует никакой особенной библиотеки:
- вектор направления для каждой точки к центру камеры вычисляется по ориентации камеры и пиксельным координатам точки;
- по двум точками вычисляем точку пересечения прямых, заданных точкой начала и вектором направления - это и есть координаты центра камеры;
Гироскоп дает достаточно точную ориентацию, по крайне мере на устройствах Apple и большинстве Android-смартфонов и таблеток, так что погрешность получается более чем приемлемая.
И достаточно только двух точек, вычисления простейшие линейные.
Ну и поскольку в реальности прямые не пересекутся из-за погрешностей - надо искать точку их минимального сближения, задача на 2 оператора сложнее

Answer 2

[Urrus]

Это задача Perspective-n-Point (PnP) - а она не такая простая, как кажется, и решается множеством разных способов (типа ePnP: cvlabwww.epfl.ch/~lepetit/papers/lepetit_ijcv08.pdf, с использованием RANSAC, и т.д). В общем, математики там полно, поэтому лучше найти уже готовую библиотеку, которая умеет делать такие вещи. Сам я пользовался только методами из OpenCV (типа solvePnP(...), solvePnPRansac(...)), она есть для питона, но кажется будет довольно жирновато тащить ее только ради плагина.

Answer 3

[Андрей Ларин]

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

Comments

[Роман Володин]

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

[Андрей Ларин]

Роман Володин: ух ты, а фотки тречить не приходило в голову. Спасибо.