saf_tracker_internal.h

Go to the documentation of this file.

/*
 * This file is part of the saf_tracker module.
 * Copyright (c) 2020 - Leo McCormack
 *
 * The saf_tracker module is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at your
 * option) any later version.
 *
 * The saf_tracker module is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
 * more details.
 *
 * See <http://www.gnu.org/licenses/> for a copy of the GNU General Public
 * License.
 */
 
#ifndef __SAF_TRACKER_INTERNAL_H_INCLUDED__
#define __SAF_TRACKER_INTERNAL_H_INCLUDED__
 
#include "saf_tracker.h"
#include "../../modules/saf_utilities/saf_utilities.h"
#include "saf_externals.h"
 
#ifdef __cplusplus
extern "C" {
#endif /* __cplusplus */
 
#ifdef  SAF_ENABLE_TRACKER_MODULE
 
//#define TRACKER_VERBOSE
#ifdef TRACKER_VERBOSE
//# define TRACKER_VERY_VERBOSE
#endif
 
#define TRACKER3D_MAX_NUM_TARGETS ( 24 )
#define TRACKER3D_MAX_NUM_EVENTS ( 24 )
#define TRACKER3D_MAX_NUM_PARTICLES ( 100 )
 
/* ========================================================================== */
/*                            Internal Structures                             */
/* ========================================================================== */
 
typedef void* voidPtr;
 
typedef struct _M6 {
    union {
        struct { float m0, m1, m2, m3, m4, m5; };
        float M[6];
    };
} M6;
 
typedef struct _P66 {
    union {
        struct {
            float p00, p01, p02, p03, p04, p05,
                  p10, p11, p12, p13, p14, p15,
                  p20, p21, p22, p23, p24, p25,
                  p30, p31, p32, p33, p34, p35,
                  p40, p41, p42, p43, p44, p45,
                  p50, p51, p52, p53, p54, p55;
        };
        float P[6][6];
    };
} P66;
 
typedef struct _MCS {
    float W;         
    float W_prev;    
    float W0;        
    int nTargets;    
    float dt;        
    M6* M;           
    P66* P;          
    int* targetIDs;  
    int* Tcount;     
#ifdef TRACKER_VERBOSE
    char evstr[256]; 
#endif
 
} MCS_data;
 
typedef struct _tracker3d
{
    tracker3d_config tpars;
 
    /* Internal */
    void* hKF6;         
    voidPtr* SS;        
    voidPtr* SS_resamp; 
    float R[3][3];      
    float A[6][6];      
    float Q[6][6];      
    float H[3][6];      
    int incrementTime;  
    float W0;           
    /* Events */
#ifdef TRACKER_VERBOSE
    char evt[TRACKER3D_MAX_NUM_EVENTS][256]; 
#endif
    int evta[TRACKER3D_MAX_NUM_EVENTS];    
    float evp[TRACKER3D_MAX_NUM_EVENTS];   
    float evl[TRACKER3D_MAX_NUM_EVENTS];   
    float imp[TRACKER3D_MAX_NUM_EVENTS];   
    voidPtr str[TRACKER3D_MAX_NUM_EVENTS]; 
} tracker3d_data;
     
 
/* ========================================================================== */
/*                             Internal Functions                             */
/* ========================================================================== */
 
void tracker3d_particleCreate(void** phPart,
                              float W0, 
                              float dt);
 
void tracker3d_particleReset(void* hPart);
 
void tracker3d_particleCopy(void* hPart1,
                            void* hPart2);
 
void tracker3d_particleDestroy(void** phPart);
 
void tracker3d_predict(void* const hT3d, 
                       int Tinc);
 
void tracker3d_update(void* const hT3d,
                      float* Y,
                      int Tinc);
 
int tracker3d_getMaxParticleIdx(void* const hT3d);
 
 
/* ========================================================================== */
/*                              RBMCDA Functions                              */
/* ========================================================================== */
 
void resampstr(voidPtr* SS,
               int NP,
               int* s);
 
float eff_particles(voidPtr* SS,
                    int NP);
 
void normalise_weights(voidPtr* SS,
                       int NP);
 
void kf_predict6(float M[6],
                 float P[6][6],
                 float A[6][6],
                 float Q[6][6]);
 
void kf_update6_create(void ** const phUp6);
 
void kf_update6_destroy(void ** const phUp6);
 
void kf_update6(void * const hUp6,
                float X[6],
                float P[6][6],
                float y[3],
                float H[3][6],
                float R[3][3],
                float X_out[6],
                float P_out[6][6],
                float* LH);
 
float gamma_cdf(float x,
                float gam,
                float beta,
                float mu);
 
void lti_disc(/* Input Arguments */
              float* F,
              int len_N,
              int len_Q,
              float* opt_L,
              float* opt_Qc,
              float dt,
              /* Output Arguments */
              float* A,
              float* Q);
 
float gauss_pdf3(/* Input Arguments */
                 void * const hUp6,
                 float X[3],
                 float M[3],
                 float S[3][3]);
 
int categ_rnd(float* P,
              int len_P);
 
#endif /* SAF_ENABLE_TRACKER_MODULE */
 
 
#ifdef __cplusplus
} /* extern "C" */
#endif /* __cplusplus */
 
#endif /* __SAF_TRACKER_INTERNAL_H_INCLUDED__ */