Commit 3f0b8c07 authored by luroth's avatar luroth
Browse files

...

parent a65ec9f1
......@@ -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,6 +19,7 @@ import shapefile
from scipy.interpolate import griddata
from osgeo import gdalnumeric
import time
import ctypes
#from shapely.iterops import intersects
#queue = Queue()
......@@ -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,38 +146,33 @@ 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 = 0#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 % 50 == 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)[0: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")
count = 0#queue.qsize()
if verbose:
t_found = time.time()- t_start
count = 0#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")
count = 0#queue.qsize()
if verbose:
t_found = time.time()- t_start
count = 0#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):
......@@ -275,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
......@@ -330,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])
......@@ -349,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)
......@@ -378,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', multiprocessing.cpu_count()-2, '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])))
......
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