Commit 37a23275 authored by luroth's avatar luroth
Browse files

.

parent 4792beb4
......@@ -4,6 +4,38 @@ import numpy as np
import geojson
from osgeo import ogr
from stl import mesh
from joblib import Parallel, delayed
import multiprocessing
from itertools import repeat
import csv
from urllib import request
import os
import warnings
def frange(start, stop, step):
i = start
while i < stop:
yield i
i += step
def cartesian(arrays, out=None):
with warnings.catch_warnings():
warnings.simplefilter("ignore")
arrays = [np.asarray(x) for x in arrays]
dtype = arrays[0].dtype
n = np.prod([x.size for x in arrays])
if out is None:
out = np.zeros([n, len(arrays)], dtype=dtype)
m = n / arrays[0].size
out[:,0] = np.repeat(arrays[0], m)
if arrays[1:]:
cartesian(arrays[1:], out=out[0:m,1:])
for j in range(1, arrays[0].size):
out[j*m:(j+1)*m,1:] = out[0:m,1:]
return out
def rotation_matrix(axis, theta):
"""
......@@ -20,14 +52,9 @@ def rotation_matrix(axis, theta):
[2*(bc-ad), aa+cc-bb-dd, 2*(cd+ab)],
[2*(bd+ac), 2*(cd-ab), aa+dd-bb-cc]])
## SONY-RX100II-VIS Specification
sensor_dimension_x = 13.2 # mm
sensor_dimension_y = 8.8 # mm
focal_lengh = 10.4 # mm
def getWKT_PRJ (epsg_code):
import urllib
wkt = urllib.request.urlopen('http://spatialreference.org/ref/epsg/{0}/prettywkt/'.format(epsg_code))
wkt = request.urlopen('http://spatialreference.org/ref/epsg/{0}/prettywkt/'.format(epsg_code))
remove_spaces = wkt.read().decode("utf-8")
remove_spaces = remove_spaces.replace(' ', '')
output = remove_spaces.replace("\n", "")
......@@ -51,110 +78,212 @@ def point_in_poly(x,y,poly):
return inside
# Test DATA
#PhotoID = 'DSC01518.JPG'
#X = 2693775.2436063010000000
#Y = 1256295.7234344794000000
#Z = 607.0859311561650900
#Omega = 4.2298653063595717
#Phi = -1.6345979613024908
#Kappa = 141.9575462861694500
PhotoID = 'DSC01520.JPG'
X = 2693786.2141234782000000
Y = 1256310.0950113330000000
Z = 606.8090010659472000
Omega = 2.7787812806211063
Phi = -1.3090272913737455
Kappa = 141.0244127303900300
# Coordinates of focal poin
focal_point = np.array([X, Y, Z])
# Relative coordinates of corners of sensor
sensor_Z = focal_lengh / 1000
sensor_X_C1u2 = - (sensor_dimension_x / 2000)
sensor_X_C3u4 = (sensor_dimension_x / 2000)
sensor_Y_C1u4 = (sensor_dimension_y / 2000)
sensor_Y_C2u3 = - (sensor_dimension_y / 2000)
sensor_corners = np.array([(sensor_X_C1u2, sensor_Y_C1u4, sensor_Z),
(sensor_X_C1u2, sensor_Y_C2u3, sensor_Z),
(sensor_X_C3u4, sensor_Y_C2u3, sensor_Z),
(sensor_X_C3u4, sensor_Y_C1u4, sensor_Z)])
def intersect_point_with_planes(plane_normales, plane_points, line_vector, line_point):
# Based on http://geomalgorithms.com/a05-_intersect-1.html
epsilon=1e-6
for plane_index in range(plane_normales.shape[0]):
plane_normal = plane_normales[plane_index]
plane_point = plane_points[plane_index, [0,1,2]]
ndotu = plane_normal.dot(line_vector)
# Rotation (order omega phi kappa correct?)
if abs(ndotu) < epsilon:
#print ("no intersection or line is within plane")
pass
else:
w = line_point - plane_point
si = -plane_normal.dot(w) / ndotu
Psi = w + si * line_vector + plane_point
x_axis = [1, 0, 0]
y_axis = [0, 1, 0]
z_axis = [0, 0, 1]
omega = math.radians(Omega)
phi = math.radians(Phi)
kappa = math.radians(Kappa)
# test if in polygon
hit = point_in_poly(Psi[0], Psi[1], [plane_points[plane_index, [0,1]].tolist(), plane_points[plane_index, [3,4]].tolist(), plane_points[plane_index, [6,7]].tolist()] )
if hit:
print(".", end="", flush=True)
sensor_corners_rot_all = sensor_corners
sensor_corners_rot_all = np.array([np.dot(rotation_matrix(z_axis, kappa), sensor_corner) for sensor_corner in sensor_corners_rot_all])
sensor_corners_rot_all = np.array([np.dot(rotation_matrix(y_axis, phi), sensor_corner) for sensor_corner in sensor_corners_rot_all])
sensor_corners_rot_all = np.array([np.dot(rotation_matrix(x_axis, omega), sensor_corner) for sensor_corner in sensor_corners_rot_all])
return Psi.tolist()
return [None, None, None]
print(sensor_corners_rot_all)
def main():
project_dir = r'E:\UAV\_Workspace_\ethz_eschikon_FIP_50m_soja_only_20160526\output\\'
DEM_file = 'DEM_superrough.stl'
# Ray calc
# Based on http://geomalgorithms.com/a05-_intersect-1.html
DEM_stl = mesh.Mesh.from_file(project_dir + DEM_file)
epsilon=1e-6
plane_normales = DEM_stl.normals
plane_points = DEM_stl.points
#Define plane
DEM_stl = mesh.Mesh.from_file(r'E:\UAV\_Workspace_\ethz_eschikon_FIP_50m_soja_only_20160526\output\DEM_rough.stl')
planeNormales = DEM_stl.normals
planePoints = DEM_stl.points
print("init camera parameters")
#Define ray
intersects = np.empty([sensor_corners_rot_all.shape[0],2])
## SONY-RX100II-VIS Specification
sensor_dimension_x = 13.2 # mm
sensor_dimension_y = 8.8 # mm
focal_lengh = 10.4 # mm
sensor_resolution_x = 5472 # pixel
sensor_resolution_y = 3648 # pixel
for corner in range(sensor_corners_rot_all.shape[0]):
print("Point", corner+1)
sensor_corner = sensor_corners_rot_all[corner]
rayDirection = sensor_corner
rayPoint = focal_point
sensor_dimension_x_inM = sensor_dimension_x/1000
sensor_dimension_y_inM = sensor_dimension_y/1000
focal_lengh_inM = focal_lengh / 1000
pixel_size_x = sensor_dimension_x_inM / sensor_resolution_x
pixel_size_y = sensor_dimension_y_inM / sensor_resolution_y
for plane in range(planeNormales.shape[0]):
planeNormal = planeNormales[plane]
planePoint = planePoints[plane, [0,1,2]]
ndotu = planeNormal.dot(rayDirection)
resolution_inter_x = 64
resolution_inter_y = 64
if abs(ndotu) < epsilon:
#print ("no intersection or line is within plane")
pass
else:
w = rayPoint - planePoint
si = -planeNormal.dot(w) / ndotu
Psi = w + si * rayDirection + planePoint
x_axis = [1, 0, 0]
y_axis = [0, 1, 0]
z_axis = [0, 0, 1]
# test if in polygon
hit = point_in_poly(Psi[0], Psi[1], [planePoints[plane, [0,1]].tolist(), planePoints[plane, [3,4]].tolist(), planePoints[plane, [6,7]].tolist()] )
# Relative coordinates of corners of sensor
sensor_Z = focal_lengh_inM
sensor_X_C1u2 = - (sensor_dimension_x_inM / 2)
sensor_X_C3u4 = (sensor_dimension_x_inM / 2)
sensor_Y_C1u4 = (sensor_dimension_y_inM / 2)
sensor_Y_C2u3 = - (sensor_dimension_y_inM / 2)
sensor_x = [x for x in frange((-(sensor_dimension_x_inM / 2)), (sensor_dimension_x_inM / 2), pixel_size_x*resolution_inter_x)]
sensor_y = [y for y in frange((-(sensor_dimension_y_inM / 2)), (sensor_dimension_y_inM / 2), pixel_size_y*resolution_inter_y)]
sensor_pixels = cartesian([sensor_x, sensor_y, sensor_Z])
sensor_corners = np.array([(sensor_X_C1u2, sensor_Y_C1u4, sensor_Z),
(sensor_X_C1u2, sensor_Y_C2u3, sensor_Z),
(sensor_X_C3u4, sensor_Y_C2u3, sensor_Z),
(sensor_X_C3u4, sensor_Y_C1u4, sensor_Z)])
with open(project_dir + 'camera_position_RGB.txt', 'r') as csvfile:
spamreader = csv.reader(csvfile, delimiter='\t', quotechar='|')
next(spamreader, None)
next(spamreader, None)
for row in spamreader:
PhotoID = os.path.splitext(row[0])[0]
print('PROCESSING', PhotoID)
X = float(row[1])
Y = float(row[2])
Z = float(row[3])
Omega = float(row[4])
Phi = float(row[5])
Kappa = float(row[6])
#PhotoID = 'DSC01520'
#X = 2693786.2141234782000000
#Y = 1256310.0950113330000000
#Z = 606.8090010659472000
#Omega = 2.7787812806211063
#Phi = -1.3090272913737455
#Kappa = 141.0244127303900300
# Coordinates of focal poin
focal_point = np.array([X, Y, Z])
# Rotation (order omega phi kappa correct?)
omega = math.radians(Omega)
phi = math.radians(Phi)
kappa = math.radians(Kappa)
print("rotate camera corners")
sensor_corners_rot_all = sensor_corners
sensor_corners_rot_all = np.array([np.dot(rotation_matrix(z_axis, kappa), sensor_corner) for sensor_corner in sensor_corners_rot_all])
sensor_corners_rot_all = np.array([np.dot(rotation_matrix(y_axis, phi), sensor_corner) for sensor_corner in sensor_corners_rot_all])
sensor_corners_rot_all = np.array([np.dot(rotation_matrix(x_axis, omega), sensor_corner) for sensor_corner in sensor_corners_rot_all])
print("calculate position of corners")
#Define plane
line_point=focal_point
# corners = [intersect_point_with_planes(plane_normales, plane_points, line_vector, line_point) for line_vector in sensor_corners_rot_all]
corners = Parallel(n_jobs=4)(delayed(intersect_point_with_planes)(plane_normales, plane_points, line_vector, line_point) for line_vector in sensor_corners_rot_all)
corners.append(corners[0])
print("\ncorners found:", corners)
if(corners[0][0] == None or corners[1][0] is None or corners[2][0] is None or corners[3][0] is None):
print("corners missing, skipping Photo")
continue
print("save to shapefile")
import shapefile
shp = shapefile.Writer(shapeType=shapefile.POLYGON)
shp.field('label')
shp.poly(parts=[corners], shapeType=shapefile.POLYGON)
shp.record('a')
shp.save(project_dir + PhotoID + '_corners')
prj = open(project_dir + PhotoID + '_corners.prj', "w")
epsg = getWKT_PRJ("2056")
prj.write(epsg)
prj.close()
print("rotate pixel matrix")
sensor_pixels_rot = sensor_pixels
sensor_pixels_rot = np.array([np.dot(rotation_matrix(z_axis, kappa), sensor_pixel) for sensor_pixel in sensor_pixels_rot])
sensor_pixels_rot = np.array([np.dot(rotation_matrix(y_axis, phi), sensor_pixel) for sensor_pixel in sensor_pixels_rot])
sensor_pixels_rot = np.array([np.dot(rotation_matrix(x_axis, omega), sensor_pixel) for sensor_pixel in sensor_pixels_rot])
print("calculate pixel matrix position")
# prepare DEM
print("reducing DEM")
corners_np = np.array(corners)
plane_normal_red = []
plane_point_red = []
for plane_index in range(plane_normales.shape[0]):
plane_normal = plane_normales[plane_index]
plane_point1 = plane_points[plane_index, [0,1,2]]
plane_point2 = plane_points[plane_index, [3,4,5]]
plane_point3 = plane_points[plane_index, [6,7,8]]
hit = point_in_poly(plane_point1[0], plane_point1[1], corners_np[:,[0,1]] ) or point_in_poly(plane_point2[0], plane_point2[1], corners_np[:,[0,1]] ) or point_in_poly(plane_point3[0], plane_point3[1], corners_np[:,[0,1]] )
if hit:
intersects[corner] = [Psi[0], Psi[1]]
print ("intersection at", Psi)
break
plane_normal_red.append(plane_normal.tolist())
plane_point_red.append(plane_points[plane_index].tolist())
plane_normal_red_np = np.array(plane_normal_red)
plane_point_red_np = np.array(plane_point_red)
print('Points to calculate:', sensor_pixels_rot.shape)
print('Planes to intersect with:', plane_normal_red_np.shape)
print('calculate intersections, running on', multiprocessing.cpu_count(), 'cores')
pixels= Parallel(n_jobs=multiprocessing.cpu_count()-5)(delayed(intersect_point_with_planes)(plane_normal_red_np, plane_point_red_np, line_vector, line_point) for line_vector in sensor_pixels_rot)
import shapefile
image_x = [x for x in range(0, sensor_resolution_x, resolution_inter_x)]
image_y = [y for y in range(0, sensor_resolution_y, resolution_inter_y)]
sensor_pixelcoords = cartesian([image_x, image_y])
print("calculating angles")
angles = [None] * len(pixels)
for index, pixel in enumerate(pixels):
# rotation in xy plane
if pixel[0] is not None and pixel[1] is not None and pixel[2] is not None:
angles[index] =([math.atan((Y-pixel[1] / X-pixel[0])), math.atan( (Z-pixel[2]) / math.sqrt( (X-pixel[0])*(X-pixel[0]) + (Y-pixel[1])*(Y-pixel[1]) ) ) ])
else:
angles[index] = ([None, None])
shp = shapefile.Writer(shapeType=shapefile.POLYGON)
shp.field('label')
shp.poly(parts=[intersects.tolist() + ([intersects[0].tolist()])], shapeType=shapefile.POLYGON)
shp.record('a')
shp.save(r'E:\UAV\_Workspace_\ethz_eschikon_FIP_50m_soja_only_20160526\output\test')
pixels_all = np.append(sensor_pixelcoords, pixels, axis=1)
prj = open(r'E:\UAV\_Workspace_\ethz_eschikon_FIP_50m_soja_only_20160526\output\test.prj', "w")
epsg = getWKT_PRJ("2056")
prj.write(epsg)
prj.close()
angles_all = np.append(sensor_pixelcoords, angles, axis=1)
print("\nwrite pixel matrix to csv")
with open(project_dir + PhotoID + '_pixel_to_position.csv', 'w') as f:
writer = csv.writer(f)
writer.writerows(pixels_all.tolist())
f.close()
print("\nwrite angle matrix to csv")
with open(project_dir + PhotoID + '_pixle_to_angle.csv', 'w') as f:
writer = csv.writer(f)
writer.writerows(angles_all.tolist())
f.close()
if __name__ == "__main__":
main()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment