idun/test.py at master · putertubby/idun · GitHub

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
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
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
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
#TODO: This file is obsolete, remove when convenient!
import numpy as np

#def appendSpherical_np(xyz):
#    ptsnew = np.hstack((xyz, np.zeros(xyz.shape)))
#    xy = xyz[:,0]**2 + xyz[:,1]**2
#    ptsnew[:,3] = np.sqrt(xy + xyz[:,2]**2)
#    ptsnew[:,4] = np.arctan2(np.sqrt(xy), xyz[:,2]) # for elevation angle defined from Z-axis down
#    #ptsnew[:,4] = np.arctan2(xyz[:,2], np.sqrt(xy)) # for elevation angle defined from XY-plane up
#    ptsnew[:,5] = np.arctan2(xyz[:,1], xyz[:,0])
#    return ptsnew

# bb = point + delta => delta = bb - point


# TODO: Check if division by zero is a problem in reality (YES IT IS!). Also polar(0, theta, phi) should => (0, 0, -180)
# Use this version instead to avoid division (might lead to division by zero)
def polar2(x, y, z):
    xy = x**2 + y**2
    r = np.sqrt(xy + z**2)
    theta = np.arctan2(np.sqrt(xy), z) # for elevation angle defined from Z-axis down
    #theta = np.arctan2(z, np.sqrt(xy)) # for elevation angle defined from XY-plane up
    phi = np.arctan2(y, x)
    return r, theta, phi

# Official version
def polar(x, y, z):
    r = np.sqrt(x * x + y * y + z * z)
    theta = np.arccos(z / r) # for elevation angle defined from Z-axis down
    phi = np.arctan2(y, x)
    return r, theta, phi

# Strategy:
# =========
# 1) classify and calculate bb properties for each return using CNN
# 2) find bb properties for each classified return (inverse normalization)
# 3) 'cluster' bb properties [2D should be sufficient since cars shouldn't be on top of each other], discard clusters with 'few' returns (need to find threshold!)
# 4) for each cluster average the bb properties (consider classification and only factor in properly classified returns!)
# 5) for each cluster classify all returns in the bb to be the same as majority classification

# Official version
#
#>>> x[...,0:3].shape
#(8, 64, 512, 3)
#
#>>> x[...,0:3][0,0,0:2,...]
#array([[0.15986861, 0.01165385, 0.00056379],
#       [0.27936447, 0.02350379, 0.00299972]], dtype=float32)
#
#>>> x[...,0:3][0,0,0:2,:]
#array([[0.15986861, 0.01165385, 0.00056379],
#       [0.27936447, 0.02350379, 0.00299972]], dtype=float32)
#
#>>> cartesian(y[...,0:3][0,0,0:2,:], [20, 3.1415926536, 6.2831853072], [0, 0, -3.1415926536])
#>>> cartesian(y[...,0:3][0,0,0:10,:], [20, 3.1415926536, 6.2831853072], [0, 0, -3.1415926536])
def cartesian(arg, scale, offset):
    arg = np.asarray(arg)
    res = np.empty(arg.shape, arg.dtype)
    r = arg[...,0] * scale[0] + offset[0]
    theta = arg[...,1] * scale[1] + offset[1]
    phi = arg[...,2] * scale[2] + offset[2]
    res[...,0] = r * np.sin( theta ) * np.cos( phi )
    res[...,1] = r * np.sin( theta ) * np.sin( phi )
    res[...,2] = r * np.cos( theta )
    return res

# pip install scikit-image
from skimage.draw import disk
#from skimage.draw import circle_perimeter
from skimage.draw import ellipse_perimeter
from skimage.draw import line

#>>> img = np.zeros((10, 10), dtype=np.uint8)
#>>> rr, cc = disk((4, 4), 5)
#>>> img[rr, cc] = 1
#>>> img
#array([[0, 0, 1, 1, 1, 1, 1, 0, 0, 0],
#       [0, 1, 1, 1, 1, 1, 1, 1, 0, 0],
#       [1, 1, 1, 1, 1, 1, 1, 1, 1, 0],
#       [1, 1, 1, 1, 1, 1, 1, 1, 1, 0],
#       [1, 1, 1, 1, 1, 1, 1, 1, 1, 0],
#       [1, 1, 1, 1, 1, 1, 1, 1, 1, 0],
#       [1, 1, 1, 1, 1, 1, 1, 1, 1, 0],
#       [0, 1, 1, 1, 1, 1, 1, 1, 0, 0],
#       [0, 0, 1, 1, 1, 1, 1, 0, 0, 0],
#       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype=uint8)
# makegrid(bborigins[...,0], bborigins[...,1], 0, 100, -50, 50)
def makegrid(xcoords, ycoords, xmin, xmax, ymin, ymax, shape=[256, 256], radius=5):
    xwidth = xmax - xmin
    xind = np.floor(((xcoords - xmin) / xwidth) * shape[0]).astype(np.int)

    ywidth = ymax - ymin
    yind = np.floor(((ycoords - ymin) / ywidth) * shape[1]).astype(np.int)

    grid = np.zeros(shape, dtype = np.int)
    # disk expects (i, j) coordinates not (x, y)!
    for ind in zip(xind, yind):
        rr, cc = disk(ind, radius, shape=shape)
        grid[rr, cc] += 1
    return grid

# rr, cc = makefovoutline(100, -0.7853981634, 0.7853981634, 0, 106, -71, 71, shape)
def makefovoutline(rmax, hmin, hmax, xmin, xmax, ymin, ymax, shape):
    xwidth = xmax - xmin
    x0 = np.floor(((0 - xmin) / xwidth) * shape[0]).astype(np.int)
    x1 = np.floor(((rmax * np.cos(hmin) - xmin) / xwidth) * shape[0]).astype(np.int)
    x2 = np.floor(((rmax * np.cos(hmax) - xmin) / xwidth) * shape[0]).astype(np.int)
    xr = np.floor((rmax / xwidth) * shape[0]).astype(np.int)
    #print(xwidth, x0, x1, x2, xr)

    ywidth = ymax - ymin
    y0 = np.floor(((0 - ymin) / ywidth) * shape[1]).astype(np.int)
    y1 = np.floor(((rmax * np.sin(hmin) - ymin) / ywidth) * shape[1]).astype(np.int)
    y2 = np.floor(((rmax * np.sin(hmax) - ymin) / ywidth) * shape[1]).astype(np.int)
    yr = np.floor((rmax / ywidth) * shape[1]).astype(np.int)
    #print(ywidth, y0, y1, y2, yr)

    rr1, cc1 = ellipse_perimeter(x0, y0, xr, yr, orientation=0, shape=shape)
    rr2, cc2 = line(x0, y0, x1, y1)
    rr3, cc3 = line(x0, y0, x2, y2)

    rr = np.concatenate((rr1, rr2, rr3))
    cc = np.concatenate((cc1, cc2, cc3))
    return rr, cc

# plt.contourf(bins)
# plt.show()

# import matplotlib.pyplot as plt
# x, y = next(generate_segmentation_batches('dataset', 8, 8))
# heatmap(x[0], y['instance_output'][0])
# https://stackoverflow.com/questions/7821518/matplotlib-save-plot-to-numpy-array
# https://stackoverflow.com/a/31671355/2912670 (draw line)

from PIL import Image
#>>> hm = heatmap(x[3], y['instance_output'][3])
#>>> img = Image.fromarray(hm, 'RGB')
#>>> img.show()
def heatmap(x, y, shape=[384, 512], rgb=[80/255, 168/255, 227/255]):
    # x scale=[rmax=100m, vfov=27deg, hfov=90deg] offset=[rmin=0m, vmin=87deg, hmin=-45deg]
    points = cartesian(x[...,0:3], [100, 0.47123889804, 1.5707963268], [0, 1.5184364492, -0.7853981634])

    # y scale=[] offset=[]
    bbcandidates = points + cartesian(y[...,1:4], [20, 3.1415926536, 6.2831853072], [0, 0, -3.1415926536])

    # keep only returns belonging to a known class (the only ones we have trained bounding boxes for)
    bborigins = bbcandidates[y[...,0] > 0]

    # create grid with range between 0 and 100 meters and horizontal fov -45x45 degrees (-71m to 71 m @ 100 m)
    grid = makegrid(bborigins[...,0], bborigins[...,1], 0, 106, -71, 71, shape, 5)

    # create a heatmap suitable for display
    heatmap = (np.multiply(grid[..., np.newaxis], rgb) * 255 / np.max(grid)).astype(np.uint8)

    # add field-of-view outline
    rr, cc = makefovoutline(100, -0.7853981634, 0.7853981634, 0, 106, -71, 71, shape)
    heatmap[rr, cc] = [128, 128, 128];

    return np.flipud(np.fliplr(heatmap))

# from SegmenterUtils import *
# from test import *
# x, y = next(generate_segmentation_batches('dataset', 8, 8))
# hm=heatmap(x[0], y['instance_output'][0])
# img=Image.fromarray(hm, 'RGB')
# img.show()

# bb inference:
#
# 1) find max cell in heatmap
# 2) if cell value < min_cell_value_threshold
# 2.1  exit
# 3) set cellset = max cell
# 4) calculate mean of (x, y, l, w, h, rz) from cells in cellset to produce a temporary bb
# 5) if new cells contained in temporary bb
# 5.1  add new cells to cellset and jump to #4
# 6) temporary bb => bb[n]
# 7) if number of loops < max_loops_threshold
# 7.1  jump to #1
# 8) exit