Commit 4792beb4 authored by luroth's avatar luroth
Browse files

Init

parents
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>MSc_Lukas_Roth</name>
<comment></comment>
<projects>
</projects>
<buildSpec>
<buildCommand>
<name>org.python.pydev.PyDevBuilder</name>
<arguments>
</arguments>
</buildCommand>
</buildSpec>
<natures>
<nature>org.python.pydev.pythonNature</nature>
</natures>
</projectDescription>
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?eclipse-pydev version="1.0"?><pydev_project>
<pydev_pathproperty name="org.python.pydev.PROJECT_SOURCE_PATH">
<path>/${PROJECT_DIR_NAME}</path>
</pydev_pathproperty>
<pydev_property name="org.python.pydev.PYTHON_PROJECT_VERSION">python 2.7</pydev_property>
<pydev_property name="org.python.pydev.PYTHON_PROJECT_INTERPRETER">MSc</pydev_property>
</pydev_project>
#######################################
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
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