In [7]:
from math import sin, cos, tan, pi
import numpy as np
In [4]:
# Ground point, should produce image point 400 samples and 500 lines
X = 1116890
Y = -1604470
Z = 1459570
# Hardcoded values from json
# some set to 0 for simplicity
omega = 2.256130940792258
phi = 0.09433201631102328
kappa = -0.9630375478615623
focal_length = 549.1178195372703
pixel_pitch = 0
principal_point_x = 0
principal_point_y = 0
spacecraft_x = 1728357.7031238307
spacecraft_y = -2088409.0061042644
spacecraft_z = 2082873.9280557402
In [5]:
def rotation_from_opk(o, p, k):
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)
In [12]:
rm = np.empty((3, 3))
rm[:3, :3] = rotation_from_opk(omega, phi, kappa)
# Collinearity equations
samp = -focal_length * ((rm[0, 0] * (X - spacecraft_x) + rm[0, 1] * (Y - spacecraft_y) + rm[0, 2] * (Z - spacecraft_z))/(rm[2, 0] * (X - spacecraft_x) + rm[2, 1] * (Y - spacecraft_y) + rm[2, 2] * (Z - spacecraft_z))) + principal_point_x
line = -focal_length * ((rm[1, 0] * (X - spacecraft_x) + rm[1, 1] * (Y - spacecraft_y) + rm[1, 2] * (Z - spacecraft_z))/(rm[2, 0] * (X - spacecraft_x) + rm[2, 1] * (Y - spacecraft_y) + rm[2, 2] * (Z - spacecraft_z))) + principal_point_y
# Image point
print("Sample: ", samp)
print("Line: ", line)
In [ ]: