Commit 1c4ba36f authored by luroth's avatar luroth
Browse files
parents 3323f0b7 063804a7
...@@ -7,7 +7,7 @@ from stl import mesh ...@@ -7,7 +7,7 @@ from stl import mesh
import stl.base as stl import stl.base as stl
from joblib import Parallel, delayed from joblib import Parallel, delayed
import multiprocessing import multiprocessing
from multiprocessing import Process, Queue from multiprocessing import Process, Queue, Manager
from itertools import repeat from itertools import repeat
import csv import csv
import os import os
...@@ -19,9 +19,10 @@ import shapefile ...@@ -19,9 +19,10 @@ import shapefile
from scipy.interpolate import griddata from scipy.interpolate import griddata
from osgeo import gdalnumeric from osgeo import gdalnumeric
import time import time
import ctypes
#from shapely.iterops import intersects #from shapely.iterops import intersects
queue = Queue() #queue = Queue()
def rotation_matrix(axis, theta): def rotation_matrix(axis, theta):
axis = np.asarray(axis) axis = np.asarray(axis)
...@@ -96,7 +97,7 @@ def point_in_poly(x,y,poly): ...@@ -96,7 +97,7 @@ def point_in_poly(x,y,poly):
return inside return inside
def ray_trace(line_vector, line_point, plane_point, plane_normal): def ray_trace(line_vector, line_point, plane_point, plane_normal):
epsilon=1e-8 epsilon=1e-64
ndotu = plane_normal.dot(line_vector) ndotu = plane_normal.dot(line_vector)
if abs(ndotu) < epsilon: if abs(ndotu) < epsilon:
...@@ -108,61 +109,20 @@ def ray_trace(line_vector, line_point, plane_point, plane_normal): ...@@ -108,61 +109,20 @@ def ray_trace(line_vector, line_point, plane_point, plane_normal):
Psi = w + si * line_vector + plane_point Psi = w + si * line_vector + plane_point
return Psi return Psi
def intersect_line_with_planes(plane_normales, plane_points, line_vector, line_point, corners): def intersect_worker(work_queue, result_queue, plane_normales, plane_points):
t_start = time.time() for job in iter(work_queue.get, 'STOP'):
# create corner polygons pixel_nr = job['pixel_nr']
corners_poly1_normale = get_polygon_normale([corners[0], corners[1], corners[2]]) line_vector = job['line_vector']
point_poly1 = corners[0] line_point = job['line_point']
corners_poly2_normale = get_polygon_normale([corners[2], corners[3], corners[0]]) corners = job['corners']
point_poly2 = corners[2] verbose = job['verbose']
t_init_normales = time.time() limit = job['limit']
# 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]
plane_normal = plane_normales[plane_index] intersects = intersect_line_with_planes(pixel_nr, plane_normales, plane_points, line_vector, line_point, corners, verbose, limit)
plane_point = plane_points[plane_index, [0,1,2]] result_queue.put([pixel_nr, intersects])
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
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() if verbose: t_start = time.time()
...@@ -186,42 +146,37 @@ def intersect_line_with_planes_2(plane_normales, plane_points, line_vector, line ...@@ -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_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) plane_delta_dist = (plane_delta_pos[:,0]**2) + (plane_delta_pos[:,1]**2)
t_add = time.time() 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 if verbose: t_sort = time.time() - t_start
intersects = process_planes(index_list, plane_normales, plane_points, line_vector, line_point) intersects = process_planes(index_list, plane_normales, plane_points, line_vector, line_point)
if intersects is not None: if intersects is not None:
queue.put("yep")
count = queue.qsize()
if verbose: if verbose:
t_found = time.time()- t_start t_found = time.time()- t_start
if verbose: 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)
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) elif pixel_nr % 50 == 0:
if count % 20 == 0: print("{} done".format(pixel_nr), flush=True)
print("{} / {} done".format(count, total))
return intersects return intersects
else: else:
index_list = np.argsort(plane_delta_dist) index_list = np.argsort(plane_delta_dist)[0:limit]
process_planes(index_list, plane_normales, plane_points, line_vector, line_point, 10000) intersects = process_planes(index_list, plane_normales, plane_points, line_vector, line_point)
if intersects is not None: if intersects is not None:
queue.put("yep")
if verbose: if verbose:
t_found = time.time()- t_start t_found = time.time()- t_start
count = queue.qsize() 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)
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) elif pixel_nr % 50 == 0:
print("{} done".format(pixel_nr), flush=True)
return intersects return intersects
else: else:
queue.put("nop")
if verbose: if verbose:
t_found = time.time()- t_start t_found = time.time()- t_start
count = queue.qsize() 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)
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) elif pixel_nr % 50 == 0:
print("{} done".format(pixel_nr), flush=True)
return [np.nan, np.nan, np.nan] return [np.nan, np.nan, np.nan]
def process_planes(index_list, plane_normales, plane_points, line_vector, line_point, limit=None): def process_planes(index_list, plane_normales, plane_points, line_vector, line_point):
count = 0
for plane_index in index_list: for plane_index in index_list:
count += 1
# get plane normale and point # get plane normale and point
plane_normal = plane_normales[plane_index] plane_normal = plane_normales[plane_index]
plane_point = plane_points[plane_index, [0,1,2]] 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 ...@@ -235,10 +190,7 @@ def process_planes(index_list, plane_normales, plane_points, line_vector, line_p
if hit: if hit:
return intersect_point.tolist() return intersect_point.tolist()
else: else:
if limit is not None and count>limit: pass
return None
else:
pass
return None return None
def main(argv): def main(argv):
...@@ -278,9 +230,20 @@ def main(argv): ...@@ -278,9 +230,20 @@ def main(argv):
print("loading DEM") print("loading DEM")
DEM_stl = mesh.Mesh.from_file(DEM_stl_file) 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_normales = DEM_stl.normals
DEM_points = DEM_stl.points 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) minx, maxx, miny, maxy, minz, maxz = find_mins_maxs(DEM_stl)
meanz = (maxz + minz)/2 meanz = (maxz + minz)/2
...@@ -333,14 +296,6 @@ def main(argv): ...@@ -333,14 +296,6 @@ def main(argv):
# in radian # in radian
omega_rad, phi_rad, kappa_rad = (math.radians(x) for x in [Omega, Phi, Kappa]) 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 # Coordinates of focal point
focal_point = np.array([X, Y, Z]) focal_point = np.array([X, Y, Z])
...@@ -352,10 +307,39 @@ def main(argv): ...@@ -352,10 +307,39 @@ def main(argv):
print("calculate ground points for corners") print("calculate ground points for corners")
sensor_corner_vectors = sensor_pixels_vectors[corner_index] 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]) m = Manager()
while not queue.empty(): jobs = m.Queue()
queue.get() results = m.Queue()
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) 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) print("\ncorners found:\n", ground_corner_coords)
...@@ -381,15 +365,44 @@ def main(argv): ...@@ -381,15 +365,44 @@ def main(argv):
print("\n------\ncalculate now ground points for all sample pixels:") print("\n------\ncalculate now ground points for all sample pixels:")
print('Points to calculate:', sensor_pixels_vectors.shape[0]) print('Points to calculate:', sensor_pixels_vectors.shape[0])
print('Planes to intersect with:', DEM_normales.shape[0]) print('Planes to intersect with:', DEM_normales.shape[0])
print('calculate intersections, running on', multiprocessing.cpu_count(), 'cores\n') print('calculate intersections, running on', 10, 'cores\n')
#ground_pixel_coords=[] m = Manager()
#for sensor_pixels_vector in sensor_pixels_vectors: jobs = m.Queue()
# ground_pixel_coords.append(intersect_line_with_planes(DEM_normales, DEM_points, sensor_pixels_vector, focal_point, ground_corner_coords)) results = m.Queue()
#ground_pixel_coords = np.array(ground_pixel_coords) processes = []
while not queue.empty():
queue.get() for pixel_nr, vector in enumerate(sensor_pixels_vectors):
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)) 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") print("\ncalculating view angles")
ground_pixel_angle_xy = np.arctan(((Y-ground_pixel_coords[:,1]) / (X-ground_pixel_coords[:,0]))) ground_pixel_angle_xy = np.arctan(((Y-ground_pixel_coords[:,1]) / (X-ground_pixel_coords[:,0])))
...@@ -464,4 +477,4 @@ def main(argv): ...@@ -464,4 +477,4 @@ def main(argv):
if __name__ == "__main__": if __name__ == "__main__":
main(sys.argv[1:]) 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