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

.

parent 4792beb4
#######################################
import math
import numpy as np
import geojson
from osgeo import ogr
from stl import mesh
def rotation_matrix(axis, theta):
"""
Return the rotation matrix associated with counterclockwise rotation about
the given axis by theta radians.
"""
axis = np.asarray(axis)
axis = axis/math.sqrt(np.dot(axis, axis))
a = math.cos(theta/2.0)
b, c, d = -axis*math.sin(theta/2.0)
aa, bb, cc, dd = a*a, b*b, c*c, d*d
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)],
[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))
remove_spaces = wkt.read().decode("utf-8")
remove_spaces = remove_spaces.replace(' ', '')
output = remove_spaces.replace("\n", "")
return output
def point_in_poly(x,y,poly):
n = len(poly)
inside = False
p1x,p1y = poly[0]
for i in range(n+1):
p2x,p2y = poly[i % n]
if y > min(p1y,p2y):
if y <= max(p1y,p2y):
if x <= max(p1x,p2x):
if p1y != p2y:
xints = (y-p1y)*(p2x-p1x)/(p2y-p1y)+p1x
if p1x == p2x or x <= xints:
inside = not inside
p1x,p1y = p2x,p2y
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)])
# Rotation (order omega phi kappa correct?)
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)
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(sensor_corners_rot_all)
# Ray calc
# Based on http://geomalgorithms.com/a05-_intersect-1.html
epsilon=1e-6
#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
#Define ray
intersects = np.empty([sensor_corners_rot_all.shape[0],2])
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
for plane in range(planeNormales.shape[0]):
planeNormal = planeNormales[plane]
planePoint = planePoints[plane, [0,1,2]]
ndotu = planeNormal.dot(rayDirection)
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
# 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()] )
if hit:
intersects[corner] = [Psi[0], Psi[1]]
print ("intersection at", Psi)
break
import shapefile
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')
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()
\ No newline at end of file
#######################################
import math
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):
"""
Return the rotation matrix associated with counterclockwise rotation about
the given axis by theta radians.
"""
axis = np.asarray(axis)
axis = axis/math.sqrt(np.dot(axis, axis))
a = math.cos(theta/2.0)
b, c, d = -axis*math.sin(theta/2.0)
aa, bb, cc, dd = a*a, b*b, c*c, d*d
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)],
[2*(bc-ad), aa+cc-bb-dd, 2*(cd+ab)],
[2*(bd+ac), 2*(cd-ab), aa+dd-bb-cc]])
def getWKT_PRJ (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", "")
return output
def point_in_poly(x,y,poly):
n = len(poly)
inside = False
p1x,p1y = poly[0]
for i in range(n+1):
p2x,p2y = poly[i % n]
if y > min(p1y,p2y):
if y <= max(p1y,p2y):
if x <= max(p1x,p2x):
if p1y != p2y:
xints = (y-p1y)*(p2x-p1x)/(p2y-p1y)+p1x
if p1x == p2x or x <= xints:
inside = not inside
p1x,p1y = p2x,p2y
return inside
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)
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
# 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)
return Psi.tolist()
return [None, None, None]
def main():
project_dir = r'E:\UAV\_Workspace_\ethz_eschikon_FIP_50m_soja_only_20160526\output\\'
DEM_file = 'DEM_superrough.stl'
DEM_stl = mesh.Mesh.from_file(project_dir + DEM_file)
plane_normales = DEM_stl.normals
plane_points = DEM_stl.points
print("init camera parameters")
## 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
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
resolution_inter_x = 64
resolution_inter_y = 64
x_axis = [1, 0, 0]
y_axis = [0, 1, 0]
z_axis = [0, 0, 1]
# 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:
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