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

.

parent bcfce3f5
......@@ -7,6 +7,7 @@ from stl import mesh
import stl.base as stl
from joblib import Parallel, delayed
import multiprocessing
from multiprocessing import Process, Queue
from itertools import repeat
import csv
import os
......@@ -18,7 +19,9 @@ import shapefile
from scipy.interpolate import griddata
from osgeo import gdalnumeric
import time
#from shapely.iterops import intersects
queue = Queue()
def rotation_matrix(axis, theta):
axis = np.asarray(axis)
......@@ -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_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()
# create index with closes points first
plane_delta_pos = plane_points[:, [0,1]] - approximation_point[[0,1]]
plane_delta_dist = np.sqrt(plane_delta_pos[:,0]**2 + plane_delta_pos[:,1]**2)
index_list = np.argsort(plane_delta_dist)
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]>5):
print("?", end="\n" ,flush=True)
return [np.nan, np.nan, np.nan]
#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]
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
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}, 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()
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):
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):
camera_position_file = ''
DEM_stl_file = ''
......@@ -265,7 +337,10 @@ def main(argv):
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(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)
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