-
Notifications
You must be signed in to change notification settings - Fork 15
/
depth_map.py
35 lines (29 loc) · 1.18 KB
/
depth_map.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
import numpy as np
# From Github https://github.com/balcilar/DenseDepthMap
def dense_map(Pts, n, m, grid):
ng = 2 * grid + 1
mX = np.zeros((m,n)) + np.float("inf")
mY = np.zeros((m,n)) + np.float("inf")
mD = np.zeros((m,n))
mX[np.int32(Pts[1]),np.int32(Pts[0])] = Pts[0] - np.round(Pts[0])
mY[np.int32(Pts[1]),np.int32(Pts[0])] = Pts[1] - np.round(Pts[1])
mD[np.int32(Pts[1]),np.int32(Pts[0])] = Pts[2]
KmX = np.zeros((ng, ng, m - ng, n - ng))
KmY = np.zeros((ng, ng, m - ng, n - ng))
KmD = np.zeros((ng, ng, m - ng, n - ng))
for i in range(ng):
for j in range(ng):
KmX[i,j] = mX[i : (m - ng + i), j : (n - ng + j)] - grid - 1 +i
KmY[i,j] = mY[i : (m - ng + i), j : (n - ng + j)] - grid - 1 +i
KmD[i,j] = mD[i : (m - ng + i), j : (n - ng + j)]
S = np.zeros_like(KmD[0,0])
Y = np.zeros_like(KmD[0,0])
for i in range(ng):
for j in range(ng):
s = 1/np.sqrt(KmX[i,j] * KmX[i,j] + KmY[i,j] * KmY[i,j])
Y = Y + s * KmD[i,j]
S = S + s
S[S == 0] = 1
out = np.zeros((m,n))
out[grid + 1 : -grid, grid + 1 : -grid] = Y/S
return out