Commit 01b56fca authored by luroth's avatar luroth
Browse files

.

parent bcfce3f5
...@@ -7,6 +7,7 @@ from stl import mesh ...@@ -7,6 +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 itertools import repeat from itertools import repeat
import csv import csv
import os import os
...@@ -18,7 +19,9 @@ import shapefile ...@@ -18,7 +19,9 @@ import shapefile
from scipy.interpolate import griddata from scipy.interpolate import griddata
from osgeo import gdalnumeric from osgeo import gdalnumeric
import time import time
#from shapely.iterops import intersects
queue = Queue()
def rotation_matrix(axis, theta): def rotation_matrix(axis, theta):
axis = np.asarray(axis) axis = np.asarray(axis)
...@@ -120,20 +123,25 @@ def intersect_line_with_planes(plane_normales, plane_points, line_vector, line_p ...@@ -120,20 +123,25 @@ def intersect_line_with_planes(plane_normales, plane_points, line_vector, line_p
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_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])]) 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 = (intersect_point1 if hit_1 else intersect_point2) approximation_point = np.array((intersect_point1 if hit_1 else intersect_point2)*100, dtype='int')
t_approximation = time.time() t_approximation = time.time()
# create index with closes points first # create index with closes points first
plane_delta_pos = plane_points[:, [0,1]] - approximation_point[[0,1]] plane_delta_pos = np.array(plane_points[:, [0,1]]*100, dtype='int') - approximation_point[[0,1]]
plane_delta_dist = np.sqrt(plane_delta_pos[:,0]**2 + plane_delta_pos[:,1]**2) plane_delta_dist = (plane_delta_pos[:,0]**2) + (plane_delta_pos[:,1]**2)
index_list = np.argsort(plane_delta_dist) 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() t_sort = time.time()
# for all planes # for all planes
tries= 0
for plane_index in index_list: for plane_index in index_list:
if(plane_delta_dist[plane_index]>5): #if(plane_delta_dist[plane_index]>4):
print("?", end="\n" ,flush=True) # print("Pixel not found.", flush=True)
return [np.nan, np.nan, np.nan] # return [np.nan, np.nan, np.nan]
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]]
...@@ -145,14 +153,78 @@ def intersect_line_with_planes(plane_normales, plane_points, line_vector, line_p ...@@ -145,14 +153,78 @@ def intersect_line_with_planes(plane_normales, plane_points, line_vector, line_p
hit = point_in_poly(intersect_point[0], intersect_point[1], [(p1_x, p1_y), (p2_x, p2_y), (p3_x, p3_y)] ) hit = point_in_poly(intersect_point[0], intersect_point[1], [(p1_x, p1_y), (p2_x, p2_y), (p3_x, p3_y)] )
if hit: if hit:
t_found= time.time() t_found= time.time()
print("Pixel calculated. Tries: {0}, Times: Init normal: {1}, Approximation: {2}, Sort: {3}, Intersect: {4}".format(t_init_normales-t_start, t_approximation-t_start, t_sort-t_start, t_found-t_start), flush=True) 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() return intersect_point.tolist()
else: else:
tries+=1
pass pass
print("Pixel not found.", flush=True) print("Pixel not found.", flush=True)
return [np.nan, np.nan, np.nan] return [np.nan, np.nan, np.nan]
def intersect_line_with_planes_2(plane_normales, plane_points, line_vector, line_point, corners, verbose=False):
if verbose: 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]
if verbose: t_init_normales = time.time() - t_start
# 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')
if verbose: t_approximation = time.time() - t_start
# create index with closes points in first partition
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, 99)[0:99]
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:
if verbose:
t_found = time.time()- t_start
print("{0}: Pixel calculated. Iteration: {1}, Times: Init normal: {2}, Approximation: {3}, Add: {4}, Sort: {5}, Intersect: {6}".format(count, tries, t_init_normales, t_approximation, t_add, t_sort, t_found), flush=True)
return intersects
else:
if verbose: print("Pixel not found. Process all", flush=True)
index_list = np.sort(plane_delta_dist)
process_planes(index_list, plane_normales, plane_points, line_vector, line_point)
if intersects is not None:
if verbose:
t_found = time.time()- t_start
print("{0}: Pixel calculated. Iteration: {1}, Times: Init normal: {2}, Approximation: {3}, Add: {4}, Sort: {5}, Intersect: {6}".format(count, tries, t_init_normales, t_approximation, t_add, t_sort, t_found), flush=True)
return intersects
else:
return [np.nan, np.nan, np.nan]
def process_planes(index_list, plane_normales, plane_points, line_vector, line_point):
for plane_index in index_list:
# get plane normale and point
plane_normal = plane_normales[plane_index]
plane_point = plane_points[plane_index, [0,1,2]]
# intersect line with plane
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:
return intersect_point.tolist()
else:
pass
return None
def main(argv): def main(argv):
camera_position_file = '' camera_position_file = ''
DEM_stl_file = '' DEM_stl_file = ''
...@@ -265,7 +337,10 @@ def main(argv): ...@@ -265,7 +337,10 @@ def main(argv):
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]) #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])
ground_corner_coords = np.array(Parallel(n_jobs=4)(delayed(intersect_line_with_planes)(DEM_normales, DEM_points, sensor_corner_vector, focal_point, corners_global) for sensor_corner_vector in sensor_corner_vectors), dtype=np.float64) 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)
#ground_corner_coords = Queue()
print("\ncorners found:", ground_corner_coords) print("\ncorners found:", ground_corner_coords)
if(np.isnan(ground_corner_coords.flat).any()): if(np.isnan(ground_corner_coords.flat).any()):
......
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