In [18]:
from math import sin, cos, tan, pi, radians
import numpy as np
In [19]:
# Ground point, should produce image point 300 samples and 450 lines
X = 1124722.490145
Y = -1602802.718797
Z = 1455389.588660
# Hardcoded values from json
# some set to 0 for simplicity
o = 2.256130940792258
p = 0.09433201631102328
k = -0.9630375478615623
f = 549.1178195372703
pixel_pitch = 0.014
principal_point_x = 0
principal_point_y = 0
XL = 1728357.7031238307
YL = -2088409.0061042644
ZL = 2082873.9280557402
In [20]:
def opk_to_rotation(o, p, k):
"""
Convert from Omega, Phi, Kappa to a 3x3 rotation matrix
Parameters
----------
o : float
Omega in radians
p : float
Phi in radians
k : float
Kappa in radians
Returns
-------
: ndarray
(3,3) rotation array
"""
om = np.empty((3,3))
om[:,0] = [1,0,0]
om[:,1] = [0, cos(o), -sin(o)]
om[:,2] = [0, sin(o), cos(o)]
pm = np.empty((3,3))
pm[:,0] = [cos(p), 0, sin(p)]
pm[:,1] = [0,1,0]
pm[:,2] = [-sin(p), 0, cos(p)]
km = np.empty((3,3))
km[:,0] = [cos(k), -sin(k), 0]
km[:,1] = [sin(k), cos(k), 0]
km[:,2] = [0,0,1]
return km.dot(pm).dot(om)
# ground to image
def collinearity(f, M, camera_position, ground_position, principal_point = (principal_point_x, principal_point_y)):
XL, YL, ZL = camera_position
X, Y, Z = ground_position
x0, y0 = principal_point
x = (-f * ((M[0,0] * (X - XL) + M[0,1] * (Y - YL) + M[0,2] * (Z - ZL))/
(M[2,0] * (X - XL) + M[2,1] * (Y - YL) + M[2,2] * (Z - ZL)))) + x0
y = (-f * ((M[1,0] * (X - XL) + M[1,1] * (Y - YL) + M[1,2] * (Z - ZL))/
(M[2,0] * (X - XL) + M[2,1] * (Y - YL) + M[2,2] * (Z - ZL)))) + y0
return x, y, -f
# image to ground
def collinearity_inv(f, M, camera_position, pixel_position, elevation, principal_point=(0,0)):
XL, YL, ZL = camera_position
x, y = pixel_position
#x = x / pixel_pitch
#y = y / pixel_pitch
Z = elevation
x0, y0 = principal_point
X = (Z-ZL) * ((M[0,0] * (x - x0) + M[1,0] * (y - y0) + M[2,0] * (-f))/
(M[0,2] * (x - x0) + M[1,2] * (y - y0) + M[2,2] * (-f))) + XL
Y = (Z-ZL) * ((M[0,1] * (x - x0) + M[1,1] * (y - y0) + M[2,1] * (-f))/
(M[0,2] * (x - x0) + M[1,2] * (y - y0) + M[2,2] * (-f))) + YL
return X,Y
In [21]:
# First from pixel to ground:
# We know that the pixel size is 0.014^2 mm per pixel (14.4mm / 1024 pixels)
pixel_size = 0.014
x0 = 512 * pixel_size # Convert from pixel based principal point to metric principal point
y0 = 512 * pixel_size
majorAxis = 2439.7 * 1000
sample_pix = 300 * pixel_size
line_pix = 450 * pixel_size
M = opk_to_rotation(o, p, k)
# This is image to ground
X, Y = collinearity_inv(f, M, [XL, YL, ZL], [sample_pix, line_pix], 1455389.588660, (x0, y0))
print('Ground: ', X, Y, 1455389.588660) # Arbitrary 1000m elevation - here is where iteration with intersection is needed.
# Now reverse! This is ground to image
# These are in mm and need to convert to pixels
x, y, f = collinearity(f, M, [XL, YL, ZL], [X, Y, 1455389.588660], [x0,y0])
x = x / pixel_size
y = y / pixel_size
print('Image: ', x, y)
In [ ]: