//----------------------------------------------------------------------------- // // SFMEKFb.h++ // // 3D Structure Estimation Interface Class Declaration with Bias // // Tony Jebara (Copyright (C) 1997) // //----------------------------------------------------------------------------- #ifndef SFMEKFb_h #define SFMEKb_h #include "EKF.h++" // Default unit image scale values #define SMKF_DEFAULT_VAR_TR_NOISE (.1) #define SMKF_DEFAULT_VAR_ROT_NOISE (.01) #define SMKF_DEFAULT_VAR_MEAS_NOISE (.00006) #define SMKF_DEFAULT_VAR_DEPTH_NOISE (.00006) #define SMKF_DEFAULT_B_O (2.) #define SMKF_DEFAULT_X_O (0.) #define SMKF_DEFAULT_Y_O (0.) #define SMKF_DEFAULT_ZB_O (0.) #define SMKF_DEFAULT_THX_O (0.) #define SMKF_DEFAULT_THY_O (0.) #define SMKF_DEFAULT_THZ_O (0.) #define SMKF_DEFAULT_B_O (2.) #define SMKF_DEFAULT_PX_O (0.) #define SMKF_DEFAULT_PY_O (0.) #define SMKF_DEFAULT_A_O (1.) #define SMKF_DEFAULT_VAR_X_O (0.) #define SMKF_DEFAULT_VAR_Y_O (0.) #define SMKF_DEFAULT_VAR_ZB_O (0.) #define SMKF_DEFAULT_VAR_THX_O (0.) #define SMKF_DEFAULT_VAR_THY_O (0.) #define SMKF_DEFAULT_VAR_THZ_O (0.) #define SMKF_DEFAULT_VAR_B_O (4.) #define SMKF_DEFAULT_VAR_PX_O (0.) #define SMKF_DEFAULT_VAR_PY_O (0.) #define SMKF_DEFAULT_VAR_A_O (.1) #define SMKF_DEFAULT_VAR_BIAS (.01) // Total Number of Motion and Camera params #define NSTATES_MINUS_STRUCTURE 9 #define SMKF_NSMS NSTATES_MINUS_STRUCTURE // Pull out params and depth from the state vector #define SMKF_X(vector) (vector->d[0]) #define SMKF_Y(vector) (vector->d[1]) #define SMKF_ZB(vector) (vector->d[2]) #define SMKF_THX(vector) (vector->d[3]) #define SMKF_THY(vector) (vector->d[4]) #define SMKF_THZ(vector) (vector->d[5]) #define SMKF_B(vector) (vector->d[6]) #define SMKF_PX(vector) (vector->d[7]) #define SMKF_PY(vector) (vector->d[8]) #define SMKF_ALPHA(vector, i) (vector->d[SMKF_NSMS + i]) #define OSMKF_X(vector) (vector[0]) #define OSMKF_Y(vector) (vector[1]) #define OSMKF_ZB(vector) (vector[2]) #define OSMKF_THX(vector) (vector[3]) #define OSMKF_THY(vector) (vector[4]) #define OSMKF_THZ(vector) (vector[5]) #define OSMKF_B(vector) (vector[6]) #define OSMKF_PX(vector) (vector[7]) #define OSMKF_PY(vector) (vector[8]) #define OSMKF_ALPHA(vector, i) (vector[SMKF_NSMS + i]) // Pull out variances from a correlation matrix on the state #define SMKF_VAR_BETA(matrix) (matrix->d2[6][6]) #define SMKF_VAR_PX(matrix) (matrix->d2[7][7]) #define SMKF_VAR_PY(matrix) (matrix->d2[8][8]) #define SMKF_VAR_ALPHA(matrix, i) (matrix->d2[SMKF_NSMS+i][SMKF_NSMS+i]) class SFMEKFb { public: int width; int height; int npoints; int n; int n_meas; int n_stat; Vector qp; Vector qe; Vector yO; Vector xp; Vector Vxp; Vector yo; // Original feature locations Matrix T3x3; double Qro; double Qtho; double Ro; Matrix R; Vector coords3D; Vector pred2D; int tp; int te; EKF *ekf; void set_X(double v) { SMKF_X(xp) = v; } void set_Y(double v) { SMKF_Y(xp) = v; } void set_ZB(double v) { SMKF_ZB(xp) = v; } void set_THX(double v) { SMKF_THX(xp) = v; } void set_THY(double v) { SMKF_THY(xp) = v; } void set_THZ(double v) { SMKF_THZ(xp) = v; } void set_B(double v) { SMKF_B(xp) = v; } void set_PX(double v) { SMKF_PX(xp) = v; } void set_PY(double v) { SMKF_PY(xp) = v; } void set_vX(double v) { SMKF_X(Vxp) = v; } void set_vY(double v) { SMKF_Y(Vxp) = v; } void set_vZB(double v) { SMKF_ZB(Vxp) = v; } void set_vTHX(double v) { SMKF_THX(Vxp) = v; } void set_vTHY(double v) { SMKF_THY(Vxp) = v; } void set_vTHZ(double v) { SMKF_THZ(Vxp) = v; } void set_vB(double v) { SMKF_B(Vxp) = v; } void set_vPX(double v) { SMKF_PX(Vxp) = v; } void set_vPY(double v) { SMKF_PY(Vxp) = v; } void set_Qro(double v) { Qro = v; } void set_Qtho(double v) { Qtho = v; } void set_Ro(double v) { Ro = v; VectorSet((Vector) R, 0.0); int i; for (i=0; irows; i++) R->d2[i][i] = Ro; } void set_Alpha(Vector inv) { int i; for (i=0;id[i]; } void set_VarAlpha(Vector inv) { int i; for (i=0;id[i]; } void set_OriginalFeats(Vector inv) { VectorMove(yo,inv); } SFMEKFb(int in_npoints, double in_width, double in_height); ~SFMEKFb() {destroy();} void changeCoords(Vector y); void changeCoords(Matrix r); void compute3DCoords(); void initializeRAW(Vector in_yo); void initialize(Vector in_yo) { changeCoords(in_yo); initializeRAW(in_yo); } void initializeEKF(); void reinitializeRAW(Vector in_yo); void measurementRAW(Vector y); void measurement(Vector y) { changeCoords(y); measurementRAW(y); } void noiseRAW(Matrix r); void noise(Matrix r) { changeCoords(r); noiseRAW(r); } void Update(); void Filter_Init(); void Linearize(); void Predict_Y(); void Estimate_X(); void Predict_X(); void destroy(); #ifdef NOTNOW void reinitialize(float *in_x, float *in_y); void input(int node, float in_x, float in_y, float in_vx, float in_vy); void step(int refresh, float *in_x, float *in_y, float *in_vx, float *in_vy); float goodness(float *in_x, float *in_y, float *in_vx, float *in_vy); #endif }; #endif