-
Notifications
You must be signed in to change notification settings - Fork 3
/
Copy pathmain.py
88 lines (71 loc) · 3.02 KB
/
main.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
# -*- coding: utf-8 -*-
"""
Created on Fri Jun 30 08:38:19 2023
@author: anton
"""
import numpy as np
import rayCasting
import dataPrep
import manager3DCoords
import matplotlib.pyplot as plt
# ----------------------------------------------------------------------------
# CONFIG
#%% settings that are obligatory for usage!
method = ['real images','feature images','sobel','canny','mask','mask and sobel','mask and canny']
image_type = ['real images','segmentation']
#%% settings to be changed by the user
GNSS = np.loadtxt('./data/GNSS/9_Route3.txt')
chosenMethod = method[0]
chosenImageType = image_type[0]
curr_model = 'LoD3' #or 'LoD2'
folder_mask = './data/Images/Route_3_seg_buildings'
if chosenImageType == 'segmentation':
ImageFolder = './data/Images/Route_3_seg'
elif chosenImageType == 'real images':
ImageFolder = './data/Images/Route_3'
else:
print('Please select the image type to get the images from the right folder.')
# data preparation, settings of the camera for the real images
pixel_M = 3.4500e-6
c_9 = 0.0082198514/pixel_M
c_10 = 0.0081814370/pixel_M
width = 2464
height = 2056
dx_9 = 2.3104
dy_9 = -0.7484
dz_9 = 0.1080
roll = -101.2307
pitch = -0.0849
yaw = 85.3330
camera = [c_9,width,height,dx_9,dy_9,dz_9,roll,pitch,yaw,pixel_M]
GNSS = dataPrep.main(GNSS, camera)
# ----------------------------------------------------------------------------
# START of the Actual Program
#%% Ray Casting and Coordinate Calculation
points_traj = np.array([0,0,0])
points_traj = points_traj[:,np.newaxis]
i = 1
for img in range(1,3):
# step 1: create images of the LoD models
raycast_results,mesh,path = rayCasting.main(camera,curr_model,ImageFolder,GNSS,img)
# step 2: get the coordinates and calculate the camera position
points_traj, std = manager3DCoords.main(camera,GNSS,mesh,ImageFolder,path,folder_mask,raycast_results,points_traj,chosenMethod,img)
print("Point " + str(img))
print('Standard deviation of the current point: ' + str(std[0:3]))
traj_test = points_traj.T
GNSS_m = np.sqrt(np.power((GNSS[i,0]-traj_test[i,0]),2) + np.power((GNSS[i,1]-traj_test[i,1]),2) + np.power((GNSS[i,2]-traj_test[i,2]),2))
print('current deviation from GNSS: ' + str(GNSS_m) + 'm')
i = i + 1
#%% Plotting of the Resulting Trajectory
points_traj = points_traj.T
points_traj = points_traj[1:,:]
fig = plt.figure("Trajectory")
ax = fig.add_subplot(projection='3d')
ax.scatter(GNSS[:,0],GNSS[:,1],GNSS[:,2])
ax.scatter(GNSS[img,0],GNSS[img,1],GNSS[img,2],c='g',marker='o')
ax.plot(GNSS[:,0],GNSS[:,1],GNSS[:,2])
ax.scatter(points_traj[:,0],points_traj[:,1],points_traj[:,2],c='r', marker='o')
ax.plot(points_traj[:,0],points_traj[:,1],points_traj[:,2],c='r')
GNSS_m = np.sqrt(np.power((GNSS[img:img+len(points_traj),0]-points_traj[:,0]),2) + np.power((GNSS[img:img+len(points_traj),1]-points_traj[:,1]),2) + np.power((GNSS[img:img+len(points_traj),2]-points_traj[:,2]),2))
print('current max deviation: ' + str(round(np.amax(GNSS_m),4)) + 'm')
print('current min deviation: ' + str(round(np.amin(GNSS_m),4)) + 'm')