Commit 1c4ba36f authored by luroth's avatar luroth
Browse files
parents 3323f0b7 063804a7
......@@ -7,7 +7,7 @@ from stl import mesh
import stl.base as stl
from joblib import Parallel, delayed
import multiprocessing
from multiprocessing import Process, Queue
from multiprocessing import Process, Queue, Manager
from itertools import repeat
import csv
import os
......@@ -19,9 +19,10 @@ import shapefile
from scipy.interpolate import griddata
from osgeo import gdalnumeric
import time
import ctypes
#from shapely.iterops import intersects
queue = Queue()
#queue = Queue()
def rotation_matrix(axis, theta):
axis = np.asarray(axis)
......@@ -96,7 +97,7 @@ def point_in_poly(x,y,poly):
return inside
def ray_trace(line_vector, line_point, plane_point, plane_normal):
epsilon=1e-8
epsilon=1e-64
ndotu = plane_normal.dot(line_vector)
if abs(ndotu) < epsilon:
......@@ -108,61 +109,20 @@ def ray_trace(line_vector, line_point, plane_point, plane_normal):
Psi = w + si * line_vector + plane_point
return Psi
def intersect_line_with_planes(plane_normales, plane_points, line_vector, line_point, corners):
t_start = time.time()
# create corner polygons
corners_poly1_normale = get_polygon_normale([corners[0], corners[1], corners[2]])
point_poly1 = corners[0]
corners_poly2_normale = get_polygon_normale([corners[2], corners[3], corners[0]])
point_poly2 = corners[2]
t_init_normales = time.time()
# estimate location with corner polygons
intersect_point1 = ray_trace(line_vector, line_point, point_poly1, corners_poly1_normale)
intersect_point2 = ray_trace(line_vector, line_point, point_poly2, corners_poly2_normale)
hit_1 = point_in_poly(intersect_point1[0], intersect_point1[1], [(corners[0][0],corners[0][1]), (corners[1][0],corners[1][1]), (corners[2][0],corners[2][1])])
hit_2 = point_in_poly(intersect_point2[0], intersect_point2[1], [(corners[2][0],corners[0][1]), (corners[3][0],corners[1][1]), (corners[0][0],corners[2][1])])
approximation_point = np.array((intersect_point1 if hit_1 else intersect_point2)*100, dtype='int')
t_approximation = time.time()
# create index with closes points first
plane_delta_pos = np.array(plane_points[:, [0,1]]*100, dtype='int') - approximation_point[[0,1]]
plane_delta_dist = (plane_delta_pos[:,0]**2) + (plane_delta_pos[:,1]**2)
t_add = time.time()
#plane_delta_dist_masked = np.ma.masked_greater(plane_delta_dist,4)
#index_list = np.ma.argsort(plane_delta_dist_masked)
index_list = np.argpartition(plane_delta_dist, 200)[0:200]
#index_list= np.argsort(plane_delta_dist[index_list])
t_sort = time.time()
# for all planes
tries= 0
for plane_index in index_list:
#if(plane_delta_dist[plane_index]>4):
# print("Pixel not found.", flush=True)
# return [np.nan, np.nan, np.nan]
def intersect_worker(work_queue, result_queue, plane_normales, plane_points):
for job in iter(work_queue.get, 'STOP'):
pixel_nr = job['pixel_nr']
line_vector = job['line_vector']
line_point = job['line_point']
corners = job['corners']
verbose = job['verbose']
limit = job['limit']
plane_normal = plane_normales[plane_index]
plane_point = plane_points[plane_index, [0,1,2]]
intersect_point = ray_trace(line_vector, line_point, plane_point, plane_normal)
if(intersect_point is not None):
# test if in polygon
p1_x, p1_y, p2_x, p2_y, p3_x, p3_y = plane_points[plane_index, [0,1,3,4,6,7]].tolist()
hit = point_in_poly(intersect_point[0], intersect_point[1], [(p1_x, p1_y), (p2_x, p2_y), (p3_x, p3_y)] )
if hit:
t_found= time.time()
print("Pixel calculated. Tries: {0}, Times: Init normal: {1}, Approximation: {2}, Add: {3}, Sort: {4}, Intersect: {5}".format(tries, t_init_normales-t_start, t_approximation-t_start, t_add-t_start, t_sort-t_start, t_found-t_start), flush=True)
return intersect_point.tolist()
else:
tries+=1
pass
intersects = intersect_line_with_planes(pixel_nr, plane_normales, plane_points, line_vector, line_point, corners, verbose, limit)
result_queue.put([pixel_nr, intersects])
print("Pixel not found.", flush=True)
return [np.nan, np.nan, np.nan]
def intersect_line_with_planes_2(plane_normales, plane_points, line_vector, line_point, corners, verbose=False, total=0):
def intersect_line_with_planes(pixel_nr, plane_normales, plane_points, line_vector, line_point, corners, verbose=False, limit=10000):
if verbose: t_start = time.time()
......@@ -186,42 +146,37 @@ def intersect_line_with_planes_2(plane_normales, plane_points, line_vector, line
plane_delta_pos = np.array(plane_points[:, [0,1]]*100, dtype='int') - approximation_point[[0,1]]
plane_delta_dist = (plane_delta_pos[:,0]**2) + (plane_delta_pos[:,1]**2)
t_add = time.time()
index_list = np.argpartition(plane_delta_dist,199)[0:199]
index_list = np.argpartition(plane_delta_dist,199)[0:200]
if verbose: t_sort = time.time() - t_start
intersects = process_planes(index_list, plane_normales, plane_points, line_vector, line_point)
if intersects is not None:
queue.put("yep")
count = queue.qsize()
if verbose:
t_found = time.time()- t_start
if verbose:
print("{0}: Pixel calculated. Times: Init normal: {1}, Approximation: {2}, Add: {3}, Sort: {4}, Intersect: {5}".format(count, t_init_normales, t_approximation, t_add, t_sort, t_found), flush=True)
if count % 20 == 0:
print("{} / {} done".format(count, total))
print("Pixel {0} calculated. Times: Init normal: {1}, Approximation: {2}, Add: {3}, Sort: {4}, Intersect: {5}".format(pixel_nr, t_init_normales, t_approximation, t_add, t_sort, t_found), flush=True)
elif pixel_nr % 50 == 0:
print("{} done".format(pixel_nr), flush=True)
return intersects
else:
index_list = np.argsort(plane_delta_dist)
process_planes(index_list, plane_normales, plane_points, line_vector, line_point, 10000)
index_list = np.argsort(plane_delta_dist)[0:limit]
intersects = process_planes(index_list, plane_normales, plane_points, line_vector, line_point)
if intersects is not None:
queue.put("yep")
if verbose:
t_found = time.time()- t_start
count = queue.qsize()
if verbose: print("{0}: Pixel calculated, full run. Times: Init normal: {1}, Approximation: {2}, Add: {3}, Sort: {4}, Intersect: {5}".format(count, t_init_normales, t_approximation, t_add, t_sort, t_found), flush=True)
print("Pixel {0} calculated, full run. Times: Init normal: {1}, Approximation: {2}, Add: {3}, Sort: {4}, Intersect: {5}".format(pixel_nr, t_init_normales, t_approximation, t_add, t_sort, t_found), flush=True)
elif pixel_nr % 50 == 0:
print("{} done".format(pixel_nr), flush=True)
return intersects
else:
queue.put("nop")
if verbose:
t_found = time.time()- t_start
count = queue.qsize()
print("{0}: Pixel not found. Times: Init normal: {1}, Approximation: {2}, Add: {3}, Sort: {4}, Intersect: {5}".format(count, t_init_normales, t_approximation, t_add, t_sort, t_found), flush=True)
print("Pixel {0} not found. Times: Init normal: {1}, Approximation: {2}, Add: {3}, Sort: {4}, Intersect: {5}".format(pixel_nr, t_init_normales, t_approximation, t_add, t_sort, t_found), flush=True)
elif pixel_nr % 50 == 0:
print("{} done".format(pixel_nr), flush=True)
return [np.nan, np.nan, np.nan]
def process_planes(index_list, plane_normales, plane_points, line_vector, line_point, limit=None):
count = 0
def process_planes(index_list, plane_normales, plane_points, line_vector, line_point):
for plane_index in index_list:
count += 1
# get plane normale and point
plane_normal = plane_normales[plane_index]
plane_point = plane_points[plane_index, [0,1,2]]
......@@ -235,10 +190,7 @@ def process_planes(index_list, plane_normales, plane_points, line_vector, line_p
if hit:
return intersect_point.tolist()
else:
if limit is not None and count>limit:
return None
else:
pass
pass
return None
def main(argv):
......@@ -278,9 +230,20 @@ def main(argv):
print("loading DEM")
DEM_stl = mesh.Mesh.from_file(DEM_stl_file)
DEM_normales_raw = DEM_stl.normals
DEM_points_raw = DEM_stl.points
DEM_normales = DEM_stl.normals
DEM_points = DEM_stl.points
# Prepare for shared access
# DEM_normales_flat = multiprocessing.Array(ctypes.c_double, DEM_normales_raw.reshape(DEM_normales_raw.shape[0]*DEM_normales_raw.shape[1]))
# DEM_points_flat = multiprocessing.Array(ctypes.c_double, DEM_points_raw.reshape(DEM_points_raw.shape[0]*DEM_points_raw.shape[1]))
# DEM_normales = np.ctypeslib.as_array(DEM_normales_flat.get_obj())
# DEM_points = np.ctypeslib.as_array(DEM_points_flat.get_obj())
# DEM_normales = DEM_normales.reshape(DEM_normales_raw.shape)
# DEM_points = DEM_points.reshape(DEM_points_raw.shape)
minx, maxx, miny, maxy, minz, maxz = find_mins_maxs(DEM_stl)
meanz = (maxz + minz)/2
......@@ -333,14 +296,6 @@ def main(argv):
# in radian
omega_rad, phi_rad, kappa_rad = (math.radians(x) for x in [Omega, Phi, Kappa])
#PhotoID = 'DSC01520'
#X = 2693786.2141234782000000
#Y = 1256310.0950113330000000
#Z = 606.8090010659472000
#Omega = 2.7787812806211063
#Phi = -1.3090272913737455
#Kappa = 141.0244127303900300
# Coordinates of focal point
focal_point = np.array([X, Y, Z])
......@@ -352,10 +307,39 @@ def main(argv):
print("calculate ground points for corners")
sensor_corner_vectors = sensor_pixels_vectors[corner_index]
#ground_corner_coords = np.array([intersect_line_with_planes(DEM_normales, DEM_points, sensor_corner_vector, focal_point, corners_global) for sensor_corner_vector in sensor_corner_vectors])
while not queue.empty():
queue.get()
ground_corner_coords = np.array(Parallel(n_jobs=4)(delayed(intersect_line_with_planes_2)(DEM_normales, DEM_points, sensor_corner_vector, focal_point, corners_global, True) for sensor_corner_vector in sensor_corner_vectors), dtype=np.float64)
m = Manager()
jobs = m.Queue()
results = m.Queue()
processes = []
for pixel_nr, vector in enumerate(sensor_corner_vectors):
job = dict()
job['pixel_nr'] = pixel_nr
job['line_vector'] = vector
job['line_point'] = focal_point
job['corners'] = corners_global
job['verbose'] = True
job['limit'] = None
jobs.put(job)
for w in range(4):
p = Process(target=intersect_worker, args=(jobs, results, DEM_normales, DEM_points))
p.start()
processes.append(p)
jobs.put('STOP')
for p in processes:
p.join()
results.put('STOP')
corners = []
indexs = []
for pixel in iter(results.get, 'STOP'):
print(pixel)
corners.append(pixel[1])
indexs.append(pixel[0])
ground_corner_coords = np.array(corners)
ground_corner_coords = ground_corner_coords[np.argsort(indexs)]
print("\ncorners found:\n", ground_corner_coords)
......@@ -381,15 +365,44 @@ def main(argv):
print("\n------\ncalculate now ground points for all sample pixels:")
print('Points to calculate:', sensor_pixels_vectors.shape[0])
print('Planes to intersect with:', DEM_normales.shape[0])
print('calculate intersections, running on', multiprocessing.cpu_count(), 'cores\n')
#ground_pixel_coords=[]
#for sensor_pixels_vector in sensor_pixels_vectors:
# ground_pixel_coords.append(intersect_line_with_planes(DEM_normales, DEM_points, sensor_pixels_vector, focal_point, ground_corner_coords))
#ground_pixel_coords = np.array(ground_pixel_coords)
while not queue.empty():
queue.get()
ground_pixel_coords= np.array(Parallel(n_jobs=multiprocessing.cpu_count())(delayed(intersect_line_with_planes_2)(DEM_normales, DEM_points, sensor_pixels_vector, focal_point, ground_corner_coords, False, len(sensor_pixels_vectors)) for sensor_pixels_vector in sensor_pixels_vectors))
print('calculate intersections, running on', 10, 'cores\n')
m = Manager()
jobs = m.Queue()
results = m.Queue()
processes = []
for pixel_nr, vector in enumerate(sensor_pixels_vectors):
job = dict()
job['pixel_nr'] = pixel_nr
job['line_vector'] = vector
job['line_point'] = focal_point
job['corners'] = ground_corner_coords
job['verbose'] = False
job['limit'] = 10000
jobs.put(job)
for w in range(multiprocessing.cpu_count()-2):
p = Process(target=intersect_worker, args=(jobs, results, DEM_normales, DEM_points))
p.start()
processes.append(p)
jobs.put('STOP')
for p in processes:
p.join()
print("\n...done. Getting results")
results.put('STOP')
pixels = []
indexs = []
for pixel in iter(results.get, 'STOP'):
pixels.append(pixel[1])
indexs.append(pixel[0])
ground_pixel_coords = np.array(pixels)
ground_pixel_coords = ground_pixel_coords[np.argsort(indexs)]
print("\ncalculating view angles")
ground_pixel_angle_xy = np.arctan(((Y-ground_pixel_coords[:,1]) / (X-ground_pixel_coords[:,0])))
......@@ -464,4 +477,4 @@ def main(argv):
if __name__ == "__main__":
main(sys.argv[1:])
print("\n============\All photos processed, END\n")
print("\n============\nAll photos processed, END\n")
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