-
Notifications
You must be signed in to change notification settings - Fork 19
/
testGtrack.py
201 lines (184 loc) · 12.3 KB
/
testGtrack.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
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
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
import cppyy
from cppyy import gbl
from cppyy import ll
import numpy as np
from glob import glob
def gtrack_cppyy_init(gtrackRootPath="gtrack",use3D=True):
# set include path
cppyy.add_include_path(gtrackRootPath)
cppyy.add_include_path(gtrackRootPath+"/include/")
# set 2D/3D macro
if use3D:
cppyy.cppdef("#define GTRACK_3D")
print("GTRACK_3D")
else:
cppyy.cppdef("#define GTRACK_2D")
print("GTRACK_2D")
# set debug log macro
cppyy.cppdef("#define GTRACK_LOG_ENABLED")
print("loading C files")
# load header file
cppyy.include(gtrackRootPath+"/gtrack.h")
# load c files
for cFile in sorted(glob(gtrackRootPath+"/src/*.c")):
# print(cFile)
cppyy.c_include(cFile)
print("C files loaded")
def gtrack_cppyy_create(use3D=True):
# instantiation
config=gbl.GTRACK_moduleConfig()
advParams=gbl.GTRACK_advancedParameters()
# Mandatory Configuration Parameters
config.maxNumPoints = 500
config.maxNumTracks = 50
if use3D:
config.stateVectorType = gbl.GTRACK_STATE_VECTORS_3DA # Track three dimensions with acceleration
else:
config.stateVectorType = gbl.GTRACK_STATE_VECTORS_2DA # Track two dimensions with acceleration
config.initialRadialVelocity = 0 # Expected target radial velocity at the moment of detection, m/s
config.maxAcceleration=np.array([# Maximum targets acceleration in
1, # lateral direction
1, # longitudinal direction
1 # vertical direction. For 2D options, the vertical component is ignored
],dtype=np.float32)
config.verbose = gbl.GTRACK_VERBOSE_MAXIMUM
# config.verbose = gbl.GTRACK_VERBOSE_DEBUG
# config.verbose = gbl.GTRACK_VERBOSE_NONE
## This shall match sensor chirp configuration
config.deltaT = 0.05 # 50ms per frame
config.maxRadialVelocity = 5.29 # Radial velocity from sensor is limited to +/- maxURV (in m/s)
config.radialVelocityResolution = 0.083 # Radial velocity resolution (in m/s)
# Advanced parameters
## Scenery Parameters
sceneryParams = gbl.GTRACK_sceneryParams()
sceneryParams.sensorPosition=gbl.GTRACK_sensorPosition(0,0,0) # sensor position, (X,Y,Z), is in cartesian space relative to the [3-dimentional] world.
sceneryParams.sensorOrientation=gbl.GTRACK_sensorOrientation(0,0)# sensor orientation, boresight (azumuthal,elevation) tilt, negative left/up, positive right/down, in degrees
sceneryParams.numBoundaryBoxes=1 # Number of scene boundary boxes. If defined (numBoundaryBoxes > 0), only points within the boundary box(s) can be associated with tracks
sceneryParams.boundaryBox[0].x1=-4 # Left boundary, m
sceneryParams.boundaryBox[0].x2=4 # Right boundary, m
sceneryParams.boundaryBox[0].y1=0.5# Near boundary, m
sceneryParams.boundaryBox[0].y2=7.5# Far boundary, m
sceneryParams.boundaryBox[0].z1=0 # Bottom boundary, m
sceneryParams.boundaryBox[0].z2=3 # Top boundary, m
sceneryParams.boundaryBox[1].x1=0 # Left boundary, m
sceneryParams.boundaryBox[1].x2=0 # Right boundary, m
sceneryParams.boundaryBox[1].y1=0 # Near boundary, m
sceneryParams.boundaryBox[1].y2=0 # Far boundary, m
sceneryParams.boundaryBox[1].z1=0 # Bottom boundary, m
sceneryParams.boundaryBox[1].z2=0 # Top boundary, m
sceneryParams.numStaticBoxes=1 # Number of scene static boxes. If defined (numStaticBoxes > 0), only targets within the static box(s) can persist as static
sceneryParams.staticBox[0].x1=-3 # Left boundary, m
sceneryParams.staticBox[0].x2=3 # Right boundary, m
sceneryParams.staticBox[0].y1=2 # Near boundary, m
sceneryParams.staticBox[0].y2=6 # Far boundary, m
sceneryParams.staticBox[0].z1=0.5# Bottom boundary, m
sceneryParams.staticBox[0].z2=2.5# Top boundary, m
sceneryParams.staticBox[1].x1=0 # Left boundary, m
sceneryParams.staticBox[1].x2=0 # Right boundary, m
sceneryParams.staticBox[1].y1=0 # Near boundary, m
sceneryParams.staticBox[1].y2=0 # Far boundary, m
sceneryParams.staticBox[1].z1=0 # Bottom boundary, m
sceneryParams.staticBox[1].z2=0 # Top boundary, m
## Allocation Parameters
allocationParams = gbl.GTRACK_allocationParams(60, 200, 0.1, 5, 1, 2)#60 in clear, 200 obscured SNRs, 0.1m/s minimal velocity, 5 points, 1.5m in distance, 2m/s in velocity
## State Transition Parameters
stateParams = gbl.GTRACK_stateParams(10, 5, 50, 100, 5)#det2act, det2free, act2free, stat2free, exit2free
## Gating Parameters
gatingParams = gbl.GTRACK_gatingParams()
gatingParams.gain=3 # Gating constant gain 3x
gatingParams.limitsArray=np.array([1.5,1.5,2,0],dtype=np.float32) # Limits are set to 1.5m in depth, width, 2m (if applicable) in height and no limits in doppler
## Presence Parameters
presenceParams = gbl.GTRACK_presenceParams()
presenceParams.pointsThre=0 # occupancy threshold, number of points. Setting pointsThre to 0 disables presence detection
# load params into config
advParams.allocationParams = allocationParams
advParams.gatingParams = gatingParams
advParams.stateParams = stateParams
advParams.sceneryParams = sceneryParams
advParams.presenceParams = presenceParams
config.advParams = advParams
# init point and target array
cppyy.cppdef(r"""
GTRACK_measurementPoint pointCloud[GTRACK_NUM_POINTS_MAX];
GTRACK_targetDesc targetDescr[GTRACK_NUM_TRACKS_MAX];
""")
print("test gtrack_create")
errno=np.array(0,dtype=np.int32)
hTrackModule = gbl.gtrack_create(config,errno)
if errno!=0:
print("[gtrack_create]errno:",errno)
return cppyy.nullptr
hTrackModule = cppyy.bind_object(hTrackModule, 'GtrackModuleInstance')
print("hTrackModule:",hTrackModule)
return hTrackModule
def gtrack_cppyy_step(hTrackModule,use3D=True):
pointCloud=gbl.pointCloud# Pointer to an array of input measurments. Each measurement has range/angle/radial velocity information
if use3D:
dim=4
else:
dim=3
pointCloud[0].array=np.array( [3.83,-0.12,0.28,-0.55],dtype=np.float32)[:dim]; pointCloud[0].snr=15.32
pointCloud[1].array=np.array( [3.83,-0.10,0.28,-0.55],dtype=np.float32)[:dim]; pointCloud[1].snr=15.92
pointCloud[2].array=np.array( [3.78,-0.09,0.23,-0.99],dtype=np.float32)[:dim]; pointCloud[2].snr=15.21
pointCloud[3].array=np.array( [3.83,-0.09,0.28,-0.55],dtype=np.float32)[:dim]; pointCloud[3].snr=16.55
pointCloud[4].array=np.array( [3.78,-0.07,0.23,-0.99],dtype=np.float32)[:dim]; pointCloud[4].snr=15.85
pointCloud[5].array=np.array( [3.83,-0.07,0.28,-0.55],dtype=np.float32)[:dim]; pointCloud[5].snr=17.10
pointCloud[6].array=np.array( [3.78,-0.05,0.23,-0.99],dtype=np.float32)[:dim]; pointCloud[6].snr=16.49
pointCloud[7].array=np.array( [3.83,-0.05,0.28,-0.55],dtype=np.float32)[:dim]; pointCloud[7].snr=17.44
pointCloud[8].array=np.array( [3.88,-0.05,0.33,-0.55],dtype=np.float32)[:dim]; pointCloud[8].snr=15.57
pointCloud[9].array=np.array( [3.93,-0.05,0.21,-0.66],dtype=np.float32)[:dim]; pointCloud[9].snr=15.31
pointCloud[10].array=np.array( [3.98,-0.05,0.21,-0.66],dtype=np.float32)[:dim]; pointCloud[10].snr=15.91
pointCloud[11].array=np.array( [4.03,-0.05,0.19,-0.66],dtype=np.float32)[:dim]; pointCloud[11].snr=15.36
pointCloud[12].array=np.array( [3.78,-0.03,0.23,-0.99],dtype=np.float32)[:dim]; pointCloud[12].snr=16.87
pointCloud[13].array=np.array( [3.83,-0.03,0.28,-0.55],dtype=np.float32)[:dim]; pointCloud[13].snr=17.45
pointCloud[14].array=np.array( [3.88,-0.03,0.33,-0.55],dtype=np.float32)[:dim]; pointCloud[14].snr=15.94
pointCloud[15].array=np.array( [3.93,-0.03,0.21,-0.66],dtype=np.float32)[:dim]; pointCloud[15].snr=16.52
pointCloud[16].array=np.array( [3.98,-0.03,0.21,-0.66],dtype=np.float32)[:dim]; pointCloud[16].snr=17.33
pointCloud[17].array=np.array( [4.08,-0.03,0.19,-0.66],dtype=np.float32)[:dim]; pointCloud[17].snr=15.57
pointCloud[18].array=np.array( [4.13,-0.03,0.21,-0.66],dtype=np.float32)[:dim]; pointCloud[18].snr=16.51
pointCloud[19].array=np.array( [3.78,-0.02,0.23,-0.99],dtype=np.float32)[:dim]; pointCloud[19].snr=16.70
pointCloud[20].array=np.array( [3.83,-0.02,0.28,-0.55],dtype=np.float32)[:dim]; pointCloud[20].snr=17.20
pointCloud[21].array=np.array( [3.88,-0.02,0.33,-0.55],dtype=np.float32)[:dim]; pointCloud[21].snr=16.00
pointCloud[22].array=np.array( [3.93,-0.02,0.21,-0.66],dtype=np.float32)[:dim]; pointCloud[22].snr=18.22
pointCloud[23].array=np.array( [3.98,-0.02,0.21,-0.66],dtype=np.float32)[:dim]; pointCloud[23].snr=18.78
pointCloud[24].array=np.array( [4.08,-0.02,0.19,-0.66],dtype=np.float32)[:dim]; pointCloud[24].snr=15.83
pointCloud[25].array=np.array( [4.13,-0.02,0.21,-0.66],dtype=np.float32)[:dim]; pointCloud[25].snr=17.79
pointCloud[26].array=np.array( [3.78,0.00,0.23,-0.99 ],dtype=np.float32)[:dim]; pointCloud[26].snr=16.12
pointCloud[27].array=np.array( [3.83,0.00,0.28,-0.55 ],dtype=np.float32)[:dim]; pointCloud[27].snr=16.83
pointCloud[28].array=np.array( [3.88,0.00,0.33,-0.55 ],dtype=np.float32)[:dim]; pointCloud[28].snr=15.70
pointCloud[29].array=np.array( [3.93,0.00,0.21,-0.66 ],dtype=np.float32)[:dim]; pointCloud[29].snr=18.46
pointCloud[30].array=np.array( [3.98,0.00,0.21,-0.66 ],dtype=np.float32)[:dim]; pointCloud[30].snr=17.56
pointCloud[31].array=np.array( [4.13,0.00,0.21,-0.66 ],dtype=np.float32)[:dim]; pointCloud[31].snr=16.73
pointCloud[32].array=np.array( [4.28,0.00,0.10,-6.51 ],dtype=np.float32)[:dim]; pointCloud[32].snr=15.39
pointCloud[33].array=np.array( [3.78,0.02,0.23,-0.99 ],dtype=np.float32)[:dim]; pointCloud[33].snr=15.46
pointCloud[34].array=np.array( [3.83,0.02,0.28,-0.55 ],dtype=np.float32)[:dim]; pointCloud[34].snr=16.42
pointCloud[35].array=np.array( [3.88,0.02,0.33,-0.55 ],dtype=np.float32)[:dim]; pointCloud[35].snr=15.21
pointCloud[36].array=np.array( [3.93,0.02,0.21,-0.66 ],dtype=np.float32)[:dim]; pointCloud[36].snr=16.76
pointCloud[37].array=np.array( [3.98,0.02,0.21,-0.66 ],dtype=np.float32)[:dim]; pointCloud[37].snr=16.03
pointCloud[38].array=np.array( [4.13,0.02,0.21,-0.66 ],dtype=np.float32)[:dim]; pointCloud[38].snr=15.29
pointCloud[39].array=np.array( [3.83,0.03,0.28,-0.55 ],dtype=np.float32)[:dim]; pointCloud[39].snr=15.98
pointCloud[40].array=np.array( [3.93,0.03,0.21,-0.66 ],dtype=np.float32)[:dim]; pointCloud[40].snr=15.51
pointCloud[41].array=np.array( [3.83,0.05,0.28,-0.55 ],dtype=np.float32)[:dim]; pointCloud[41].snr=15.53
pointCloud[42].array=np.array( [3.83,0.07,0.28,-0.55 ],dtype=np.float32)[:dim]; pointCloud[42].snr=15.08
mNum=int(np.array(43,dtype=np.uint16)) # Number of input measurements
targetDescr=gbl.targetDescr# Pointer to an array of GTRACK_targetDesc. This function populates the descritions for each of the tracked target
tNum=np.zeros(1,dtype=np.uint16)# Function returns a number of populated target descriptos
mIndex=np.zeros(((mNum-1)>>3)+1,dtype=np.uint8)#This function populates target indices, indicating which tracking ID was assigned to each measurment.
uIndex=np.zeros(mNum,dtype=np.uint8)#This function populates the bit array. The unique-ness of measurement N is represented by a bit = (N & 0xFF) in (N-1)>>3 byte.
presence=np.zeros(1,dtype=np.uint8)#Pointer to boolean presence indication.
gbl.gtrack_step(hTrackModule, pointCloud[0], cppyy.nullptr, mNum, targetDescr[0], tNum, mIndex, uIndex, presence, cppyy.nullptr)
return targetDescr,tNum[0],mIndex,uIndex,presence[0]
def gtrack_cppyy_delete(hTrackModule):
gbl.gtrack_delete(hTrackModule)
use3D=True
gtrack_cppyy_init(gtrackRootPath="gtrack",use3D=use3D)
hTrackModule=gtrack_cppyy_create(use3D)
if hTrackModule is not cppyy.nullptr:
targetDescr,tNum,mIndex,uIndex,presence=gtrack_cppyy_step(hTrackModule,use3D)
print(tNum)
for i in range(tNum):
S=np.frombuffer(targetDescr[i].S, dtype=np.float32).reshape((-1,3))
uCenter=np.frombuffer(targetDescr[i].uCenter, dtype=np.float32)
print(S) # pos(X,Y,Z),vel(X,Y,Z),acc(X,Y,Z)
print(uCenter) # Range(m),Azimuth(rad),Elevation(rad),Radial velocity(m/s)
gtrack_cppyy_delete(hTrackModule)