SAF
Loading...
Searching...
No Matches
safmex_tracker3d.c
Go to the documentation of this file.
1/*
2 * This file is part of the saf_tracker module.
3 * Copyright (c) 2020 - Leo McCormack
4 *
5 * The saf_tracker module is free software; you can redistribute it and/or
6 * modify it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or (at your
8 * option) any later version.
9 *
10 * The saf_tracker module is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
12 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
13 * more details.
14 *
15 * See <http://www.gnu.org/licenses/> for a copy of the GNU General Public
16 * License.
17 */
18
27#include "safmex.h"
28
29/* ===================================================================== */
30/* Vars */
31/* ===================================================================== */
32
33/* user arguments */
34tracker3d_config tpars;
35
36/* internal parameters */
37void* hT3d = NULL; /* tracker3d handle */
38float* target_pos_xyz = NULL;
39float* target_var_xyz = NULL;
40int* target_IDs = NULL;
41int nTargets;
42
43
44/* ===================================================================== */
45/* MEX Wrapper */
46/* ===================================================================== */
47
48void mexFunction
49(
50 int nlhs, /* Number of output argments */
51 mxArray *plhs[], /* Pointers for output arguments */
52 int nrhs, /* Number of input argments */
53 const mxArray *prhs[] /* Pointers for input arguments */
54)
55{
56 /* mex variables */
57 int nDims;
58 int *pDims = NULL;
59
60 /* DESTROY */
61 if(nrhs == 0 && nlhs == 0){
62 if(hT3d!=NULL){
63 mexPrintf("Destroying tracker3d.\n");
64 tracker3d_destroy(&hT3d);
65 free(target_pos_xyz); target_pos_xyz = NULL;
66 free(target_var_xyz); target_var_xyz = NULL;
67 free(target_IDs); target_IDs = NULL;
68 hT3d = NULL;
69 }
70 else
71 mexPrintf("tracker3d is already dead!\n");
72 }
73
74 /* CREATE */
75 else if(nrhs == 1 && nlhs == 0){
76 if(hT3d!=NULL)
77 mexErrMsgIdAndTxt("MyToolbox:inputError","tracker3d is already initialised! First destroy it if you want to change its configuration.");
78
79 /* Check if prhs[1] is a struct */
80 if(!mxIsStruct(prhs[0]))
81 mexErrMsgIdAndTxt("MyToolbox:inputError","Input must be a struct");
82
83 /* get the values from the struct */
84 const mxArray *mxTmp;
85 double *tmp;
86
87 /* tpars.Np */
88 mxTmp = mxGetField(prhs[0],0,"Np");
89 if(mxTmp==NULL)
90 mexErrMsgIdAndTxt("MyToolbox:inputError","'Np' is not defined");
91 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=1 || (int)mxGetScalar(mxTmp)<1 || (int)mxGetScalar(mxTmp)>100)
92 mexErrMsgIdAndTxt("MyToolbox:inputError","'Np' must be an integer between 1 and 100");
93 tpars.Np = (int)mxGetScalar(mxTmp);
94
95 /* tpars.maxNactiveTargets */
96 mxTmp = mxGetField(prhs[0],0,"maxNactiveTargets");
97 if(mxTmp==NULL)
98 mexErrMsgIdAndTxt("MyToolbox:inputError","'maxNactiveTargets' is not defined");
99 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=1 || (int)mxGetScalar(mxTmp)<1 || (int)mxGetScalar(mxTmp)>100)
100 mexErrMsgIdAndTxt("MyToolbox:inputError","'maxNactiveTargets' must be an integer between 1 and 100");
101 tpars.maxNactiveTargets = (int)mxGetScalar(mxTmp);
102
103 /* tpars.noiseLikelihood */
104 mxTmp = mxGetField(prhs[0],0,"noiseLikelihood");
105 if(mxTmp==NULL)
106 mexErrMsgIdAndTxt("MyToolbox:inputError","'noiseLikelihood' is not defined");
107 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=1 || mxGetScalar(mxTmp)<0 || mxGetScalar(mxTmp)>1)
108 mexErrMsgIdAndTxt("MyToolbox:inputError","'noiseLikelihood' must be a scalar between 0 and 1");
109 tpars.noiseLikelihood = mxGetScalar(mxTmp);
110
111 /* tpars.measNoiseSD */
112 mxTmp = mxGetField(prhs[0],0,"measNoiseSD");
113 if(mxTmp==NULL)
114 mexErrMsgIdAndTxt("MyToolbox:inputError","'measNoiseSD' is not defined");
115 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=1 || mxGetScalar(mxTmp)<0)
116 mexErrMsgIdAndTxt("MyToolbox:inputError","'measNoiseSD' must be a scalar");
117 tpars.measNoiseSD = mxGetScalar(mxTmp);
118
119 /* tpars.noiseSpecDen */
120 mxTmp = mxGetField(prhs[0],0,"noiseSpecDen");
121 if(mxTmp==NULL)
122 mexErrMsgIdAndTxt("MyToolbox:inputError","'noiseSpecDen' is not defined");
123 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=1 || mxGetScalar(mxTmp)<0)
124 mexErrMsgIdAndTxt("MyToolbox:inputError","'noiseSpecDen' must be a scalar");
125 tpars.noiseSpecDen = mxGetScalar(mxTmp);
126
127 /* tpars.ALLOW_MULTI_DEATH */
128 mxTmp = mxGetField(prhs[0],0,"ALLOW_MULTI_DEATH");
129 if(mxTmp==NULL)
130 mexErrMsgIdAndTxt("MyToolbox:inputError","'ALLOW_MULTI_DEATH' is not defined");
131 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=1 || (int)mxGetScalar(mxTmp)<0 || (int)mxGetScalar(mxTmp)>1)
132 mexErrMsgIdAndTxt("MyToolbox:inputError","'ALLOW_MULTI_DEATH' must be 0 or 1");
133 tpars.ALLOW_MULTI_DEATH = (int)mxGetScalar(mxTmp);
134
135 /* tpars.init_birth */
136 mxTmp = mxGetField(prhs[0],0,"init_birth");
137 if(mxTmp==NULL)
138 mexErrMsgIdAndTxt("MyToolbox:inputError","'init_birth' is not defined");
139 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=1 || mxGetScalar(mxTmp)<0 || mxGetScalar(mxTmp)>1)
140 mexErrMsgIdAndTxt("MyToolbox:inputError","'init_birth' must be a scalar between 0 and 1");
141 tpars.init_birth = mxGetScalar(mxTmp);
142
143 /* tpars.alpha_death */
144 mxTmp = mxGetField(prhs[0],0,"alpha_death");
145 if(mxTmp==NULL)
146 mexErrMsgIdAndTxt("MyToolbox:inputError","'alpha_death' is not defined");
147 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=1 || mxGetScalar(mxTmp)<0 )
148 mexErrMsgIdAndTxt("MyToolbox:inputError","'alpha_death' must be a scalar");
149 tpars.alpha_death = mxGetScalar(mxTmp);
150
151 /* tpars.beta_death */
152 mxTmp = mxGetField(prhs[0],0,"beta_death");
153 if(mxTmp==NULL)
154 mexErrMsgIdAndTxt("MyToolbox:inputError","'beta_death' is not defined");
155 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=1 || mxGetScalar(mxTmp)<0 )
156 mexErrMsgIdAndTxt("MyToolbox:inputError","'beta_death' must be a scalar");
157 tpars.beta_death = mxGetScalar(mxTmp);
158
159 /* tpars.dt */
160 mxTmp = mxGetField(prhs[0],0,"dt");
161 if(mxTmp==NULL)
162 mexErrMsgIdAndTxt("MyToolbox:inputError","'dt' is not defined");
163 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=1 || mxGetScalar(mxTmp)<0 )
164 mexErrMsgIdAndTxt("MyToolbox:inputError","'dt' must be a scalar");
165 tpars.dt = mxGetScalar(mxTmp);
166
167 /* tpars.W_avg_coeff */
168 mxTmp = mxGetField(prhs[0],0,"W_avg_coeff");
169 if(mxTmp==NULL)
170 mexErrMsgIdAndTxt("MyToolbox:inputError","'W_avg_coeff' is not defined");
171 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=1 || mxGetScalar(mxTmp)<0 || mxGetScalar(mxTmp)>1)
172 mexErrMsgIdAndTxt("MyToolbox:inputError","'W_avg_coeff' must be a scalar between 0 and 1");
173 tpars.W_avg_coeff = mxGetScalar(mxTmp);
174
175 /* tpars.FORCE_KILL_TARGETS */
176 mxTmp = mxGetField(prhs[0],0,"FORCE_KILL_TARGETS");
177 if(mxTmp==NULL)
178 mexErrMsgIdAndTxt("MyToolbox:inputError","'FORCE_KILL_TARGETS' is not defined");
179 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=1 || (int)mxGetScalar(mxTmp)<0 || (int)mxGetScalar(mxTmp)>1)
180 mexErrMsgIdAndTxt("MyToolbox:inputError","'FORCE_KILL_TARGETS' must be 0 or 1");
181 tpars.FORCE_KILL_TARGETS = (int)mxGetScalar(mxTmp);
182
183 /* tpars.forceKillDistance */
184 mxTmp = mxGetField(prhs[0],0,"forceKillDistance");
185 if(mxTmp==NULL)
186 mexErrMsgIdAndTxt("MyToolbox:inputError","'forceKillDistance' is not defined");
187 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=1 || mxGetScalar(mxTmp)<0 )
188 mexErrMsgIdAndTxt("MyToolbox:inputError","'forceKillDistance' must be a scalar");
189 tpars.forceKillDistance = mxGetScalar(mxTmp);
190
191 /* tpars.M0 */
192 mxTmp = mxGetField(prhs[0],0,"M0");
193 if(mxTmp==NULL)
194 mexErrMsgIdAndTxt("MyToolbox:inputError","'M0' is not defined");
195 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=6)
196 mexErrMsgIdAndTxt("MyToolbox:inputError","'M0' must be a 6-element vector");
197 tmp = mxGetData(mxTmp);
198 for(int jj = 0; jj<6; jj++)
199 tpars.M0[jj] = (float)tmp[jj];
200
201 /* tpars.P0 */
202 mxTmp = mxGetField(prhs[0],0,"P0");
203 if(mxTmp==NULL)
204 mexErrMsgIdAndTxt("MyToolbox:inputError","'P0' is not defined");
205 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=36)
206 mexErrMsgIdAndTxt("MyToolbox:inputError","'P0' must be a 6-by-6 matrix (or a stacked 36-element vector)");
207 tmp = mxGetData(mxTmp);
208 for(int jj = 0; jj<6; jj++)
209 for(int kk=0; kk<6; kk++)
210 tpars.P0[jj][kk] = (float)tmp[jj*6+kk];
211
212 /* tpars.cd */
213 mxTmp = mxGetField(prhs[0],0,"cd");
214 if(mxTmp==NULL)
215 mexErrMsgIdAndTxt("MyToolbox:inputError","'cd' is not defined");
216 if (mxIsComplex(mxTmp) || mxGetNumberOfElements(mxTmp)!=1 || mxGetScalar(mxTmp)<0 )
217 mexErrMsgIdAndTxt("MyToolbox:inputError","'cd' must be a scalar");
218 tpars.cd = mxGetScalar(mxTmp);
219
220 /* Create an instance of tracker3d */
221 mexPrintf("Creating tracker3d.\n");
222 tracker3d_create(&hT3d, tpars);
223 }
224
225 /* Step */
226 else if(nrhs == 1 && nlhs == 2){
227 if(hT3d==NULL)
228 mexErrMsgIdAndTxt("MyToolbox:inputError","safmex_tracker3d is uninitialised!");
229
230 /* Find dimensionality of input */
231 mwSize nDims_mx;
232 const mwSize *pDims_mx;
233 nDims_mx = mxGetNumberOfDimensions(prhs[0]);
234 pDims_mx = mxGetDimensions(prhs[0]);
235 if (nDims_mx!=2)
236 mexErrMsgIdAndTxt("MyToolbox:inputError","Observations must be N x 3 (x,y,z)");
237 if (pDims_mx[1]!=3)
238 mexErrMsgIdAndTxt("MyToolbox:inputError","Observations must be N x 3 (x,y,z)");
239
240 /* New measurements */
241 float* newObs_xyz = NULL;
242 int nObs;
243 MEXdouble2SAFsingle(prhs[0], &newObs_xyz, &nDims, &pDims);
244 nObs = pDims[0];
245
246 /* Pass to tracker */
247 tracker3d_step(hT3d, newObs_xyz, nObs, &target_pos_xyz, &target_var_xyz, &target_IDs, &nTargets);
248
249 /* output */
250 if(nTargets==0){
251 plhs[0] = mxCreateDoubleMatrix( 0, 0, mxREAL );
252 plhs[1] = mxCreateDoubleMatrix( 0, 0, mxREAL );
253 }
254 else{
255 nDims = 2;
256 pDims = realloc1d(pDims, nDims*sizeof(int));
257 pDims[0] = nTargets;
258 pDims[1] = 3;
259 SAFsingle2MEXdouble(target_pos_xyz, nDims, pDims, &plhs[0]);
260 pDims[0] = nTargets;
261 pDims[1] = 1;
262 SAFsingle2MEXdouble_int(target_IDs, nDims, pDims, &plhs[1]);
263 }
264
265 /* Clean-up */
266 free(newObs_xyz);
267
268 }
269
270 /* ERROR */
271 else
272 mexErrMsgIdAndTxt("MyToolbox:inputError","Unrecognised input/output configuration, refer to help instructions.");
273}
tracker3d_destroy
void tracker3d_destroy(void **const phT3d)
Destroys an instance of the mighty tracker3d.
Definition saf_tracker.c:127
tracker3d_step
void tracker3d_step(void *const hT3d, float *newObs_xyz, int nObs, float **target_pos_xyz, float **target_var_xyz, int **target_IDs, int *nTargets)
Tracker time step to update & predict current target locations and to parse new measurements/observat...
Definition saf_tracker.c:168
tracker3d_create
void tracker3d_create(void **const phT3d, tracker3d_config tpars)
Creates an instance of the mighty tracker3d.
Definition saf_tracker.c:50
realloc1d
void * realloc1d(void *ptr, size_t dim1_data_size)
1-D realloc (same as realloc, but with error checking)
Definition md_malloc.c:79
safmex.h
Main include header for safmex.
MEXdouble2SAFsingle
void MEXdouble2SAFsingle(const mxArray *in, float **out, int *nDims, int **pDims)
Convert a mex double-precision array into single precision array for SAF.
Definition safmex.h:151
SAFsingle2MEXdouble_int
void SAFsingle2MEXdouble_int(int *in, int nDims, int *dims, mxArray **out)
Convert a single precision array used by SAF to mex double-precision array (integers)
Definition safmex.h:410
SAFsingle2MEXdouble
void SAFsingle2MEXdouble(float *in, int nDims, int *dims, mxArray **out)
Convert a single precision array used by SAF to mex double-precision array.
Definition safmex.h:309
tracker3d_config
User parameters for tracker3d.
Definition saf_tracker.h:59
tracker3d_config::dt
float dt
Elapsed time (in seconds) between observations/measurements.
Definition saf_tracker.h:84
tracker3d_config::M0
float M0[6]
[0,1,2] Position of sound source PRIORs (x,y,z), [3,4,5] Mean velocity PRIORs (x,y,...
Definition saf_tracker.h:97
tracker3d_config::cd
float cd
PRIOR probability of noise.
Definition saf_tracker.h:106
tracker3d_config::init_birth
float init_birth
PRIOR probability of birth [0 1].
Definition saf_tracker.h:79
tracker3d_config::measNoiseSD
float measNoiseSD
Measurement noise standard deviation.
Definition saf_tracker.h:71
tracker3d_config::ALLOW_MULTI_DEATH
int ALLOW_MULTI_DEATH
FLAG whether to allow for multiple target deaths in the same tracker prediction step.
Definition saf_tracker.h:76
tracker3d_config::FORCE_KILL_TARGETS
int FORCE_KILL_TARGETS
FLAG force kill targets that are too close to one another.
Definition saf_tracker.h:90
tracker3d_config::P0
float P0[6][6]
Diagonal matrix, [0,1,2] Variance PRIORs of estimates along the x,y,z axes; [3,4,5] Velocity PRIORs o...
Definition saf_tracker.h:102
tracker3d_config::maxNactiveTargets
int maxNactiveTargets
Maximum number of simultaneous targets permitted.
Definition saf_tracker.h:67
tracker3d_config::forceKillDistance
float forceKillDistance
Euclidian distance at which to kill targets that come too close to other (older) targets (<=).
Definition saf_tracker.h:94
tracker3d_config::beta_death
float beta_death
Coefficient influencing the likelihood that a target will die; always >= 1.
Definition saf_tracker.h:82
tracker3d_config::alpha_death
float alpha_death
Coefficient influencing the likelihood that a target will die; always >= 1.
Definition saf_tracker.h:80
tracker3d_config::Np
int Np
Number of Monte Carlo samples/particles.
Definition saf_tracker.h:60
tracker3d_config::noiseLikelihood
float noiseLikelihood
Likelihood of an estimate being noise/clutter between [0..1].
Definition saf_tracker.h:69
tracker3d_config::W_avg_coeff
float W_avg_coeff
Real-time tracking is based on the particle with highest weight.
Definition saf_tracker.h:86
tracker3d_config::noiseSpecDen
float noiseSpecDen
Noise spectral density; influences the smoothness of the traget tracks.
Definition saf_tracker.h:74