LinePlaneIntersection.py 19.5 KB
Newer Older
luroth's avatar
.    
luroth committed
1
#######################################
luroth's avatar
.    
luroth committed
2
import math as math
luroth's avatar
.    
luroth committed
3
4
5
import numpy as np
import geojson
from stl import mesh
luroth's avatar
.    
luroth committed
6
import stl.base as stl
luroth's avatar
.    
luroth committed
7
8
from joblib import Parallel, delayed
import multiprocessing
luroth's avatar
...    
luroth committed
9
from multiprocessing import Process, Queue, Manager
luroth's avatar
.    
luroth committed
10
11
12
from itertools import repeat
import csv
import os
luroth's avatar
.    
luroth committed
13
14
import sys
import getopt
luroth's avatar
.    
luroth committed
15
import warnings
luroth's avatar
.    
luroth committed
16
17
import matplotlib.pyplot as plt
import shapefile
luroth's avatar
.    
luroth committed
18
from scipy.interpolate import griddata
luroth's avatar
.    
luroth committed
19
20
import gdal
import gdalnumeric
luroth's avatar
luroth committed
21
import time
luroth's avatar
...    
luroth committed
22
import ctypes
luroth's avatar
luroth committed
23

luroth's avatar
.    
luroth committed
24
from Common import *
luroth's avatar
.    
luroth committed
25
26


luroth's avatar
.    
luroth committed
27
def ray_trace(line_vector, line_point, plane_point, plane_normal):
luroth's avatar
...    
luroth committed
28
    epsilon=1e-64
luroth's avatar
.    
luroth committed
29
    ndotu = plane_normal.dot(line_vector) 
luroth's avatar
.    
luroth committed
30
    
luroth's avatar
.    
luroth committed
31
32
33
34
35
36
37
38
    if abs(ndotu) < epsilon:
        #no intersection or line is within plane
        return None
    else:
        w = line_point - plane_point
        si = -plane_normal.dot(w) / ndotu
        Psi = w + si * line_vector + plane_point
        return Psi
luroth's avatar
.    
luroth committed
39

luroth's avatar
...    
luroth committed
40
41
42
43
44
45
46
47
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']
luroth's avatar
.    
luroth committed
48
        
luroth's avatar
...    
luroth committed
49
50
        intersects = intersect_line_with_planes(pixel_nr, plane_normales, plane_points, line_vector, line_point, corners, verbose, limit)
        result_queue.put([pixel_nr, intersects])
luroth's avatar
.    
luroth committed
51

luroth's avatar
.    
luroth committed
52

luroth's avatar
...    
luroth committed
53
def intersect_line_with_planes(pixel_nr, plane_normales, plane_points, line_vector, line_point, corners, verbose=False, limit=10000): 
luroth's avatar
.    
luroth committed
54
55
56
57
58
59
60
61
62
63
64
65
66

    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)
luroth's avatar
.    
luroth committed
67
68
    hit_1 = point_in_polygon2(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_polygon2(intersect_point2[0], intersect_point2[1], [(corners[2][0],corners[0][1]), (corners[3][0],corners[1][1]), (corners[0][0],corners[2][1])])
luroth's avatar
.    
luroth committed
69
70
71
72
73
74
75
76
    
    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()
luroth's avatar
...    
luroth committed
77
    index_list = np.argpartition(plane_delta_dist,199)[0:200]
luroth's avatar
.    
luroth committed
78
79
80
81
82
83
    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
luroth's avatar
...    
luroth committed
84
85
86
            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)
luroth's avatar
.    
luroth committed
87
88
        return intersects
    else:
luroth's avatar
...    
luroth committed
89
        index_list = np.argsort(plane_delta_dist)[0:limit]
luroth's avatar
.    
luroth committed
90
        intersects = process_planes(index_list, plane_normales, plane_points, line_vector, line_point)
luroth's avatar
.    
luroth committed
91
92
93
        if intersects is not None:
                if verbose: 
                    t_found = time.time()- t_start
luroth's avatar
...    
luroth committed
94
95
96
                    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)
luroth's avatar
.    
luroth committed
97
98
                return intersects
        else:
luroth's avatar
.    
luroth committed
99
100
            if verbose: 
                t_found = time.time()- t_start
luroth's avatar
...    
luroth committed
101
102
103
                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)                
luroth's avatar
.    
luroth committed
104
105
            return [np.nan, np.nan, np.nan]
        
luroth's avatar
.    
luroth committed
106
def process_planes(index_list, plane_normales, plane_points, line_vector, line_point):
luroth's avatar
.    
luroth committed
107
108
109
110
111
112
113
114
115
116
    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()
luroth's avatar
.    
luroth committed
117
            hit = point_in_polygon2(intersect_point[0], intersect_point[1], [(p1_x, p1_y), (p2_x, p2_y), (p3_x, p3_y)] )
luroth's avatar
.    
luroth committed
118
119
120
            if hit: 
                return intersect_point.tolist()
            else:
luroth's avatar
.    
luroth committed
121
                pass
luroth's avatar
.    
luroth committed
122
123
    return None

luroth's avatar
.    
luroth committed
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
def main(argv):
    camera_position_file = ''
    DEM_stl_file = ''
    no_of_interpolation_points_x = 0
    no_of_interpolation_points_y = 0
    output_directory = ''
    try:
        opts, args = getopt.getopt(argv,"hc:d:x:y:o:")
    except getopt.GetoptError:
        print('test.py -c <camera position file> -d <DEM STL file> -x <no of interpolation points x> -y <no of interpolation points y> -o <Output directory>')
        sys.exit(2)
    for opt, arg in opts:
        if opt == '-h':
            print('test.py -c <camera position file> -d <DEM STL file> -x <no of interpolation points x> -y <no of interpolation points y> -o <Output directory>')
            sys.exit()
        elif opt == "-c":
            camera_position_file = arg
        elif opt == "-d":
            DEM_stl_file = arg
        elif opt == "-x":
            no_of_interpolation_points_x = int(arg)
        elif opt == "-y":
            no_of_interpolation_points_y = int(arg)      
        elif opt == "-o":
            output_directory = arg                   
    print('Camera position file is', camera_position_file)
    print('DEM STL file is', DEM_stl_file)
luroth's avatar
.    
luroth committed
151
    
luroth's avatar
.    
luroth committed
152
153
154
    # graph output off
    plt.ioff()
     
luroth's avatar
.    
luroth committed
155
156
157
    epsg = getWKT_PRJ("2056")

    # Load DEM    
luroth's avatar
luroth committed
158
    print("loading DEM")
luroth's avatar
.    
luroth committed
159
    DEM_stl = mesh.Mesh.from_file(DEM_stl_file)
luroth's avatar
.    
luroth committed
160
            
luroth's avatar
...    
luroth committed
161
162
163
    DEM_normales_raw = DEM_stl.normals
    DEM_points_raw = DEM_stl.points
    
luroth's avatar
.    
luroth committed
164
165
    DEM_normales = DEM_stl.normals
    DEM_points = DEM_stl.points
luroth's avatar
.    
luroth committed
166
    
luroth's avatar
...    
luroth committed
167
168
169
170
171
172
173
174
    # 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)
    
luroth's avatar
.    
luroth committed
175
176
177
178
179
180
181
182
183
    minx, maxx, miny, maxy, minz, maxz = find_mins_maxs(DEM_stl)
    
    meanz = (maxz + minz)/2
    
    # Calculating global bounding box
    corners_global = [( minx, miny, meanz), ( minx, maxy, meanz), ( maxx, maxy, meanz), ( maxx, miny, meanz)]
    print("Found:", corners_global)
      
    print("Initializing camera settings")
luroth's avatar
.    
luroth committed
184
185
186
187
188
189
    ## SONY-RX100II-VIS Specification
    sensor_dimension_x = 13.2 # mm
    sensor_dimension_y = 8.8 # mm
    focal_lengh = 10.4 # mm
    sensor_resolution_x = 5472 # pixel
    sensor_resolution_y = 3648 # pixel
luroth's avatar
.    
luroth committed
190
191
        
    # Axis for rotations
luroth's avatar
.    
luroth committed
192
193
194
195
    x_axis = [1, 0, 0]
    y_axis = [0, 1, 0]
    z_axis = [0, 0, 1]
    
luroth's avatar
.    
luroth committed
196
197
    # Coordinates of corners of sensor, relative to center
    sensor_Z = focal_lengh/1000
luroth's avatar
.    
luroth committed
198
        
luroth's avatar
.    
luroth committed
199
    # Coordinates of sample pixels of sensor (later used for interpolation), relative to center
luroth's avatar
.    
luroth committed
200
201
    sensor_pixels_x = np.ceil(np.linspace(0, sensor_resolution_x-1, no_of_interpolation_points_x, dtype='int'))
    sensor_pixels_y = np.ceil(np.linspace(0, sensor_resolution_y-1, no_of_interpolation_points_y, dtype='int'))
luroth's avatar
luroth committed
202
    sensor_pixels_xy = np.transpose([np.repeat(sensor_pixels_x, len(sensor_pixels_y)), np.tile(sensor_pixels_y, len(sensor_pixels_x))])
luroth's avatar
.    
luroth committed
203
    
luroth's avatar
luroth committed
204
    corner_index = [0, (no_of_interpolation_points_y-1), (no_of_interpolation_points_x*no_of_interpolation_points_y-1), (no_of_interpolation_points_x*no_of_interpolation_points_y-no_of_interpolation_points_y)]
luroth's avatar
.    
luroth committed
205

luroth's avatar
luroth committed
206
207
208
    sensor_pixels_coords_x = (((sensor_pixels_x / sensor_resolution_x) - 0.5 ) * sensor_dimension_x) / 1000
    sensor_pixels_coords_y = (((sensor_pixels_y / sensor_resolution_y) - 0.5 ) * sensor_dimension_y) / 1000
    sensor_pixels_coords_xy = np.transpose([np.repeat(sensor_pixels_coords_x, len(sensor_pixels_coords_y)), np.tile(sensor_pixels_coords_y, len(sensor_pixels_coords_x))])
luroth's avatar
.    
luroth committed
209
210
    sensor_pixel_coords_z = np.full((sensor_pixels_coords_xy.shape[0], 1), sensor_Z, dtype=np.float64)
    sensor_pixels_coords_xyz = np.concatenate((sensor_pixels_coords_xy,sensor_pixel_coords_z), axis=1)
luroth's avatar
.    
luroth committed
211

luroth's avatar
.    
luroth committed
212
    # Calculate coordinates on ground for all cameras
luroth's avatar
.    
luroth committed
213
    with open(camera_position_file, 'r') as csvfile:
luroth's avatar
.    
luroth committed
214
215
216
217
218
219
        spamreader = csv.reader(csvfile, delimiter='\t', quotechar='|')
        next(spamreader, None)
        next(spamreader, None)
        for row in spamreader:   
            
            PhotoID = os.path.splitext(row[0])[0]
luroth's avatar
.    
luroth committed
220
            print('\n===========================\nPROCESSING', PhotoID, '\n')
luroth's avatar
.    
luroth committed
221
222
223
224
225
            
            X, Y, Z = (float(row[index]) for index in [1,2,3])
            Omega, Phi, Kappa = (float(row[index]) for index in [4,5,6])            
            # in radian
            omega_rad, phi_rad, kappa_rad = (math.radians(x) for x in [Omega, Phi, Kappa])
luroth's avatar
.    
luroth committed
226
            
luroth's avatar
.    
luroth committed
227
            # Coordinates of focal point
luroth's avatar
.    
luroth committed
228
229
            focal_point = np.array([X, Y, Z])
            
luroth's avatar
.    
luroth committed
230
            print("rotate camera")
luroth's avatar
.    
luroth committed
231
232
233
234
            sensor_pixels_vectors = np.array([np.dot(rotation_matrix(z_axis, kappa_rad), sensor_corner) for sensor_corner in sensor_pixels_coords_xyz])
            sensor_pixels_vectors = np.array([np.dot(rotation_matrix(y_axis, phi_rad), sensor_corner) for sensor_corner in sensor_pixels_vectors])
            sensor_pixels_vectors = np.array([np.dot(rotation_matrix(x_axis, omega_rad), sensor_corner) for sensor_corner in sensor_pixels_vectors])
        
luroth's avatar
.    
luroth committed
235
236
            print("calculate ground points for corners")       
            sensor_corner_vectors = sensor_pixels_vectors[corner_index]                   
luroth's avatar
.    
luroth committed
237
            
luroth's avatar
...    
luroth committed
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
            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)]
luroth's avatar
.    
luroth committed
271
            
luroth's avatar
.    
luroth committed
272
273

            print("\ncorners found:\n", ground_corner_coords)
luroth's avatar
.    
luroth committed
274
            if(np.isnan(ground_corner_coords.flat).any()):
luroth's avatar
.    
luroth committed
275
276
                print("corners missing, have to calculate with global corners :(")
                ground_corner_coords = corners_global            
luroth's avatar
.    
luroth committed
277
278
279
280
                     
            # write shapefile:
            # add start point as end point to close polygon
            corner_polygon = np.append(ground_corner_coords, [ground_corner_coords[0]], axis=0)
luroth's avatar
.    
luroth committed
281
            print("\nsave corner polygon to shapefile")
luroth's avatar
.    
luroth committed
282
283
            shp = shapefile.Writer(shapeType=shapefile.POLYGON)
            shp.field('label') 
luroth's avatar
.    
luroth committed
284
285
            shp.poly(parts=[corner_polygon.tolist()], shapeType=shapefile.POLYGON)
            shp.record('field_of_view')
luroth's avatar
luroth committed
286
            shp.save(os.path.join(output_directory, PhotoID + '_corners'))  
luroth's avatar
.    
luroth committed
287
            
luroth's avatar
luroth committed
288
            prj = open(os.path.join(output_directory, PhotoID + '_corners.prj'), "w")
luroth's avatar
.    
luroth committed
289
290
291
292
            prj.write(epsg)
            prj.close()
            
            
luroth's avatar
.    
luroth committed
293
            print("\n------\ncalculate now ground points for all sample pixels:")  
luroth's avatar
.    
luroth committed
294
295
            print('Points to calculate:', sensor_pixels_vectors.shape[0])
            print('Planes to intersect with:', DEM_normales.shape[0])
luroth's avatar
.    
luroth committed
296
            print('calculate intersections, running on', multiprocessing.cpu_count()-2, 'cores\n')
luroth's avatar
...    
luroth committed
297
298
299
300
301
            
            m = Manager()
            jobs = m.Queue()
            results = m.Queue()
            processes = []
luroth's avatar
.    
luroth committed
302
            
luroth's avatar
...    
luroth committed
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
            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)]


luroth's avatar
.    
luroth committed
334

luroth's avatar
.    
luroth committed
335
            print("\ncalculating view angles")
luroth's avatar
luroth committed
336
            ground_pixel_angle_xy = np.arctan(((Y-ground_pixel_coords[:,1]) / (X-ground_pixel_coords[:,0])))
luroth's avatar
.    
luroth committed
337
338
339
            ground_pixel_angle_z =  np.arctan( (Z-ground_pixel_coords[:,2]) / np.sqrt( (X-ground_pixel_coords[:,0])*(X-ground_pixel_coords[:,0]) + (Y-ground_pixel_coords[:,1])*(Y-ground_pixel_coords[:,1]) ) )

            # interpolating 
luroth's avatar
.    
luroth committed
340
            print("\ninterpolating")
luroth's avatar
luroth committed
341
342
343
344
345
346
            grid_x, grid_y = np.mgrid[0:sensor_resolution_x, 0:sensor_resolution_y]
            

            sensor_pixels_xy_sel = sensor_pixels_xy[np.logical_not(np.isnan(ground_pixel_coords[:,0]))]            
            ground_pixel_coords_x = ground_pixel_coords[np.logical_not(np.isnan(ground_pixel_coords[:,0])),0]
            ground_pixel_coords_x_interpol = griddata(sensor_pixels_xy_sel, ground_pixel_coords_x, (grid_x, grid_y), method='linear')
luroth's avatar
.    
luroth committed
347
            
luroth's avatar
luroth committed
348
349
350
            sensor_pixels_xy_sel = sensor_pixels_xy[np.logical_not(np.isnan(ground_pixel_coords[:,1]))]
            ground_pixel_coords_y = ground_pixel_coords[np.logical_not(np.isnan(ground_pixel_coords[:,1])),1]
            ground_pixel_coords_y_interpol = griddata(sensor_pixels_xy_sel, ground_pixel_coords_y, (grid_x, grid_y), method='linear')
luroth's avatar
.    
luroth committed
351
            
luroth's avatar
luroth committed
352
353
354
            sensor_pixels_xy_sel = sensor_pixels_xy[np.logical_not(np.isnan(ground_pixel_coords[:,2]))]
            ground_pixel_coords_z = ground_pixel_coords[np.logical_not(np.isnan(ground_pixel_coords[:,2])),2]
            ground_pixel_coords_z_interpol = griddata(sensor_pixels_xy_sel, ground_pixel_coords_z, (grid_x, grid_y), method='linear')
luroth's avatar
.    
luroth committed
355
            
luroth's avatar
luroth committed
356
357
358
            sensor_pixels_xy_sel = sensor_pixels_xy[np.logical_not(np.isnan(ground_pixel_angle_xy))]
            ground_pixel_angle_xy = ground_pixel_angle_xy[np.logical_not(np.isnan(ground_pixel_angle_xy))]
            ground_pixel_angle_xy_interpol = griddata(sensor_pixels_xy_sel, ground_pixel_angle_xy, (grid_x, grid_y), method='linear')
luroth's avatar
.    
luroth committed
359
            
luroth's avatar
luroth committed
360
361
362
            sensor_pixels_xy_sel = sensor_pixels_xy[np.logical_not(np.isnan(ground_pixel_angle_z))]
            ground_pixel_angle_z = ground_pixel_angle_z[np.logical_not(np.isnan(ground_pixel_angle_z))]
            ground_pixel_angle_z_interpol = griddata(sensor_pixels_xy_sel, ground_pixel_angle_z, (grid_x, grid_y), method='linear')
luroth's avatar
.    
luroth committed
363
            
luroth's avatar
.    
luroth committed
364
            print("\ncreate overview file")
luroth's avatar
.    
luroth committed
365
366
367
            plt.figure(figsize=(10,7))
            
            plt.subplot(231)
luroth's avatar
luroth committed
368
            plt.imshow(np.fliplr(ground_pixel_coords_x_interpol.T))
luroth's avatar
.    
luroth committed
369
            plt.title('X coord')
luroth's avatar
luroth committed
370
            plt.colorbar(orientation='horizontal')
luroth's avatar
.    
luroth committed
371
372
            
            plt.subplot(232)
luroth's avatar
luroth committed
373
            plt.imshow(np.fliplr(ground_pixel_coords_y_interpol.T))
luroth's avatar
.    
luroth committed
374
            plt.title('Y coord')
luroth's avatar
luroth committed
375
            plt.colorbar(orientation='horizontal')
luroth's avatar
.    
luroth committed
376
377
            
            plt.subplot(233)
luroth's avatar
luroth committed
378
            plt.imshow(np.fliplr(ground_pixel_coords_z_interpol.T))
luroth's avatar
.    
luroth committed
379
            plt.title('Z coord')
luroth's avatar
luroth committed
380
            plt.colorbar(orientation='horizontal')
luroth's avatar
.    
luroth committed
381
382
            
            plt.subplot(234)
luroth's avatar
luroth committed
383
            plt.imshow(np.fliplr(ground_pixel_angle_xy_interpol.T))
luroth's avatar
.    
luroth committed
384
            plt.title('xy view angle')
luroth's avatar
luroth committed
385
            plt.colorbar(orientation='horizontal')
luroth's avatar
.    
luroth committed
386
387
            
            plt.subplot(235)
luroth's avatar
luroth committed
388
            plt.imshow(np.fliplr(ground_pixel_angle_z_interpol.T))
luroth's avatar
.    
luroth committed
389
            plt.title('z view angle')
luroth's avatar
luroth committed
390
            plt.colorbar(orientation='horizontal')
luroth's avatar
.    
luroth committed
391

luroth's avatar
luroth committed
392
393
            plt.savefig(os.path.join(output_directory, PhotoID + '_overview.png'), dpi = 80)
            plt.close()
luroth's avatar
.    
luroth committed
394
395
396
            
            print("\nwrite pixel matrix to TIFF")

luroth's avatar
.    
luroth committed
397
398
            gdalnumeric.SaveArray(np.array(np.fliplr(ground_pixel_coords_x_interpol.T)-DELTA_x, dtype=np.float32), os.path.join(output_directory, PhotoID + '_metadata_coord_x.tif'), format='GTiff',  )
            gdalnumeric.SaveArray(np.array(np.fliplr(ground_pixel_coords_y_interpol.T)-DELTA_y, dtype=np.float32), os.path.join(output_directory, PhotoID + '_metadata_coord_y.tif'), format='GTiff' )
luroth's avatar
.    
luroth committed
399
400
401
            gdalnumeric.SaveArray(np.array(np.fliplr(ground_pixel_coords_z_interpol.T), dtype=np.float32), os.path.join(output_directory, PhotoID + '_metadata_coord_z.tif'), format='GTiff' )
            gdalnumeric.SaveArray(np.array(np.fliplr(ground_pixel_angle_xy_interpol.T), dtype=np.float32), os.path.join(output_directory, PhotoID + '_metadata_angle_xy.tif'), format='GTiff' )
            gdalnumeric.SaveArray(np.array(np.fliplr(ground_pixel_angle_z_interpol.T), dtype=np.float32), os.path.join(output_directory, PhotoID + '_metadata_angle_z.tif'), format='GTiff' )
luroth's avatar
.    
luroth committed
402
403
            
            print("\nDONE\n")
luroth's avatar
.    
luroth committed
404
405
    
if __name__ == "__main__":
luroth's avatar
.    
luroth committed
406
    main(sys.argv[1:])
luroth's avatar
.    
luroth committed
407
    
luroth's avatar
.    
luroth committed
408
    print("\n============\nAll photos processed, END\n")