4.2 KiB
2.2 Базовое решение задачи
Обозначим последовательность изображений {u_i\ |\ i\in[0..N]}, где N – итоговое количество кадров, которое было получено во время всего полета. Пусть I_i\left(x,y\right) – интенсивность пикселя в позиции (y,\ x), где x\ \in[0..W-1] и y\ \in[0..W-1], W – ширина кадра. Матрица H_i – матрица гомографии, которая описывает переход от кадра u_{i-1} к кадру u_i: I_{i-1}\left(x,y\right)=I_i\left(x^\prime,y^\prime\right), t\ast\left(\begin{matrix}x\prime\y\prime\1\\end{matrix}\right)=H_i\times\left(\begin{matrix}x\y\1\\end{matrix}\right) Матрица гомографии (H) описывает проективное между двумя плоскостями и может быть представлена в следующем виде: H=K_1\times R\times T\times{K_2}^{-1}, где K_1 и K_2 – матрицы внутренних параметров камеры, R – матрица поворотов, T – матрица трансляции. Матрицы K_1 и K_2 представляют собой матрицы внутренних параметров и обычно равны между собой (различаются центры изображений). Однако в случаях разного разрешения картинок они могут иметь разные параметры. В общем виде такая матрица представляется следующим образом:
K_i=\left(\begin{matrix}f&\gamma&c_x^i\0&f&c_y^i\0&0&1\\end{matrix}\right),
Где f – фокусное расстояние в пикселях, c_x и c_y – координаты оптического центра камеры на плоскости изображения, \gamma – коэффициент скоса, описывающий угол наклона пикселей (в рамках симуляции равен 0). Матрицу R можно получить при помощи стандартной функции OpenCV decomposeHomographyMat, также эту матрицу можно вычислить самостоятельно, поскольку первые два столбца матрицы гомографии H инвариантны смещению. Матрица R представляет собой следующий вид: R=R_x\times R_y\times R_z Где R_x,R_y,R_z – матрицы вращения вокруг осей OX,\ OY,\ OZ соответственно. Если известны угол рыскания \psi, тангажа \theta и крена \gamma, то эти матрицы вычисляются следующим образом:
R_x\left(\gamma\right)=\left(\begin{matrix}1&0&0\0&cos\left(\gamma\right)&-sin\left(\gamma\right)\0&sin\left(\gamma\right)&cos\left(\gamma\right)\\end{matrix}\right),
R_y\left(\theta\right)=\left(\begin{matrix}cos\left(\theta\right)&0&sin\left(\theta\right)\0&1&0\-sin\left(\theta\right)&0&cos\left(\theta\right)\\end{matrix}\right),
R_z\left(\psi\right)=\left(\begin{matrix}cos\left(\psi\right)&-sin\left(\psi\right)&0\sin\left(\psi\right)&cos\left(\psi\right)&0\0&0&1\\end{matrix}\right),
Зная матрицу H,\ K_1, K_2,\ и R, можно получить матрицу T:
T=R^{-1}\times K_1^{-1}\times H\times K_2 л.в.что:T=R-1×K1-1×K1×R×T×K2-1×K2
Матрица трансляции имеет вид:
T=\lambda0λxf0λλyf00λz=λ *10xf01yf00z
Таким образом, можно получить смещение между кадрами, зная матрицы внутренних параметров камеры и матрицу гомографии. Чтобы получить положение БПЛА в момент времени t, достаточно получить матрицу трансляции из произведения всех промежуточных матриц гомографии (обозначим за G_t) на префиксе кадров [0..t]: G_t=\prod_{i=1}^{t}H_i=H_1\times H_2\times\ldots\times H_t