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

.

parent 4792beb4
####################################### #######################################
import math import math
import numpy as np import numpy as np
import geojson import geojson
from osgeo import ogr from osgeo import ogr
from stl import mesh from stl import mesh
from joblib import Parallel, delayed
def rotation_matrix(axis, theta): import multiprocessing
""" from itertools import repeat
Return the rotation matrix associated with counterclockwise rotation about import csv
the given axis by theta radians. from urllib import request
""" import os
axis = np.asarray(axis) import warnings
axis = axis/math.sqrt(np.dot(axis, axis))
a = math.cos(theta/2.0) def frange(start, stop, step):
b, c, d = -axis*math.sin(theta/2.0) i = start
aa, bb, cc, dd = a*a, b*b, c*c, d*d while i < stop:
bc, ad, ac, ab, bd, cd = b*c, a*d, a*c, a*b, b*d, c*d yield i
return np.array([[aa+bb-cc-dd, 2*(bc+ad), 2*(bd-ac)], i += step
[2*(bc-ad), aa+cc-bb-dd, 2*(cd+ab)],
[2*(bd+ac), 2*(cd-ab), aa+dd-bb-cc]]) def cartesian(arrays, out=None):
with warnings.catch_warnings():
## SONY-RX100II-VIS Specification warnings.simplefilter("ignore")
sensor_dimension_x = 13.2 # mm
sensor_dimension_y = 8.8 # mm arrays = [np.asarray(x) for x in arrays]
focal_lengh = 10.4 # mm dtype = arrays[0].dtype
def getWKT_PRJ (epsg_code): n = np.prod([x.size for x in arrays])
import urllib if out is None:
wkt = urllib.request.urlopen('http://spatialreference.org/ref/epsg/{0}/prettywkt/'.format(epsg_code)) out = np.zeros([n, len(arrays)], dtype=dtype)
remove_spaces = wkt.read().decode("utf-8")
remove_spaces = remove_spaces.replace(' ', '') m = n / arrays[0].size
output = remove_spaces.replace("\n", "") out[:,0] = np.repeat(arrays[0], m)
return output if arrays[1:]:
cartesian(arrays[1:], out=out[0:m,1:])
def point_in_poly(x,y,poly): for j in range(1, arrays[0].size):
n = len(poly) out[j*m:(j+1)*m,1:] = out[0:m,1:]
inside = False return out
p1x,p1y = poly[0] def rotation_matrix(axis, theta):
for i in range(n+1): """
p2x,p2y = poly[i % n] Return the rotation matrix associated with counterclockwise rotation about
if y > min(p1y,p2y): the given axis by theta radians.
if y <= max(p1y,p2y): """
if x <= max(p1x,p2x): axis = np.asarray(axis)
if p1y != p2y: axis = axis/math.sqrt(np.dot(axis, axis))
xints = (y-p1y)*(p2x-p1x)/(p2y-p1y)+p1x a = math.cos(theta/2.0)
if p1x == p2x or x <= xints: b, c, d = -axis*math.sin(theta/2.0)
inside = not inside aa, bb, cc, dd = a*a, b*b, c*c, d*d
p1x,p1y = p2x,p2y bc, ad, ac, ab, bd, cd = b*c, a*d, a*c, a*b, b*d, c*d
return np.array([[aa+bb-cc-dd, 2*(bc+ad), 2*(bd-ac)],
return inside [2*(bc-ad), aa+cc-bb-dd, 2*(cd+ab)],
[2*(bd+ac), 2*(cd-ab), aa+dd-bb-cc]])
# Test DATA
#PhotoID = 'DSC01518.JPG'
#X = 2693775.2436063010000000 def getWKT_PRJ (epsg_code):
#Y = 1256295.7234344794000000 wkt = request.urlopen('http://spatialreference.org/ref/epsg/{0}/prettywkt/'.format(epsg_code))
#Z = 607.0859311561650900 remove_spaces = wkt.read().decode("utf-8")
#Omega = 4.2298653063595717 remove_spaces = remove_spaces.replace(' ', '')
#Phi = -1.6345979613024908 output = remove_spaces.replace("\n", "")
#Kappa = 141.9575462861694500 return output
PhotoID = 'DSC01520.JPG' def point_in_poly(x,y,poly):
X = 2693786.2141234782000000 n = len(poly)
Y = 1256310.0950113330000000 inside = False
Z = 606.8090010659472000
Omega = 2.7787812806211063 p1x,p1y = poly[0]
Phi = -1.3090272913737455 for i in range(n+1):
Kappa = 141.0244127303900300 p2x,p2y = poly[i % n]
if y > min(p1y,p2y):
# Coordinates of focal poin if y <= max(p1y,p2y):
focal_point = np.array([X, Y, Z]) if x <= max(p1x,p2x):
if p1y != p2y:
# Relative coordinates of corners of sensor xints = (y-p1y)*(p2x-p1x)/(p2y-p1y)+p1x
sensor_Z = focal_lengh / 1000 if p1x == p2x or x <= xints:
sensor_X_C1u2 = - (sensor_dimension_x / 2000) inside = not inside
sensor_X_C3u4 = (sensor_dimension_x / 2000) p1x,p1y = p2x,p2y
sensor_Y_C1u4 = (sensor_dimension_y / 2000)
sensor_Y_C2u3 = - (sensor_dimension_y / 2000) return inside
sensor_corners = np.array([(sensor_X_C1u2, sensor_Y_C1u4, sensor_Z), def intersect_point_with_planes(plane_normales, plane_points, line_vector, line_point):
(sensor_X_C1u2, sensor_Y_C2u3, sensor_Z), # Based on http://geomalgorithms.com/a05-_intersect-1.html
(sensor_X_C3u4, sensor_Y_C2u3, sensor_Z), epsilon=1e-6
(sensor_X_C3u4, sensor_Y_C1u4, sensor_Z)]) for plane_index in range(plane_normales.shape[0]):
plane_normal = plane_normales[plane_index]
# Rotation (order omega phi kappa correct?) plane_point = plane_points[plane_index, [0,1,2]]
ndotu = plane_normal.dot(line_vector)
x_axis = [1, 0, 0]
y_axis = [0, 1, 0] if abs(ndotu) < epsilon:
z_axis = [0, 0, 1] #print ("no intersection or line is within plane")
omega = math.radians(Omega) pass
phi = math.radians(Phi) else:
kappa = math.radians(Kappa) w = line_point - plane_point
si = -plane_normal.dot(w) / ndotu
sensor_corners_rot_all = sensor_corners Psi = w + si * line_vector + plane_point
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]) # test if in polygon
sensor_corners_rot_all = np.array([np.dot(rotation_matrix(x_axis, omega), sensor_corner) for sensor_corner in sensor_corners_rot_all]) 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)
print(sensor_corners_rot_all)
return Psi.tolist()
# Ray calc return [None, None, None]
# Based on http://geomalgorithms.com/a05-_intersect-1.html
def main():
epsilon=1e-6
project_dir = r'E:\UAV\_Workspace_\ethz_eschikon_FIP_50m_soja_only_20160526\output\\'
#Define plane DEM_file = 'DEM_superrough.stl'
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 DEM_stl = mesh.Mesh.from_file(project_dir + DEM_file)
planePoints = DEM_stl.points
plane_normales = DEM_stl.normals
#Define ray plane_points = DEM_stl.points
intersects = np.empty([sensor_corners_rot_all.shape[0],2])
print("init camera parameters")
for corner in range(sensor_corners_rot_all.shape[0]):
print("Point", corner+1) ## SONY-RX100II-VIS Specification
sensor_corner = sensor_corners_rot_all[corner] sensor_dimension_x = 13.2 # mm
rayDirection = sensor_corner sensor_dimension_y = 8.8 # mm
rayPoint = focal_point focal_lengh = 10.4 # mm
sensor_resolution_x = 5472 # pixel
for plane in range(planeNormales.shape[0]): sensor_resolution_y = 3648 # pixel
planeNormal = planeNormales[plane]
planePoint = planePoints[plane, [0,1,2]] sensor_dimension_x_inM = sensor_dimension_x/1000
ndotu = planeNormal.dot(rayDirection) sensor_dimension_y_inM = sensor_dimension_y/1000
focal_lengh_inM = focal_lengh / 1000
if abs(ndotu) < epsilon: pixel_size_x = sensor_dimension_x_inM / sensor_resolution_x
#print ("no intersection or line is within plane") pixel_size_y = sensor_dimension_y_inM / sensor_resolution_y
pass
else: resolution_inter_x = 64
w = rayPoint - planePoint resolution_inter_y = 64
si = -planeNormal.dot(w) / ndotu
Psi = w + si * rayDirection + planePoint x_axis = [1, 0, 0]
y_axis = [0, 1, 0]
# test if in polygon z_axis = [0, 0, 1]
hit = point_in_poly(Psi[0], Psi[1], [planePoints[plane, [0,1]].tolist(), planePoints[plane, [3,4]].tolist(), planePoints[plane, [6,7]].tolist()] )
if hit: # Relative coordinates of corners of sensor
intersects[corner] = [Psi[0], Psi[1]] sensor_Z = focal_lengh_inM
print ("intersection at", Psi) sensor_X_C1u2 = - (sensor_dimension_x_inM / 2)
break sensor_X_C3u4 = (sensor_dimension_x_inM / 2)
sensor_Y_C1u4 = (sensor_dimension_y_inM / 2)
sensor_Y_C2u3 = - (sensor_dimension_y_inM / 2)
import shapefile 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)]
shp = shapefile.Writer(shapeType=shapefile.POLYGON)
shp.field('label') sensor_pixels = cartesian([sensor_x, sensor_y, sensor_Z])
shp.poly(parts=[intersects.tolist() + ([intersects[0].tolist()])], shapeType=shapefile.POLYGON)
shp.record('a') sensor_corners = np.array([(sensor_X_C1u2, sensor_Y_C1u4, sensor_Z),
shp.save(r'E:\UAV\_Workspace_\ethz_eschikon_FIP_50m_soja_only_20160526\output\test') (sensor_X_C1u2, sensor_Y_C2u3, sensor_Z),
(sensor_X_C3u4, sensor_Y_C2u3, sensor_Z),
prj = open(r'E:\UAV\_Workspace_\ethz_eschikon_FIP_50m_soja_only_20160526\output\test.prj', "w") (sensor_X_C3u4, sensor_Y_C1u4, sensor_Z)])
epsg = getWKT_PRJ("2056")
prj.write(epsg)
prj.close() 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:
\ No newline at end of file
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:
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)
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])
pixels_all = np.append(sensor_pixelcoords, pixels, axis=1)
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