LinePlaneIntersection.py

#######################################
import math as math
import numpy as np
import geojson
from stl import mesh
import stl.base as stl
from joblib import Parallel, delayed
import multiprocessing
from multiprocessing import Process, Queue, Manager
from itertools import repeat
import csv
import os
import sys
import getopt
import warnings
import matplotlib.pyplot as plt
import shapefile
from scipy.interpolate import griddata
import gdal
import gdalnumeric
import time
import ctypes

from Common import *


def ray_trace(line_vector, line_point, plane_point, plane_normal):
    epsilon=1e-64
    ndotu = plane_normal.dot(line_vector) 
    
    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

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']
        
        intersects = intersect_line_with_planes(pixel_nr, plane_normales, plane_points, line_vector, line_point, corners, verbose, limit)
        result_queue.put([pixel_nr, intersects])


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()
    
    # 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_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])])
    
    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,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:
        if verbose: 
            t_found = time.time()- t_start
            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:limit]
        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("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:
            if verbose: 
                t_found = time.time()- t_start
                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):
    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_polygon2(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 = ''
    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)
    
    # graph output off
    plt.ioff()
     
    epsg = getWKT_PRJ("2056")

    # Load DEM    
    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
    
    # 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")
    ## 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
        
    # Axis for rotations
    x_axis = [1, 0, 0]
    y_axis = [0, 1, 0]
    z_axis = [0, 0, 1]
    
    # Coordinates of corners of sensor, relative to center
    sensor_Z = focal_lengh/1000
        
    # Coordinates of sample pixels of sensor (later used for interpolation), relative to center
    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'))
    sensor_pixels_xy = np.transpose([np.repeat(sensor_pixels_x, len(sensor_pixels_y)), np.tile(sensor_pixels_y, len(sensor_pixels_x))])
    
    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)]

    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))])
    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)

    # Calculate coordinates on ground for all cameras
    with open(camera_position_file, 'r') as csvfile:
        spamreader = csv.reader(csvfile, delimiter='\t', quotechar='|')
        next(spamreader, None)
        next(spamreader, None)
        for row in spamreader:   
            
            PhotoID = os.path.splitext(row[0])[0]
            print('\n===========================\nPROCESSING', PhotoID, '\n')
            
            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])
            
            # Coordinates of focal point
            focal_point = np.array([X, Y, Z])
            
            print("rotate camera")
            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])
        
            print("calculate ground points for corners")       
            sensor_corner_vectors = sensor_pixels_vectors[corner_index]                   
            
            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)
            if(np.isnan(ground_corner_coords.flat).any()):
                print("corners missing, have to calculate with global corners :(")
                ground_corner_coords = corners_global            
                     
            # write shapefile:
            # add start point as end point to close polygon
            corner_polygon = np.append(ground_corner_coords, [ground_corner_coords[0]], axis=0)
            print("\nsave corner polygon to shapefile")
            shp = shapefile.Writer(shapeType=shapefile.POLYGON)
            shp.field('label') 
            shp.poly(parts=[corner_polygon.tolist()], shapeType=shapefile.POLYGON)
            shp.record('field_of_view')
            shp.save(os.path.join(output_directory, PhotoID + '_corners'))  
            
            prj = open(os.path.join(output_directory, PhotoID + '_corners.prj'), "w")
            prj.write(epsg)
            prj.close()
            
            
            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()-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])))
            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 
            print("\ninterpolating")
            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')
            
            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')
            
            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')
            
            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')
            
            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')
            
            print("\ncreate overview file")
            plt.figure(figsize=(10,7))
            
            plt.subplot(231)
            plt.imshow(np.fliplr(ground_pixel_coords_x_interpol.T))
            plt.title('X coord')
            plt.colorbar(orientation='horizontal')
            
            plt.subplot(232)
            plt.imshow(np.fliplr(ground_pixel_coords_y_interpol.T))
            plt.title('Y coord')
            plt.colorbar(orientation='horizontal')
            
            plt.subplot(233)
            plt.imshow(np.fliplr(ground_pixel_coords_z_interpol.T))
            plt.title('Z coord')
            plt.colorbar(orientation='horizontal')
            
            plt.subplot(234)
            plt.imshow(np.fliplr(ground_pixel_angle_xy_interpol.T))
            plt.title('xy view angle')
            plt.colorbar(orientation='horizontal')
            
            plt.subplot(235)
            plt.imshow(np.fliplr(ground_pixel_angle_z_interpol.T))
            plt.title('z view angle')
            plt.colorbar(orientation='horizontal')

            plt.savefig(os.path.join(output_directory, PhotoID + '_overview.png'), dpi = 80)
            plt.close()
            
            print("\nwrite pixel matrix to TIFF")

            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' )
            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' )
            
            print("\nDONE\n")
    
if __name__ == "__main__":
    main(sys.argv[1:])
    
    print("\n============\nAll photos processed, END\n")