Hello,
Api functions: "ypr2mat" and "mat2opk" works great, so i decide to ask you about formulas, whose you are using to calculate rotation matrixes. Could you provide some equations? I'm trying to develop a function in Python to calculate omega,phi,kappa directly after my UAV landing into EPSG: 2178 projection. I based on this paper: "New Calibration and Computing Method for Direct Georeferencing of Image and Scanner Data Using.pdf", but my results are not correct comparing to yours in Agisoft
According to this paper, could you explain me the origin P0 of the tangent plane system? Should it be calculated for each image separately or single set of geographic coordinates for whole photogrammetric block?
I've got longitude,latitude and angles (yaw, pitch, roll) extracted from XMP info for each image (captured by dji phantom).
import numpy as np
fi = np.deg2rad(52.113742)
lam = np.deg2rad(21.397086)
roll = np.deg2rad(-0.875)
pitch = np.deg2rad(-0.825)
yaw = np.deg2rad(65.318)
def Cnb(yaw, pitch, roll):
Rz1 = np.array([[np.cos(yaw), -np.sin(yaw), 0],
[np.sin(yaw), np.cos(yaw), 0],
[0, 0, 1]])
Ry1 = np.array([[np.cos(pitch), 0, np.sin(pitch)],
[0, 1, 0],
[-np.sin(pitch), 0, np.cos(pitch)]])
Rx1 = np.array([[1, 0, 0],
[0, np.cos(roll), -np.sin(roll)],
[0, np.sin(roll), np.cos(roll)]])
Cnb = np.matmul(np.matmul(Rz1,Ry1), Rx1)
return Cnb
def Cne(fi, lam):
Ry = np.array([[np.cos(fi+(np.pi/2)), 0, np.sin(fi+(np.pi/2))],
[0,1,0],
[-np.sin(fi+(np.pi/2)), 0, np.cos(fi+(np.pi/2))]])
Rz = np.array([[np.cos(lam), -np.sin(lam), 0],
[-np.sin(lam), np.cos(lam), 0],
[0, 0, 1]])
Cne = np.matmul(Ry, Rz)
return Cne
def Cnn(fi, lam, fi0, lam0, lam0GK):
en = (lam-lam0)*np.cos(fi)*-1
ee = fi-fi0
ev = (lam-lam0GK)*np.sin(fi)
Cnn = np.array([[1, ev, -ee],
[-ev, 1, en],
[ee, -en, 1]])
return Cnn
TBb = np.array([[-1,0,0],
[0,1,0],
[0,0,-1]])
TEn = np.array([[0,1,0],
[1,0,0],
[0,0,-1]])
def CBE(yaw, pitch, roll, fi, lam, fi0, lam0, lam0GK):
Cnn0 = Cnn(fi, lam, fi0, lam0, lam0GK)
Cn0e = Cne(fi0, lam0)
Cnie = Cne(fi, lam)
Cnib = Cnb(yaw, pitch, roll)
C = np.matmul(np.matmul(np.matmul(Cnn0, Cn0e), Cnie.transpose()), Cnib)
return np.matmul(np.matmul(TBb, C.transpose()), TEn.transpose())
#c = CBE(yaw, pitch, roll, fi, lam, fi0=np.deg2rad(0), lam0=np.deg2rad(21), lam0GK=np.deg2rad(21))
b = CBE(yaw, pitch, roll, fi, lam, fi0=fi, lam0=lam, lam0GK=np.deg2rad(21))
c = b.transpose()
print (c)
f = np.rad2deg(np.arctan(c[0,2]/((c[1,2]**2+c[2,2]**2)**0.5)))
omega = np.rad2deg(np.arctan(-c[1,2]/c[2,2]))
kappa = np.rad2deg(np.arctan(-c[0,1]/c[0,0]))
print ('omega = ', omega)
print ('phi = ', f)
print ('kappa = ', kappa)
Best regards,
Adam