-
Notifications
You must be signed in to change notification settings - Fork 1
/
tsdf.py
46 lines (38 loc) · 1.54 KB
/
tsdf.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
import numpy as np
class TSDF:
def __init__(self, size=10.0):
self.resolution = 0.05
self.size = size
self.truncation_distance = 0.5
#we assume a grid with the origin at (0,0) only defined in positive direction
n_cells_per_dimension = int(self.size/self.resolution + 1)
self.tsdf = np.zeros((n_cells_per_dimension, n_cells_per_dimension))
self.weights = np.zeros((n_cells_per_dimension, n_cells_per_dimension))
def getCellIndexAtPosition(self, position):
cell_index = (np.round(np.array(position)/self.resolution)).astype(int)
return [[cell_index[1]],[cell_index[0]]]
def getPositionAtCellIndex(self, cell_index):
return np.array((cell_index[1][0], cell_index[0][0])) * self.resolution
def setTSDF(self, position_index, value):
try:
self.tsdf[position_index] = value
except:
pass
def setWeight(self, position_index, value):
try:
self.weights[position_index] = value
except:
pass
def getTSDF(self, position_index):
try:
return self.tsdf[position_index]
except:
return math.nan
def getWeight(self, position_index):
try:
return self.weights[position_index]
except:
return math.nan
def resetWeights(self):
n_cells_per_dimension = int(self.size/self.resolution + 1)
self.weights = np.zeros((n_cells_per_dimension, n_cells_per_dimension))