void booz_sensors_model_accel_init(double time) { bsm.accel = v_get(AXIS_NB); bsm.accel->ve[AXIS_X] = 0.; bsm.accel->ve[AXIS_Y] = 0.; bsm.accel->ve[AXIS_Z] = 0.; bsm.accel_resolution = BSM_ACCEL_RESOLUTION; bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB); m_zero(bsm.accel_sensitivity); bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = BSM_ACCEL_SENSITIVITY_XX; bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_ACCEL_SENSITIVITY_YY; bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_ACCEL_SENSITIVITY_ZZ; bsm.accel_neutral = v_get(AXIS_NB); bsm.accel_neutral->ve[AXIS_X] = BSM_ACCEL_NEUTRAL_X; bsm.accel_neutral->ve[AXIS_Y] = BSM_ACCEL_NEUTRAL_Y; bsm.accel_neutral->ve[AXIS_Z] = BSM_ACCEL_NEUTRAL_Z; bsm.accel_noise_std_dev = v_get(AXIS_NB); bsm.accel_noise_std_dev->ve[AXIS_X] = BSM_ACCEL_NOISE_STD_DEV_X; bsm.accel_noise_std_dev->ve[AXIS_Y] = BSM_ACCEL_NOISE_STD_DEV_Y; bsm.accel_noise_std_dev->ve[AXIS_Z] = BSM_ACCEL_NOISE_STD_DEV_Z; bsm.accel_bias = v_get(AXIS_NB); bsm.accel_bias->ve[AXIS_P] = BSM_ACCEL_BIAS_X; bsm.accel_bias->ve[AXIS_Q] = BSM_ACCEL_BIAS_Y; bsm.accel_bias->ve[AXIS_R] = BSM_ACCEL_BIAS_Z; bsm.accel_next_update = time; bsm.accel_available = FALSE; }
// kernelRegression // x:data // dimention: x's dimention // y: label of x // num: number of data // result: coeffience double *kernelRegression(double **x,int dimention,double *y,int num,double gamma,double regilarization){ VEC *label; MAT *gram,*ident,*inv; int i,j; double *result; ident = m_get(num,num); label = v_get(num); memcpy(label->ve,y,sizeof(double)*num); gram = m_get(num,num); ident = m_get(num,num); for(i=0;i<num;i++){ for(j=0;j<num;j++){ gram->me[i][j]=kernel(x[i],x[j],dimention,gamma); } } inv=m_add(gram,sm_mlt(regilarization,m_ident(ident),NULL),NULL); inv=m_inverse(inv,NULL); //memory leak. VEC *coefficient=v_get(num); mv_mlt(inv,label,coefficient); result=malloc(sizeof(double)*num); memcpy(result,coefficient->ve,sizeof(double)*num); m_free(ident); m_free(gram); m_free(inv); v_free(label); v_free(coefficient); return result; }
void booz_sensors_model_mag_init( double time ) { bsm.mag = v_get(AXIS_NB); bsm.mag->ve[AXIS_X] = 0.; bsm.mag->ve[AXIS_Y] = 0.; bsm.mag->ve[AXIS_Z] = 0.; // bsm.mag_resolution = BSM_MAG_RESOLUTION; bsm.mag_imu_to_sensor = m_get(AXIS_NB, AXIS_NB); VEC* tmp_eulers = v_get(AXIS_NB); tmp_eulers->ve[EULER_PHI] = BSM_MAG_IMU_TO_SENSOR_PHI; tmp_eulers->ve[EULER_THETA] = BSM_MAG_IMU_TO_SENSOR_THETA; tmp_eulers->ve[EULER_PSI] = BSM_MAG_IMU_TO_SENSOR_PSI; dcm_of_eulers (tmp_eulers, bsm.mag_imu_to_sensor ); bsm.mag_sensitivity = m_get(AXIS_NB, AXIS_NB); m_zero(bsm.mag_sensitivity); bsm.mag_sensitivity->me[AXIS_X][AXIS_X] = BSM_MAG_SENSITIVITY_XX; bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_MAG_SENSITIVITY_YY; bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_MAG_SENSITIVITY_ZZ; bsm.mag_neutral = v_get(AXIS_NB); bsm.mag_neutral->ve[AXIS_X] = BSM_MAG_NEUTRAL_X; bsm.mag_neutral->ve[AXIS_Y] = BSM_MAG_NEUTRAL_Y; bsm.mag_neutral->ve[AXIS_Z] = BSM_MAG_NEUTRAL_Z; bsm.mag_noise_std_dev = v_get(AXIS_NB); bsm.mag_noise_std_dev->ve[AXIS_X] = BSM_MAG_NOISE_STD_DEV_X; bsm.mag_noise_std_dev->ve[AXIS_Y] = BSM_MAG_NOISE_STD_DEV_Y; bsm.mag_noise_std_dev->ve[AXIS_Z] = BSM_MAG_NOISE_STD_DEV_Z; bsm.mag_next_update = time; bsm.mag_available = FALSE; }
/* v_resize -- returns the vector x with dim new_dim -- x is set to the zero vector */ extern VEC *v_resize(VEC *x, int new_dim) { if (new_dim < 0) error(E_NEG,"v_resize"); if ( ! x ) return v_get(new_dim); /* nothing is changed */ if (new_dim == x->dim) return x; if ( x->max_dim == 0 ) /* assume that it's from sub_vec */ return v_get(new_dim); if ( new_dim > x->max_dim ) { if (mem_info_is_on()) { mem_bytes(TYPE_VEC,x->max_dim*sizeof(Real), new_dim*sizeof(Real)); } x->ve = RENEW(x->ve,new_dim,Real); if ( ! x->ve ) error(E_MEM,"v_resize"); x->max_dim = new_dim; } if ( new_dim > x->dim ) __zero__(&(x->ve[x->dim]),new_dim - x->dim); x->dim = new_dim; return x; }
rot2d_context_t *rot2d_create(void) { rot2d_context_t *context = (rot2d_context_t *)malloc(sizeof(rot2d_context_t)); /* allocate memory for matrix/vector data structures: */ context->matrix = m_get(2, 2); context->in = v_get(2); context->out = v_get(2); return context; }
MAT *m_inverse(const MAT *A, MAT *out) { unsigned int i; char MatrixTempBuffer[ 4000 ]; VEC *tmp = VNULL, *tmp2 = VNULL; MAT *A_cp = MNULL; PERM *pivot = PNULL; if ( ! A ) error(E_NULL,"m_inverse"); if ( A->m != A->n ) error(E_SQUARE,"m_inverse"); if ( ! out || out->m < A->m || out->n < A->n ) out = m_resize(out,A->m,A->n); if( SET_MAT_SIZE( A->m, A->n ) < 1000 ) mat_get( &A_cp, (void *)( MatrixTempBuffer + 2000 ), A->m, A->n ); else A_cp = matrix_get( A->m, A->n ); A_cp = m_copy( A, A_cp ); if( SET_VEC_SIZE( A->m ) < 1000 ) { vec_get( &tmp, (void *)MatrixTempBuffer, A->m ); vec_get( &tmp2, (void *)(MatrixTempBuffer + 1000), A->m ); } else { tmp = v_get( A->m ); tmp2 = v_get( A->m ); } if( SET_PERM_SIZE( A->m ) < 1000 ) { perm_get( &pivot, (void *)( MatrixTempBuffer + 3000 ), A->m ); } else { pivot = px_get( A->m ); } LUfactor(A_cp,pivot); //tracecatch_matrix(LUfactor(A_cp,pivot),"m_inverse"); for ( i = 0; i < A->n; i++ ){ v_zero(tmp); tmp->ve[i] = 1.0; LUsolve(A_cp,pivot,tmp,tmp2); //tracecatch_matrix(LUsolve(A_cp,pivot,tmp,tmp2),"m_inverse"); set_col(out,i,tmp2); } if( tmp != (VEC *)(MatrixTempBuffer ) ) // память выделялась, надо освободить V_FREE(tmp); if( tmp2 != (VEC *)(MatrixTempBuffer + 1000) ) // память выделялась, надо освободить V_FREE(tmp2); if( A_cp != (MAT *)(MatrixTempBuffer + 2000) ) // память выделялась, надо освободить M_FREE(A_cp); if( pivot != (PERM *)(MatrixTempBuffer + 3000) ) // память выделялась, надо освободить PX_FREE( pivot ); return out; }
void id3db_print(int db) { struct id3ent *d; int p; m_foreach(db, p, d ) { char *album = v_get(d->vs, "album", 1 ); char *title = v_get(d->vs, "title", 1 ); printf( "A: %s\n", album ); printf( "T: %s\n", title ); printf( "F: %s\n", d->path ); }
//-------------------------------------------------------------------------- Omu_IntRKsuite::Omu_IntRKsuite() { theOmu_IntRKsuite = this; _y_head.max_dim = 0; _yp_head.max_dim = 0; _yp = v_get(1); _u = v_get(1); _work = v_get(1); _thres = v_get(1); _hnext = 0.0; _tlast = 0.0; _method = 2; _ifList.append(new If_Int("prg_int_method", &_method)); }
static void kalman_init(kalman_t *kf, float q, float r, float pos, float speed) { kf->t0 = v_get(2); kf->t1 = v_get(2); kf->T0 = m_get(2, 2); kf->T1 = m_get(2, 2); kf->I = m_get(2, 2); m_ident(kf->I); /* set initial state: */ kf->x = v_get(2); v_set_val(kf->x, 0, pos); v_set_val(kf->x, 1, speed); /* no measurement or control yet: */ kf->z = v_get(2); kf->u = v_get(1); kf->P = m_get(2, 2); m_ident(kf->P); /* set up noise: */ kf->Q = m_get(2, 2); sm_mlt(q, kf->I, kf->Q); kf->R = m_get(2, 2); sm_mlt(r, kf->I, kf->R); kf->K = m_get(2, 1); kf->H = m_get(2, 2); m_set_val(kf->H, 0, 0, 1.0); //m_ident(kf->H); /* A = | 1.0 dt | | 0.0 1.0 | dt values is set in kalman_run */ kf->A = m_get(2, 2); m_set_val(kf->A, 0, 0, 1.0); m_set_val(kf->A, 1, 0, 0.0); m_set_val(kf->A, 1, 1, 1.0); /* B = | 0.5 * dt ^ 2 | | dt | dt values are set in kalman_run */ kf->B = m_get(2, 1); }
//-------------------------------------------------------------------------- Omu_IntODE::Omu_IntODE() { _sys = NULL; _xt_ptr = NULL; _Ft_ptr = NULL; _y = v_get(1); _u = v_get(1); _x = v_get(1); _v = v_get(1); _X2 = m_get(1, 1); _Y2 = m_get(1, 1); _Yx = m_get(1, 1); _Yu = m_get(1, 1); }
int16_t peakiness(int16_t input[], int16_t npts) { register int16_t i; int16_t peak_fact, scale = 4; int32_t sum_abs, L_temp; int16_t temp1, temp2, *temp_buf; temp_buf = v_get(npts); v_equ_shr(temp_buf, input, scale, npts); L_temp = L_v_magsq(temp_buf, npts, 0, 1); if (L_temp) { temp1 = melpe_norm_l(L_temp); scale = melpe_sub(scale, melpe_shr(temp1, 1)); if (scale < 0) scale = 0; } else scale = 0; sum_abs = 0; for (i = 0; i < npts; i++) { L_temp = melpe_L_deposit_l(melpe_abs_s(input[i])); sum_abs = melpe_L_add(sum_abs, L_temp); } /* Right shift input signal and put in temp buffer. */ if (scale) v_equ_shr(temp_buf, input, scale, npts); if (sum_abs > 0) { /* peak_fact = sqrt(npts * v_magsq(input, npts))/sum_abs */ /* = sqrt(npts) * (sqrt(v_magsq(input, npts))/sum_abs) */ if (scale) L_temp = L_v_magsq(temp_buf, npts, 0, 0); else L_temp = L_v_magsq(input, npts, 0, 0); L_temp = melpe_L_deposit_l(L_sqrt_fxp(L_temp, 0)); /* L_temp in Q0 */ peak_fact = L_divider2(L_temp, sum_abs, 0, 0); if (peak_fact > LIMIT_PEAKI) { peak_fact = SW_MAX; } else { /* shl 7 is mult , other shift is Q7->Q12 */ temp1 = melpe_add(scale, 5); temp2 = melpe_shl(npts, 7); temp2 = sqrt_fxp(temp2, 7); L_temp = melpe_L_mult(peak_fact, temp2); L_temp = melpe_L_shl(L_temp, temp1); peak_fact = melpe_extract_h(L_temp); } } else peak_fact = 0; v_free(temp_buf); return (peak_fact); }
double kernel(double *x1,double *x2,int num,double gamma){ VEC *xVector,*yVector,*diffVector; int i; xVector=v_get(num); memcpy(xVector->ve,x1,sizeof(double)*num); yVector=v_get(num); memcpy(yVector->ve,x2,sizeof(double)*num); diffVector=v_sub(xVector,yVector,NULL); double sum=0.0; for(i=0;i<num;i++){ sum+=diffVector->ve[i]*diffVector->ve[i]; } v_free(diffVector); v_free(xVector); v_free(yVector); return exp(-gamma*sum); }
int Tcl_AppInit(Tcl_Interp *interp) { if (Tcl_InitStubs(interp, "8.1", 0) == NULL) { return TCL_ERROR; } if (Tcl_Init(interp) == TCL_ERROR) { return TCL_ERROR; } theInterp = interp; new If_Int("if_int", &if_int); new If_Int("if_intCb", new If_GetCb<int, A>(&A::i, &a)); new If_Bool("if_bool", &if_bool); new If_Real("if_real", &if_real); if_realVec = v_get(5); new If_RealVec("if_realVec", &if_realVec); if_realMat = m_get(2,3); new If_RealMat("if_realMat", &if_realMat); if_intVec = iv_get(3); new If_IntVec("if_intVec", &if_intVec); new If_IntVec("if_intVecp", new If_GetCb<const IVECP, A>(&A::intVecp, &a), new If_SetCb<const IVECP, A>(&A::set_intVecp, &a)); new If_String("if_string", &if_string); new If_String("if_stringCb", new If_GetCb<const char *, A>(&A::string, &a), new If_SetCb<const char *, A>(&A::set_string, &a)); new If_StdString("if_stdString", new If_GetCb<const std::string&, A>(&A::stdString, &a), new If_SetCb<const std::string&, A>(&A::set_stdString, &a)); new If_Procedure("if_procedure", &if_procedure); new If_Method<A>("if_method", &A::if_method, &a); Tcl_SetVar(interp, "tcl_rcFileName", "~/.tclshrc", TCL_GLOBAL_ONLY); return TCL_OK; }
void quat_rot_vec(VEC *q1, VEC *v1){ VEC *quat_of_vec, *qmulv, *qinv, *fin_rot_vec; //all are quaternions quat_of_vec = v_get(4); v_zero(quat_of_vec); qmulv = v_get(4); v_zero(qmulv); qinv = v_get(4); v_zero(qinv); fin_rot_vec = v_get(4); v_zero(fin_rot_vec); vec2quat(v1, quat_of_vec); quat_inv(q1, qinv); quat_mul(qinv, quat_of_vec, qmulv); quat_mul(qmulv, q1, fin_rot_vec); quat2vec(fin_rot_vec, v1); v_free(quat_of_vec); v_free(qmulv); v_free(qinv); v_free(fin_rot_vec); }
double rk4(double t, VEC *x, double h, VEC *Torq) { VEC *v1=VNULL, *v2=VNULL, *v3=VNULL, *v4=VNULL; VEC *temp=VNULL; double step_size=0.5*h; if ( x == VNULL ) error(E_NULL,"rk4"); v1 = v_get(x->dim); v2 = v_get(x->dim); v3 = v_get(x->dim); v4 = v_get(x->dim); temp = v_get(x->dim); Sy_m(x, Torq, v1); v_mltadd(x,v1,step_size,temp); Sy_m(temp, Torq, v2); v_mltadd(x,v2,step_size,temp); Sy_m(temp, Torq, v3); step_size = h; v_mltadd(x, v3, step_size, temp); Sy_m(temp, Torq, v4); temp = v_copy(v1,temp); v_mltadd(temp,v2,2.0,temp); v_mltadd(temp,v3,2.0,temp); v_add(temp,v4,temp); v_mltadd(x,temp,(h/6.0),x); return t+h; V_FREE(v1); V_FREE(v2); V_FREE(v3); V_FREE(v4); V_FREE(temp); }
//------------------------------------------------------------------------- Prg_ASCEND::Prg_ASCEND() { _nvars = 0; _nrels = 0; _vars = NULL; _rels = NULL; _obj = NULL; _safe_calc = TRUE; _var_lb = v_resize(v_get(1), 0); _var_ub = v_resize(v_get(1), 0); _var_asc2hqp = iv_resize(iv_get(1), 0); _derivatives = v_resize(v_get(1), 0); _var_master_idxs = iv_resize(iv_get(1), 0); _var_solver_idxs = iv_resize(iv_get(1), 0); _Inf = 1e38; // todo: should be initialized from ASCEND data _me_bounds = 0; _m_bounds = 0; memset(&_slv_status, 0, sizeof(slv_status_t)); // create interface elements _ifList.append(new If_Int("prg_safe_calc", &_safe_calc)); slv_register_client(Prg_ASCEND::slv_register, NULL, NULL); }
extern int v_get_vars(int dim,...) { va_list ap; int i=0; VEC **par; va_start(ap, dim); while (par = va_arg(ap,VEC **)) { /* NULL ends the list*/ *par = v_get(dim); i++; } va_end(ap); return i; }
void booz_sensors_model_init(double time) { VEC* tmp_eulers = v_get(AXIS_NB); tmp_eulers->ve[EULER_PHI] = BSM_BODY_TO_IMU_PHI; tmp_eulers->ve[EULER_THETA] = BSM_BODY_TO_IMU_THETA; tmp_eulers->ve[EULER_PSI] = BSM_BODY_TO_IMU_PSI; bsm.body_to_imu = m_get(AXIS_NB, AXIS_NB); dcm_of_eulers (tmp_eulers, bsm.body_to_imu ); booz_sensors_model_accel_init(time); booz_sensors_model_gyro_init(time); booz_sensors_model_mag_init(time); booz_sensors_model_rangemeter_init(time); booz_sensors_model_baro_init(time); booz_sensors_model_gps_init(time); }
void quat_mul(VEC *q1, VEC *q2, VEC *res) { int i; VEC *temp; temp = v_get(4); if(q1->dim != 4 || q2->dim != 4) { // cliPrintf("\nquaternion dimensions not correct in quat_mul"); } temp->ve[3] = (q1->ve[3] * q2->ve[3]) - (q1->ve[0] * q2->ve[0]) - (q1->ve[1] * q2->ve[1]) - (q1->ve[2] * q2->ve[2]); temp->ve[0] = (q1->ve[3] * q2->ve[0]) + (q1->ve[0] * q2->ve[3]) + (q1->ve[1] * q2->ve[2]) - (q1->ve[2] * q2->ve[1]); temp->ve[1] = (q1->ve[3] * q2->ve[1]) + (q1->ve[1] * q2->ve[3]) + (q1->ve[2] * q2->ve[0]) - (q1->ve[0] * q2->ve[2]); temp->ve[2] = (q1->ve[3] * q2->ve[2]) + (q1->ve[2] * q2->ve[3]) + (q1->ve[0] * q2->ve[1]) - (q1->ve[1] * q2->ve[0]); for (i=0; i<4;i++){ res->ve[i] = temp->ve[i]; } v_free(temp); }
VEC *iter_cgs(ITER *ip, VEC *r0) #endif { STATIC VEC *p = VNULL, *q = VNULL, *r = VNULL, *u = VNULL; STATIC VEC *v = VNULL, *z = VNULL; VEC *tmp; Real alpha, beta, nres, rho, old_rho, sigma, inner; if (ip == INULL) error(E_NULL,"iter_cgs"); if (!ip->Ax || !ip->b || !r0) error(E_NULL,"iter_cgs"); if ( ip->x == ip->b ) error(E_INSITU,"iter_cgs"); if (!ip->stop_crit) error(E_NULL,"iter_cgs"); if ( r0->dim != ip->b->dim ) error(E_SIZES,"iter_cgs"); if ( ip->eps <= 0.0 ) ip->eps = MACHEPS; p = v_resize(p,ip->b->dim); q = v_resize(q,ip->b->dim); r = v_resize(r,ip->b->dim); u = v_resize(u,ip->b->dim); v = v_resize(v,ip->b->dim); MEM_STAT_REG(p,TYPE_VEC); MEM_STAT_REG(q,TYPE_VEC); MEM_STAT_REG(r,TYPE_VEC); MEM_STAT_REG(u,TYPE_VEC); MEM_STAT_REG(v,TYPE_VEC); if (ip->Bx) { z = v_resize(z,ip->b->dim); MEM_STAT_REG(z,TYPE_VEC); } if (ip->x != VNULL) { if (ip->x->dim != ip->b->dim) error(E_SIZES,"iter_cgs"); ip->Ax(ip->A_par,ip->x,v); /* v = A*x */ if (ip->Bx) { v_sub(ip->b,v,v); /* v = b - A*x */ (ip->Bx)(ip->B_par,v,r); /* r = B*(b-A*x) */ } else v_sub(ip->b,v,r); /* r = b-A*x */ } else { /* ip->x == 0 */ ip->x = v_get(ip->b->dim); /* x == 0 */ ip->shared_x = FALSE; if (ip->Bx) (ip->Bx)(ip->B_par,ip->b,r); /* r = B*b */ else v_copy(ip->b,r); /* r = b */ } v_zero(p); v_zero(q); old_rho = 1.0; for (ip->steps = 0; ip->steps <= ip->limit; ip->steps++) { inner = in_prod(r,r); nres = sqrt(fabs(inner)); if (ip->steps == 0) ip->init_res = nres; if (ip->info) ip->info(ip,nres,r,VNULL); if ( ip->stop_crit(ip,nres,r,VNULL) ) break; rho = in_prod(r0,r); if ( old_rho == 0.0 ) error(E_BREAKDOWN,"iter_cgs"); beta = rho/old_rho; v_mltadd(r,q,beta,u); v_mltadd(q,p,beta,v); v_mltadd(u,v,beta,p); (ip->Ax)(ip->A_par,p,q); if (ip->Bx) { (ip->Bx)(ip->B_par,q,z); tmp = z; } else tmp = q; sigma = in_prod(r0,tmp); if ( sigma == 0.0 ) error(E_BREAKDOWN,"iter_cgs"); alpha = rho/sigma; v_mltadd(u,tmp,-alpha,q); v_add(u,q,v); (ip->Ax)(ip->A_par,v,u); if (ip->Bx) { (ip->Bx)(ip->B_par,u,z); tmp = z; } else tmp = u; v_mltadd(r,tmp,-alpha,r); v_mltadd(ip->x,v,alpha,ip->x); old_rho = rho; } #ifdef THREADSAFE V_FREE(p); V_FREE(q); V_FREE(r); V_FREE(u); V_FREE(v); V_FREE(z); #endif return ip->x; }
VEC *iter_lsqr(ITER *ip) #endif { STATIC VEC *u = VNULL, *v = VNULL, *w = VNULL, *tmp = VNULL; Real alpha, beta, phi, phi_bar; Real rho, rho_bar, rho_max, theta, nres; Real s, c; /* for Givens' rotations */ int m, n; if ( ! ip || ! ip->b || !ip->Ax || !ip->ATx ) error(E_NULL,"iter_lsqr"); if ( ip->x == ip->b ) error(E_INSITU,"iter_lsqr"); if (!ip->stop_crit || !ip->x) error(E_NULL,"iter_lsqr"); if ( ip->eps <= 0.0 ) ip->eps = MACHEPS; m = ip->b->dim; n = ip->x->dim; u = v_resize(u,(unsigned int)m); v = v_resize(v,(unsigned int)n); w = v_resize(w,(unsigned int)n); tmp = v_resize(tmp,(unsigned int)n); MEM_STAT_REG(u,TYPE_VEC); MEM_STAT_REG(v,TYPE_VEC); MEM_STAT_REG(w,TYPE_VEC); MEM_STAT_REG(tmp,TYPE_VEC); if (ip->x != VNULL) { ip->Ax(ip->A_par,ip->x,u); /* u = A*x */ v_sub(ip->b,u,u); /* u = b-A*x */ } else { /* ip->x == 0 */ ip->x = v_get(ip->b->dim); ip->shared_x = FALSE; v_copy(ip->b,u); /* u = b */ } beta = v_norm2(u); if ( beta == 0.0 ) return ip->x; sv_mlt(1.0/beta,u,u); (ip->ATx)(ip->AT_par,u,v); alpha = v_norm2(v); if ( alpha == 0.0 ) return ip->x; sv_mlt(1.0/alpha,v,v); v_copy(v,w); phi_bar = beta; rho_bar = alpha; rho_max = 1.0; for (ip->steps = 0; ip->steps <= ip->limit; ip->steps++) { tmp = v_resize(tmp,m); (ip->Ax)(ip->A_par,v,tmp); v_mltadd(tmp,u,-alpha,u); beta = v_norm2(u); sv_mlt(1.0/beta,u,u); tmp = v_resize(tmp,n); (ip->ATx)(ip->AT_par,u,tmp); v_mltadd(tmp,v,-beta,v); alpha = v_norm2(v); sv_mlt(1.0/alpha,v,v); rho = sqrt(rho_bar*rho_bar+beta*beta); if ( rho > rho_max ) rho_max = rho; c = rho_bar/rho; s = beta/rho; theta = s*alpha; rho_bar = -c*alpha; phi = c*phi_bar; phi_bar = s*phi_bar; /* update ip->x & w */ if ( rho == 0.0 ) error(E_BREAKDOWN,"iter_lsqr"); v_mltadd(ip->x,w,phi/rho,ip->x); v_mltadd(v,w,-theta/rho,w); nres = fabs(phi_bar*alpha*c)*rho_max; if (ip->info) ip->info(ip,nres,w,VNULL); if (ip->steps == 0) ip->init_res = nres; if ( ip->stop_crit(ip,nres,w,VNULL) ) break; } #ifdef THREADSAFE V_FREE(u); V_FREE(v); V_FREE(w); V_FREE(tmp); #endif return ip->x; }
VEC *iter_cgne(ITER *ip) #endif { STATIC VEC *r = VNULL, *p = VNULL, *q = VNULL, *z = VNULL; Real alpha, beta, inner, old_inner, nres; VEC *rr1; /* pointer only */ if (ip == INULL) error(E_NULL,"iter_cgne"); if (!ip->Ax || ! ip->ATx || !ip->b) error(E_NULL,"iter_cgne"); if ( ip->x == ip->b ) error(E_INSITU,"iter_cgne"); if (!ip->stop_crit) error(E_NULL,"iter_cgne"); if ( ip->eps <= 0.0 ) ip->eps = MACHEPS; r = v_resize(r,ip->b->dim); p = v_resize(p,ip->b->dim); q = v_resize(q,ip->b->dim); MEM_STAT_REG(r,TYPE_VEC); MEM_STAT_REG(p,TYPE_VEC); MEM_STAT_REG(q,TYPE_VEC); z = v_resize(z,ip->b->dim); MEM_STAT_REG(z,TYPE_VEC); if (ip->x) { if (ip->x->dim != ip->b->dim) error(E_SIZES,"iter_cgne"); ip->Ax(ip->A_par,ip->x,p); /* p = A*x */ v_sub(ip->b,p,z); /* z = b - A*x */ } else { /* ip->x == 0 */ ip->x = v_get(ip->b->dim); ip->shared_x = FALSE; v_copy(ip->b,z); } rr1 = z; if (ip->Bx) { (ip->Bx)(ip->B_par,rr1,p); rr1 = p; } (ip->ATx)(ip->AT_par,rr1,r); /* r = A^T*B*(b-A*x) */ old_inner = 0.0; for ( ip->steps = 0; ip->steps <= ip->limit; ip->steps++ ) { rr1 = r; if ( ip->Bx ) { (ip->Bx)(ip->B_par,r,z); /* rr = B*r */ rr1 = z; } inner = in_prod(r,rr1); nres = sqrt(fabs(inner)); if (ip->info) ip->info(ip,nres,r,rr1); if (ip->steps == 0) ip->init_res = nres; if ( ip->stop_crit(ip,nres,r,rr1) ) break; if ( ip->steps ) /* if ( ip->steps > 0 ) ... */ { beta = inner/old_inner; p = v_mltadd(rr1,p,beta,p); } else /* if ( ip->steps == 0 ) ... */ { beta = 0.0; p = v_copy(rr1,p); old_inner = 0.0; } (ip->Ax)(ip->A_par,p,q); /* q = A*p */ if (ip->Bx) { (ip->Bx)(ip->B_par,q,z); (ip->ATx)(ip->AT_par,z,q); rr1 = q; /* q = A^T*B*A*p */ } else { (ip->ATx)(ip->AT_par,q,z); /* z = A^T*A*p */ rr1 = z; } alpha = inner/in_prod(rr1,p); v_mltadd(ip->x,p,alpha,ip->x); v_mltadd(r,rr1,-alpha,r); old_inner = inner; } #ifdef THREADSAFE V_FREE(r); V_FREE(p); V_FREE(q); V_FREE(z); #endif return ip->x; }
// Main int main(int argc, char ** argv) { // Choose the best GPU in case there are multiple available choose_GPU(); // Keep track of the start time of the program long long program_start_time = get_time(); if (argc !=3){ fprintf(stderr, "usage: %s <input file> <number of frames to process>", argv[0]); exit(1); } // Let the user specify the number of frames to process int num_frames = atoi(argv[2]); // Open video file char *video_file_name = argv[1]; avi_t *cell_file = AVI_open_input_file(video_file_name, 1); if (cell_file == NULL) { AVI_print_error("Error with AVI_open_input_file"); return -1; } int i, j, *crow, *ccol, pair_counter = 0, x_result_len = 0, Iter = 20, ns = 4, k_count = 0, n; MAT *cellx, *celly, *A; double *GICOV_spots, *t, *G, *x_result, *y_result, *V, *QAX_CENTERS, *QAY_CENTERS; double threshold = 1.8, radius = 10.0, delta = 3.0, dt = 0.01, b = 5.0; // Extract a cropped version of the first frame from the video file MAT *image_chopped = get_frame(cell_file, 0, 1, 0); printf("Detecting cells in frame 0\n"); // Get gradient matrices in x and y directions MAT *grad_x = gradient_x(image_chopped); MAT *grad_y = gradient_y(image_chopped); // Allocate for gicov_mem and strel gicov_mem = (float*) malloc(sizeof(float) * grad_x->m * grad_y->n); strel = (float*) malloc(sizeof(float) * strel_m * strel_n); m_free(image_chopped); int grad_m = grad_x->m; int grad_n = grad_y->n; #pragma acc data create(sin_angle,cos_angle,theta,tX,tY) \ create(gicov_mem[0:grad_x->m*grad_y->n]) { // Precomputed constants on GPU compute_constants(); // Get GICOV matrices corresponding to image gradients long long GICOV_start_time = get_time(); MAT *gicov = GICOV(grad_x, grad_y); long long GICOV_end_time = get_time(); // Dilate the GICOV matrices long long dilate_start_time = get_time(); MAT *img_dilated = dilate(gicov); long long dilate_end_time = get_time(); } /* end acc data */ // Find possible matches for cell centers based on GICOV and record the rows/columns in which they are found pair_counter = 0; crow = (int *) malloc(gicov->m * gicov->n * sizeof(int)); ccol = (int *) malloc(gicov->m * gicov->n * sizeof(int)); for(i = 0; i < gicov->m; i++) { for(j = 0; j < gicov->n; j++) { if(!double_eq(m_get_val(gicov,i,j), 0.0) && double_eq(m_get_val(img_dilated,i,j), m_get_val(gicov,i,j))) { crow[pair_counter]=i; ccol[pair_counter]=j; pair_counter++; } } } GICOV_spots = (double *) malloc(sizeof(double) * pair_counter); for(i = 0; i < pair_counter; i++) GICOV_spots[i] = sqrt(m_get_val(gicov, crow[i], ccol[i])); G = (double *) calloc(pair_counter, sizeof(double)); x_result = (double *) calloc(pair_counter, sizeof(double)); y_result = (double *) calloc(pair_counter, sizeof(double)); x_result_len = 0; for (i = 0; i < pair_counter; i++) { if ((crow[i] > 29) && (crow[i] < BOTTOM - TOP + 39)) { x_result[x_result_len] = ccol[i]; y_result[x_result_len] = crow[i] - 40; G[x_result_len] = GICOV_spots[i]; x_result_len++; } } // Make an array t which holds each "time step" for the possible cells t = (double *) malloc(sizeof(double) * 36); for (i = 0; i < 36; i++) { t[i] = (double)i * 2.0 * PI / 36.0; } // Store cell boundaries (as simple circles) for all cells cellx = m_get(x_result_len, 36); celly = m_get(x_result_len, 36); for(i = 0; i < x_result_len; i++) { for(j = 0; j < 36; j++) { m_set_val(cellx, i, j, x_result[i] + radius * cos(t[j])); m_set_val(celly, i, j, y_result[i] + radius * sin(t[j])); } } A = TMatrix(9,4); V = (double *) malloc(sizeof(double) * pair_counter); QAX_CENTERS = (double * )malloc(sizeof(double) * pair_counter); QAY_CENTERS = (double *) malloc(sizeof(double) * pair_counter); memset(V, 0, sizeof(double) * pair_counter); memset(QAX_CENTERS, 0, sizeof(double) * pair_counter); memset(QAY_CENTERS, 0, sizeof(double) * pair_counter); // For all possible results, find the ones that are feasibly leukocytes and store their centers k_count = 0; for (n = 0; n < x_result_len; n++) { if ((G[n] < -1 * threshold) || G[n] > threshold) { MAT * x, *y; VEC * x_row, * y_row; x = m_get(1, 36); y = m_get(1, 36); x_row = v_get(36); y_row = v_get(36); // Get current values of possible cells from cellx/celly matrices x_row = get_row(cellx, n, x_row); y_row = get_row(celly, n, y_row); uniformseg(x_row, y_row, x, y); // Make sure that the possible leukocytes are not too close to the edge of the frame if ((m_min(x) > b) && (m_min(y) > b) && (m_max(x) < cell_file->width - b) && (m_max(y) < cell_file->height - b)) { MAT * Cx, * Cy, *Cy_temp, * Ix1, * Iy1; VEC *Xs, *Ys, *W, *Nx, *Ny, *X, *Y; Cx = m_get(1, 36); Cy = m_get(1, 36); Cx = mmtr_mlt(A, x, Cx); Cy = mmtr_mlt(A, y, Cy); Cy_temp = m_get(Cy->m, Cy->n); for (i = 0; i < 9; i++) m_set_val(Cy, i, 0, m_get_val(Cy, i, 0) + 40.0); // Iteratively refine the snake/spline for (i = 0; i < Iter; i++) { int typeofcell; if(G[n] > 0.0) typeofcell = 0; else typeofcell = 1; splineenergyform01(Cx, Cy, grad_x, grad_y, ns, delta, 2.0 * dt, typeofcell); } X = getsampling(Cx, ns); for (i = 0; i < Cy->m; i++) m_set_val(Cy_temp, i, 0, m_get_val(Cy, i, 0) - 40.0); Y = getsampling(Cy_temp, ns); Ix1 = linear_interp2(grad_x, X, Y); Iy1 = linear_interp2(grad_x, X, Y); Xs = getfdriv(Cx, ns); Ys = getfdriv(Cy, ns); Nx = v_get(Ys->dim); for (i = 0; i < Ys->dim; i++) v_set_val(Nx, i, v_get_val(Ys, i) / sqrt(v_get_val(Xs, i)*v_get_val(Xs, i) + v_get_val(Ys, i)*v_get_val(Ys, i))); Ny = v_get(Xs->dim); for (i = 0; i < Xs->dim; i++) v_set_val(Ny, i, -1.0 * v_get_val(Xs, i) / sqrt(v_get_val(Xs, i)*v_get_val(Xs, i) + v_get_val(Ys, i)*v_get_val(Ys, i))); W = v_get(Nx->dim); for (i = 0; i < Nx->dim; i++) v_set_val(W, i, m_get_val(Ix1, 0, i) * v_get_val(Nx, i) + m_get_val(Iy1, 0, i) * v_get_val(Ny, i)); V[n] = mean(W) / std_dev(W); // Find the cell centers by computing the means of X and Y values for all snaxels of the spline contour QAX_CENTERS[k_count] = mean(X); QAY_CENTERS[k_count] = mean(Y) + TOP; k_count++; // Free memory v_free(W); v_free(Ny); v_free(Nx); v_free(Ys); v_free(Xs); m_free(Iy1); m_free(Ix1); v_free(Y); v_free(X); m_free(Cy_temp); m_free(Cy); m_free(Cx); } // Free memory v_free(y_row); v_free(x_row); m_free(y); m_free(x); } } // Free memory free(gicov_mem); free(strel); free(V); free(ccol); free(crow); free(GICOV_spots); free(t); free(G); free(x_result); free(y_result); m_free(A); m_free(celly); m_free(cellx); m_free(img_dilated); m_free(gicov); m_free(grad_y); m_free(grad_x); // Report the total number of cells detected printf("Cells detected: %d\n\n", k_count); // Report the breakdown of the detection runtime printf("Detection runtime\n"); printf("-----------------\n"); printf("GICOV computation: %.5f seconds\n", ((float) (GICOV_end_time - GICOV_start_time)) / (1000*1000)); printf(" GICOV dilation: %.5f seconds\n", ((float) (dilate_end_time - dilate_start_time)) / (1000*1000)); printf(" Total: %.5f seconds\n", ((float) (get_time() - program_start_time)) / (1000*1000)); // Now that the cells have been detected in the first frame, // track the ellipses through subsequent frames if (num_frames > 1) printf("\nTracking cells across %d frames\n", num_frames); else printf("\nTracking cells across 1 frame\n"); long long tracking_start_time = get_time(); int num_snaxels = 20; ellipsetrack(cell_file, QAX_CENTERS, QAY_CENTERS, k_count, radius, num_snaxels, num_frames); printf(" Total: %.5f seconds\n", ((float) (get_time() - tracking_start_time)) / (float) (1000*1000*num_frames)); // Report total program execution time printf("\nTotal application run time: %.5f seconds\n", ((float) (get_time() - program_start_time)) / (1000*1000)); return 0; }
//FIXME: add LOCALFUNC? (OR GLOBAL?) BYTE* RECURSIVE_SUBROUTINE(BYTE t, list *cur_guess, list *guesses, list_node *first_guess) { if ( t == key_length) { unsigned char* key=NULL; // generate key from the cur_guess (push guesses into a matrix and use some matrix solving library?) MAT* A; VEC *x,*b; PERM* pivot; A=m_get(key_length,key_length); b=v_get(key_length); x=v_get(key_length); int c=0,r=0; for(list_node* iter=list_head(cur_guess); iter != NULL && r<key_length; iter=list_node_next(iter)){ for(c=list_node_data_ptr(iter,byte_sum_guess_t)->i1; c <= list_node_data_ptr(iter,byte_sum_guess_t)->i2; ++c){ A->me[r][c]=1; } b->ve[r]=list_node_data_ptr(iter,byte_sum_guess_t)->value; ++r; } //Calculate matrix determinant SQRMATRIX A_det; SQRMATRIX_CreateMatrix(&A_det,key_length); for(r=0;r<key_length;++r){ for(c=0;c<key_length;++c){ A_det.array[r][c]=A->me[r][c]; } } int det; det=SQRMATRIX_CalcDeterminant(&A_det); //TODO: return this later SQRMATRIX_DestroyMatrix(&A_det); if(det==0){//If determinant is 0 continue to next guess ++count_bad_matrix; #ifdef __DEBUG //SQRMATRIX_DisplayMatrix(&A_det); v_output(b); #endif DEBUG_PRINT("Matrix determinant is 0\n"); }else{ ++count_guesses; pivot = px_get(A->m); LUfactor(A,pivot); x=LUsolve(A,pivot,b,VNULL); PX_FREE(pivot); //test key (use our RC4 impl) key=(unsigned char*)malloc(sizeof(unsigned char)*key_length); for(int i=0;i<key_length;++i){ key[i]=x->ve[i]; } int res=rc4_test_key(key); if(res){ printf("MAZAL TOV! we got the right key.\n"); print_key(key); printf("\n"); }else{ printf("Tried key: "); print_key(key); printf("\n"); free(key);key=NULL; } } //release matrix vars M_FREE(A); V_FREE(x); V_FREE(b); return key; } byte_sum_guess_t cur; //list *new_list_head=guesses; //TODO: (later) add a for loop running along the "lambeda_t" values here, for the initial impl we'll try the best guess //for () //{ for(int i=0; i<LAMBDA_T; ++i){ cur = *(list_node_data_ptr(first_guess, byte_sum_guess_t)); list_node *biatch = list_add_head(cur_guess, &cur); BYTE* res=RECURSIVE_SUBROUTINE(t+1, cur_guess, guesses, list_node_next(first_guess)); if(res!=NULL){ return res; } list_del(cur_guess,biatch); first_guess = list_node_next(first_guess); } return NULL; //TODO: do something to find the next guess and link it to the current guess //when I say something I mean find best guess (i.e best weight) of all the guesses that give us new information //(i.e not linearily dependent in our byte values and sums matrix that can be deduced from the cur_guess) //see also the note above (intuition) //IMPORTANT! //explaination how cur_guess is a matrix: each entry in cur_guess contains a list of bytes that are part of the sum and a //guess as to the value of the sum. if this matrix is solvable then solving it should give us a value for each byte of the //key thus the entire key //note: we probably should change cur_guess to a smarter database (for example a (L)x(L+1) matrix as an array?) but we //need to consider that we need to keep the ability to backtrack without making it too expensive //TODO: if weight of the guess is too small -> return FAIL (section 4.6) //These are based on section 4.4 //correct suggestions (this can be done later, basic alg should work without it) //adjust weights (this can be done later, basic alg should work without it) //merge counters (section 4.2), also skip for initial impl? need to check //go to next iteration in the recurtion //RECURSIVE_SUBROUTINE(t+1, cur_guess, ); //} end of for }
gpointer mesch_vec_new(gint i) { return(v_get(i)); }
VEC *iter_gmres(ITER *ip) #endif { STATIC VEC *u=VNULL, *r=VNULL, *rhs = VNULL; STATIC VEC *givs=VNULL, *givc=VNULL, *z = VNULL; STATIC MAT *Q = MNULL, *R = MNULL; VEC *rr, v, v1; /* additional pointers (not real vectors) */ int i,j, done; Real nres; /* Real last_h; */ if (ip == INULL) error(E_NULL,"iter_gmres"); if ( ! ip->Ax || ! ip->b ) error(E_NULL,"iter_gmres"); if ( ! ip->stop_crit ) error(E_NULL,"iter_gmres"); if ( ip->k <= 0 ) error(E_BOUNDS,"iter_gmres"); if (ip->x != VNULL && ip->x->dim != ip->b->dim) error(E_SIZES,"iter_gmres"); if (ip->eps <= 0.0) ip->eps = MACHEPS; r = v_resize(r,ip->k+1); u = v_resize(u,ip->b->dim); rhs = v_resize(rhs,ip->k+1); givs = v_resize(givs,ip->k); /* Givens rotations */ givc = v_resize(givc,ip->k); MEM_STAT_REG(r,TYPE_VEC); MEM_STAT_REG(u,TYPE_VEC); MEM_STAT_REG(rhs,TYPE_VEC); MEM_STAT_REG(givs,TYPE_VEC); MEM_STAT_REG(givc,TYPE_VEC); R = m_resize(R,ip->k+1,ip->k); Q = m_resize(Q,ip->k,ip->b->dim); MEM_STAT_REG(R,TYPE_MAT); MEM_STAT_REG(Q,TYPE_MAT); if (ip->x == VNULL) { /* ip->x == 0 */ ip->x = v_get(ip->b->dim); ip->shared_x = FALSE; } v.dim = v.max_dim = ip->b->dim; /* v and v1 are pointers to rows */ v1.dim = v1.max_dim = ip->b->dim; /* of matrix Q */ if (ip->Bx != (Fun_Ax)NULL) { /* if precondition is defined */ z = v_resize(z,ip->b->dim); MEM_STAT_REG(z,TYPE_VEC); } done = FALSE; for (ip->steps = 0; ip->steps < ip->limit; ) { /* restart */ ip->Ax(ip->A_par,ip->x,u); /* u = A*x */ v_sub(ip->b,u,u); /* u = b - A*x */ rr = u; /* rr is a pointer only */ if (ip->Bx) { (ip->Bx)(ip->B_par,u,z); /* tmp = B*(b-A*x) */ rr = z; } nres = v_norm2(rr); if (ip->steps == 0) { if (ip->info) ip->info(ip,nres,VNULL,VNULL); ip->init_res = nres; } if ( nres == 0.0 ) { done = TRUE; break; } v.ve = Q->me[0]; sv_mlt(1.0/nres,rr,&v); v_zero(r); v_zero(rhs); rhs->ve[0] = nres; for ( i = 0; i < ip->k && ip->steps < ip->limit; i++ ) { ip->steps++; v.ve = Q->me[i]; (ip->Ax)(ip->A_par,&v,u); rr = u; if (ip->Bx) { (ip->Bx)(ip->B_par,u,z); rr = z; } if (i < ip->k - 1) { v1.ve = Q->me[i+1]; v_copy(rr,&v1); for (j = 0; j <= i; j++) { v.ve = Q->me[j]; /* r->ve[j] = in_prod(&v,rr); */ /* modified Gram-Schmidt algorithm */ r->ve[j] = in_prod(&v,&v1); v_mltadd(&v1,&v,-r->ve[j],&v1); } r->ve[i+1] = nres = v_norm2(&v1); if (nres <= MACHEPS*ip->init_res) { for (j = 0; j < i; j++) rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r); set_col(R,i,r); done = TRUE; break; } sv_mlt(1.0/nres,&v1,&v1); } else { /* i == ip->k - 1 */ /* Q->me[ip->k] need not be computed */ for (j = 0; j <= i; j++) { v.ve = Q->me[j]; r->ve[j] = in_prod(&v,rr); } nres = in_prod(rr,rr) - in_prod(r,r); if (sqrt(fabs(nres)) <= MACHEPS*ip->init_res) { for (j = 0; j < i; j++) rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r); set_col(R,i,r); done = TRUE; break; } if (nres < 0.0) { /* do restart */ i--; ip->steps--; break; } r->ve[i+1] = sqrt(nres); } /* QR update */ /* last_h = r->ve[i+1]; */ /* for test only */ for (j = 0; j < i; j++) rot_vec(r,j,j+1,givc->ve[j],givs->ve[j],r); givens(r->ve[i],r->ve[i+1],&givc->ve[i],&givs->ve[i]); rot_vec(r,i,i+1,givc->ve[i],givs->ve[i],r); rot_vec(rhs,i,i+1,givc->ve[i],givs->ve[i],rhs); set_col(R,i,r); nres = fabs((double) rhs->ve[i+1]); if (ip->info) ip->info(ip,nres,VNULL,VNULL); if ( ip->stop_crit(ip,nres,VNULL,VNULL) ) { done = TRUE; break; } } /* use ixi submatrix of R */ if (i >= ip->k) i = ip->k - 1; R = m_resize(R,i+1,i+1); rhs = v_resize(rhs,i+1); /* test only */ /* test_gmres(ip,i,Q,R,givc,givs,last_h); */ Usolve(R,rhs,rhs,0.0); /* solve a system: R*x = rhs */ /* new approximation */ for (j = 0; j <= i; j++) { v.ve = Q->me[j]; v_mltadd(ip->x,&v,rhs->ve[j],ip->x); } if (done) break; /* back to old dimensions */ rhs = v_resize(rhs,ip->k+1); R = m_resize(R,ip->k+1,ip->k); } #ifdef THREADSAFE V_FREE(u); V_FREE(r); V_FREE(rhs); V_FREE(givs); V_FREE(givc); V_FREE(z); M_FREE(Q); M_FREE(R); #endif return ip->x; }
/* LUfactor -- gaussian elimination with scaled partial pivoting -- Note: returns LU matrix which is A */ MAT *LUfactor(MAT *A, PERM *pivot) { unsigned int i, j, m, n; unsigned int k, k_max; int i_max; char MatrixTempBuffer[ 1000 ]; MatrixReal **A_v, *A_piv, *A_row; MatrixReal max1, temp, tiny; VEC *scale = VNULL; if ( A==(MAT *)NULL || pivot==(PERM *)NULL ){ error(E_NULL,"LUfactor"); } if ( pivot->size != A->m ) error(E_SIZES,"LUfactor"); m = A->m; n = A->n; if( SET_VEC_SIZE( A->m ) <1000 ) vec_get( &scale, (void *)MatrixTempBuffer, A->m ); else scale = v_get( A->m ); //MEM_STAT_REG(scale,TYPE_VEC); A_v = A->me; tiny = (MatrixReal)(10.0/HUGE_VAL); /* initialise pivot with identity permutation */ for ( i=0; i<m; i++ ) pivot->pe[i] = i; /* set scale parameters */ for ( i=0; i<m; i++ ) { max1 = 0.0; for ( j=0; j<n; j++ ) { temp = (MatrixReal)fabs(A_v[i][j]); max1 = mat_max(max1,temp); } scale->ve[i] = max1; } /* main loop */ k_max = mat_min(m,n)-1; for ( k=0; k<k_max; k++ ) { /* find best pivot row */ max1 = 0.0; i_max = -1; for ( i=k; i<m; i++ ) if ( fabs(scale->ve[i]) >= tiny*fabs(A_v[i][k]) ) { temp = (MatrixReal)fabs(A_v[i][k])/scale->ve[i]; if ( temp > max1 ) { max1 = temp; i_max = i; } } /* if no pivot then ignore column k... */ if ( i_max == -1 ) { /* set pivot entry A[k][k] exactly to zero, rather than just "small" */ A_v[k][k] = 0.0; continue; } /* do we pivot ? */ if ( i_max != (int)k ) /* yes we do... */ { px_transp(pivot,i_max,k); for ( j=0; j<n; j++ ) { temp = A_v[i_max][j]; A_v[i_max][j] = A_v[k][j]; A_v[k][j] = temp; } } /* row operations */ for ( i=k+1; i<m; i++ ) /* for each row do... */ { /* Note: divide by zero should never happen */ temp = A_v[i][k] = A_v[i][k]/A_v[k][k]; A_piv = &(A_v[k][k+1]); A_row = &(A_v[i][k+1]); if ( k+1 < n ) __mltadd__(A_row,A_piv,-temp,(int)(n-(k+1))); } } if( scale != (VEC *)MatrixTempBuffer ) // память выделялась, надо освободить V_FREE(scale); return A; }
void ellipseevolve(MAT *f, double *xc0, double *yc0, double *r0, double *t, int Np, double Er, double Ey) { /* % ELLIPSEEVOLVE evolves a parametric snake according % to some energy constraints. % % INPUTS: % f............potential surface % xc0,yc0......initial center position % r0,t.........initial radii & angle vectors (with Np elements each) % Np...........number of snaxel points per snake % Er...........expected radius % Ey...........expected y position % % OUTPUTS % xc0,yc0.......final center position % r0...........final radii % % Matlab code written by: DREW GILLIAM (based on work by GANG DONG / % NILANJAN RAY) % Ported to C by: MICHAEL BOYER */ // Constants double deltax = 0.2; double deltay = 0.2; double deltar = 0.2; double converge = 0.1; double lambdaedge = 1; double lambdasize = 0.2; double lambdapath = 0.05; int iterations = 1000; // maximum number of iterations int i, j; // Initialize variables double xc = *xc0; double yc = *yc0; double *r = (double *) malloc(sizeof(double) * Np); for (i = 0; i < Np; i++) r[i] = r0[i]; // Compute the x- and y-gradients of the MGVF matrix MAT *fx = gradient_x(f); MAT *fy = gradient_y(f); // Normalize the gradients int fh = f->m, fw = f->n; for (i = 0; i < fh; i++) { for (j = 0; j < fw; j++) { double temp_x = m_get_val(fx, i, j); double temp_y = m_get_val(fy, i, j); double fmag = sqrt((temp_x * temp_x) + (temp_y * temp_y)); m_set_val(fx, i, j, temp_x / fmag); m_set_val(fy, i, j, temp_y / fmag); } } double *r_old = (double *) malloc(sizeof(double) * Np); VEC *x = v_get(Np); VEC *y = v_get(Np); // Evolve the snake int iter = 0; double snakediff = 1.0; while (iter < iterations && snakediff > converge) { // Save the values from the previous iteration double xc_old = xc, yc_old = yc; for (i = 0; i < Np; i++) { r_old[i] = r[i]; } // Compute the locations of the snaxels for (i = 0; i < Np; i++) { v_set_val(x, i, xc + r[i] * cos(t[i])); v_set_val(y, i, yc + r[i] * sin(t[i])); } // See if any of the points in the snake are off the edge of the image double min_x = v_get_val(x, 0), max_x = v_get_val(x, 0); double min_y = v_get_val(y, 0), max_y = v_get_val(y, 0); for (i = 1; i < Np; i++) { double x_i = v_get_val(x, i); if (x_i < min_x) min_x = x_i; else if (x_i > max_x) max_x = x_i; double y_i = v_get_val(y, i); if (y_i < min_y) min_y = y_i; else if (y_i > max_y) max_y = y_i; } if (min_x < 0.0 || max_x > (double) fw - 1.0 || min_y < 0 || max_y > (double) fh - 1.0) break; // Compute the length of the snake double L = 0.0; for (i = 0; i < Np - 1; i++) { double diff_x = v_get_val(x, i + 1) - v_get_val(x, i); double diff_y = v_get_val(y, i + 1) - v_get_val(y, i); L += sqrt((diff_x * diff_x) + (diff_y * diff_y)); } double diff_x = v_get_val(x, 0) - v_get_val(x, Np - 1); double diff_y = v_get_val(y, 0) - v_get_val(y, Np - 1); L += sqrt((diff_x * diff_x) + (diff_y * diff_y)); // Compute the potential surface at each snaxel MAT *vf = linear_interp2(f, x, y); MAT *vfx = linear_interp2(fx, x, y); MAT *vfy = linear_interp2(fy, x, y); // Compute the average potential surface around the snake double vfmean = sum_m(vf ) / L; double vfxmean = sum_m(vfx) / L; double vfymean = sum_m(vfy) / L; // Compute the radial potential surface int m = vf->m, n = vf->n; MAT *vfr = m_get(m, n); for (i = 0; i < n; i++) { double vf_val = m_get_val(vf, 0, i); double vfx_val = m_get_val(vfx, 0, i); double vfy_val = m_get_val(vfy, 0, i); double x_val = v_get_val(x, i); double y_val = v_get_val(y, i); double new_val = (vf_val + vfx_val * (x_val - xc) + vfy_val * (y_val - yc) - vfmean) / L; m_set_val(vfr, 0, i, new_val); } // Update the snake center and snaxels xc = xc + (deltax * lambdaedge * vfxmean); yc = (yc + (deltay * lambdaedge * vfymean) + (deltay * lambdapath * Ey)) / (1.0 + deltay * lambdapath); double r_diff = 0.0; for (i = 0; i < Np; i++) { r[i] = (r[i] + (deltar * lambdaedge * m_get_val(vfr, 0, i)) + (deltar * lambdasize * Er)) / (1.0 + deltar * lambdasize); r_diff += fabs(r[i] - r_old[i]); } // Test for convergence snakediff = fabs(xc - xc_old) + fabs(yc - yc_old) + r_diff; // Free temporary matrices m_free(vf); m_free(vfx); m_free(vfy); m_free(vfr); iter++; } // Set the return values *xc0 = xc; *yc0 = yc; for (i = 0; i < Np; i++) r0[i] = r[i]; // Free memory free(r); free(r_old); v_free( x); v_free( y); m_free(fx); m_free(fy); }
VEC *iter_mgcr(ITER *ip) #endif { STATIC VEC *As=VNULL, *beta=VNULL, *alpha=VNULL, *z=VNULL; STATIC MAT *N=MNULL, *H=MNULL; VEC *rr, v, s; /* additional pointer and structures */ Real nres; /* norm of a residual */ Real dd; /* coefficient d_i */ int i,j; int done; /* if TRUE then stop the iterative process */ int dim; /* dimension of the problem */ /* ip cannot be NULL */ if (ip == INULL) error(E_NULL,"mgcr"); /* Ax, b and stopping criterion must be given */ if (! ip->Ax || ! ip->b || ! ip->stop_crit) error(E_NULL,"mgcr"); /* at least one direction vector must exist */ if ( ip->k <= 0) error(E_BOUNDS,"mgcr"); /* if the vector x is given then b and x must have the same dimension */ if ( ip->x && ip->x->dim != ip->b->dim) error(E_SIZES,"mgcr"); if (ip->eps <= 0.0) ip->eps = MACHEPS; dim = ip->b->dim; As = v_resize(As,dim); alpha = v_resize(alpha,ip->k); beta = v_resize(beta,ip->k); MEM_STAT_REG(As,TYPE_VEC); MEM_STAT_REG(alpha,TYPE_VEC); MEM_STAT_REG(beta,TYPE_VEC); H = m_resize(H,ip->k,ip->k); N = m_resize(N,ip->k,dim); MEM_STAT_REG(H,TYPE_MAT); MEM_STAT_REG(N,TYPE_MAT); /* if a preconditioner is defined */ if (ip->Bx) { z = v_resize(z,dim); MEM_STAT_REG(z,TYPE_VEC); } /* if x is NULL then it is assumed that x has entries with value zero */ if ( ! ip->x ) { ip->x = v_get(ip->b->dim); ip->shared_x = FALSE; } /* v and s are additional pointers to rows of N */ /* they must have the same dimension as rows of N */ v.dim = v.max_dim = s.dim = s.max_dim = dim; done = FALSE; for (ip->steps = 0; ip->steps < ip->limit; ) { (*ip->Ax)(ip->A_par,ip->x,As); /* As = A*x */ v_sub(ip->b,As,As); /* As = b - A*x */ rr = As; /* rr is an additional pointer */ /* if a preconditioner is defined */ if (ip->Bx) { (*ip->Bx)(ip->B_par,As,z); /* z = B*(b-A*x) */ rr = z; } /* norm of the residual */ nres = v_norm2(rr); dd = nres; /* dd = ||r_i|| */ /* check if the norm of the residual is zero */ if (ip->steps == 0) { /* information for a user */ if (ip->info) (*ip->info)(ip,nres,As,rr); ip->init_res = fabs(nres); } if (nres == 0.0) { /* iterative process is finished */ done = TRUE; break; } /* save this residual in the first row of N */ v.ve = N->me[0]; v_copy(rr,&v); for (i = 0; i < ip->k && ip->steps < ip->limit; i++) { ip->steps++; v.ve = N->me[i]; /* pointer to a row of N (=s_i) */ /* note that we must use here &v, not v */ (*ip->Ax)(ip->A_par,&v,As); rr = As; /* As = A*s_i */ if (ip->Bx) { (*ip->Bx)(ip->B_par,As,z); /* z = B*A*s_i */ rr = z; } if (i < ip->k - 1) { s.ve = N->me[i+1]; /* pointer to a row of N (=s_{i+1}) */ v_copy(rr,&s); /* s_{i+1} = B*A*s_i */ for (j = 0; j <= i-1; j++) { v.ve = N->me[j+1]; /* pointer to a row of N (=s_{j+1}) */ /* beta->ve[j] = in_prod(&v,rr); */ /* beta_{j,i} */ /* modified Gram-Schmidt algorithm */ beta->ve[j] = in_prod(&v,&s); /* beta_{j,i} */ /* s_{i+1} -= beta_{j,i}*s_{j+1} */ v_mltadd(&s,&v,- beta->ve[j],&s); } /* beta_{i,i} = ||s_{i+1}||_2 */ beta->ve[i] = nres = v_norm2(&s); if ( nres <= MACHEPS*ip->init_res) { /* s_{i+1} == 0 */ i--; done = TRUE; break; } sv_mlt(1.0/nres,&s,&s); /* normalize s_{i+1} */ v.ve = N->me[0]; alpha->ve[i] = in_prod(&v,&s); /* alpha_i = (s_0 , s_{i+1}) */ } else { for (j = 0; j <= i-1; j++) { v.ve = N->me[j+1]; /* pointer to a row of N (=s_{j+1}) */ beta->ve[j] = in_prod(&v,rr); /* beta_{j,i} */ } nres = in_prod(rr,rr); /* rr = B*A*s_{k-1} */ for (j = 0; j <= i-1; j++) nres -= beta->ve[j]*beta->ve[j]; if (sqrt(fabs(nres)) <= MACHEPS*ip->init_res) { /* s_k is zero */ i--; done = TRUE; break; } if (nres < 0.0) { /* do restart */ i--; ip->steps--; break; } beta->ve[i] = sqrt(nres); /* beta_{k-1,k-1} */ v.ve = N->me[0]; alpha->ve[i] = in_prod(&v,rr); for (j = 0; j <= i-1; j++) alpha->ve[i] -= beta->ve[j]*alpha->ve[j]; alpha->ve[i] /= beta->ve[i]; /* alpha_{k-1} */ } set_col(H,i,beta); /* other method of computing dd */ /* if (fabs((double)alpha->ve[i]) > dd) { nres = - dd*dd + alpha->ve[i]*alpha->ve[i]; nres = sqrt((double) nres); if (ip->info) (*ip->info)(ip,-nres,VNULL,VNULL); break; } */ /* to avoid overflow/underflow in computing dd */ /* dd *= cos(asin((double)(alpha->ve[i]/dd))); */ nres = alpha->ve[i]/dd; if (fabs(nres-1.0) <= MACHEPS*ip->init_res) dd = 0.0; else { nres = 1.0 - nres*nres; if (nres < 0.0) { nres = sqrt((double) -nres); if (ip->info) (*ip->info)(ip,-dd*nres,VNULL,VNULL); break; } dd *= sqrt((double) nres); } if (ip->info) (*ip->info)(ip,dd,VNULL,VNULL); if ( ip->stop_crit(ip,dd,VNULL,VNULL) ) { /* stopping criterion is satisfied */ done = TRUE; break; } } /* end of for */ if (i >= ip->k) i = ip->k - 1; /* use (i+1) by (i+1) submatrix of H */ H = m_resize(H,i+1,i+1); alpha = v_resize(alpha,i+1); Usolve(H,alpha,alpha,0.0); /* c_i is saved in alpha */ for (j = 0; j <= i; j++) { v.ve = N->me[j]; v_mltadd(ip->x,&v,alpha->ve[j],ip->x); } if (done) break; /* stop the iterative process */ alpha = v_resize(alpha,ip->k); H = m_resize(H,ip->k,ip->k); } /* end of while */ #ifdef THREADSAFE V_FREE(As); V_FREE(beta); V_FREE(alpha); V_FREE(z); M_FREE(N); M_FREE(H); #endif return ip->x; /* return the solution */ }
/******************************************************************** ** ** Function: lspVQ () ** ** Description: ** Vector quantizes a set of int term filter coefficients ** using a multi-stage M-L tree search algorithm. ** ** Arguments: ** ** int16_t target[] : the target coefficients to be quantized (Q15/Q17) ** int16_t weight[] : weights for mse calculation (Q11) ** int16_t qout[] : the output array (Q15/Q17) ** int16_t codebook[] : codebooks, cb[0..numStages-1] (Q15/Q17) ** int16_t tos : the number of stages ** int16_t cb_size[] : codebook size (multistages) ** int16_t cb_index[] : codebook indeces; cb_index[0..numStages-1] ** (output) ** int16_t dim ** BOOLEAN flag ** ** Return value: None ** ***********************************************************************/ static void lspVQ(int16_t target[], int16_t weight[], int16_t qout[], const int16_t codebook[], int16_t tos, const int16_t cb_size[], int16_t cb_index[], int16_t dim, BOOLEAN flag) { register int16_t i, entry; register int16_t c1, s1; const int16_t *cdbk_ptr, *cdbk_ptr2, *ptr1; int16_t index[LSP_VQ_CAND][LSP_VQ_STAGES]; int16_t nextIndex[LSP_VQ_CAND][LSP_VQ_STAGES]; int16_t ncPrev; int16_t cand[LSP_VQ_CAND][2 * LPC_ORD]; int16_t max_dMin, dMin[LSP_VQ_CAND], distortion; int16_t *cand_target; int32_t L_temp; int16_t ptr_offset = 0; int16_t temp1, temp2; /*==================================================================* * Initialize the data before starting the tree search. * * - the number of candidates from the "previous" stage is set * * to 1 since there is no previous stage! * * - the candidate vector from the previous stage is set to zero * * - the list of indeces for each candidate is set to 1 * *==================================================================*/ for (i = 0; i < LSP_VQ_CAND; i++) { v_zap(cand[i], dim); v_zap(index[i], LSP_VQ_STAGES); v_zap(nextIndex[i], LSP_VQ_STAGES); } cand_target = v_get(dim); ncPrev = 1; /*==================================================================* * Now we start the search: * * For each stage * * For each candidate from the previous stage * * For each entry in the current stage codebook * * * add codebook vector to current candidate * * * compute the distortion with the target * * * retain candidate if one of the best so far * *==================================================================*/ cdbk_ptr = codebook; /* An observation for lspVQ() shows that if "flag" is FALSE, then we only */ /* need to keep track of the best one (instead of the best LSP_VQ_CAND, */ /* 8) cand[][] and index[][]. This has significant influence on */ /* execution speed. */ for (s1 = 0; s1 < tos; s1++) { /* set the distortions to huge values */ fill(dMin, SW_MAX, LSP_VQ_CAND); max_dMin = SW_MAX; /* Loop for each previous candidate selected, and try each entry */ for (c1 = 0; c1 < ncPrev; c1++) { ptr_offset = 0; /* cand_target[] is the target vector with cand[c1] removed. */ /* This moves some operations from the for-entry loop here. */ /* save_saturation(); */ v_equ(cand_target, target, dim); v_sub(cand_target, cand[c1], dim); /* restore_saturation(); */ for (entry = 0; entry < cb_size[s1]; entry++) { ptr1 = cdbk_ptr + ptr_offset; /* Pointer arithmetics. */ /* compute the distortion */ distortion = WeightedMSE(dim, weight, ptr1, cand_target, max_dMin); /*======================================================* * If the error for this entry is less than the worst * * retained candidate so far, keep it. Note that the * * error list is maintained in order of best to worst. * *=======================================================*/ if (distortion < max_dMin) { max_dMin = InsertCand(c1, s1, dMin, distortion, entry, nextIndex[0], index[0]); } ptr_offset = melpe_add(ptr_offset, dim); } } /* At this point ptr_offset is (cb_size[s1]*dim). */ /*==================================================================* * Compute the number of candidate vectors which we kept for the * * next stage. Note that if the size of the stages is less than * * the number of candidates, we build them up using all entries * * until we have kept numCand candidates. On the other hand, if * * flag is FALSE and (s1 == tos - 1), then we only need to use * * ncPrev = 1 because we only copy the best candidate before * * exiting lspVQ(). * *==================================================================*/ if (!flag && s1 == tos - 1) ncPrev = 1; else { /* ncPrev = Min(ncPrev*cb_size[s1], LSP_VQ_CAND) for regular */ /* loops, and ncPrev = Min(ncPrev*cb_size[s1], LSP_INP_CAND) for */ /* the last lap. Explanations are available near the end of this *//* function. */ L_temp = melpe_L_mult(ncPrev, cb_size[s1]); L_temp = melpe_L_shr(L_temp, 1); temp1 = melpe_extract_l(L_temp); /* temp1 = ncPrev * cb_size[s1] */ if (s1 == tos - 1) temp2 = LSP_INP_CAND; else temp2 = LSP_VQ_CAND; if (temp1 < temp2) ncPrev = temp1; else ncPrev = temp2; } /*==================================================================* * We now have the best indices for the stage just completed, so * * compute the new candidate vectors for the next stage... * *==================================================================*/ for (c1 = 0; c1 < ncPrev; c1++) { v_zap(cand[c1], dim); cdbk_ptr2 = codebook; temp1 = melpe_add(s1, 1); v_equ(index[c1], nextIndex[c1], temp1); for (i = 0; i < temp1; i++) { /* v_add(cand[c1], cdbk_ptr2 + index[c1][i]*dim, dim); */ L_temp = melpe_L_mult(index[c1][i], dim); L_temp = melpe_L_shr(L_temp, 1); temp2 = melpe_extract_l(L_temp); ptr1 = cdbk_ptr2 + temp2; v_add(cand[c1], ptr1, dim); /* cdbk_ptr2 += cb_size[i]*dim; */ L_temp = melpe_L_mult(cb_size[i], dim); L_temp = melpe_L_shr(L_temp, 1); temp2 = melpe_extract_l(L_temp); cdbk_ptr2 += temp2; } } /* cdbk_ptr += cb_size[s1] * dim; */ cdbk_ptr += ptr_offset; } /* Copy best candidate and indices into output. Here we use temp1 and */ /* temp2 to compute (c1*tos) and (c1*dim). */ /* Originally this function copies LSP_VQ_CAND (== 8) vectors before */ /* exiting if flag is TRUE. However, in the calling environment of */ /* lspVQ() when flag is passed in as TRUE, we only used LSP_INP_CAND */ /* (== 5). */ temp1 = 0; temp2 = 0; for (i = 0; i < ncPrev; i++) { v_equ(&(cb_index[temp1]), index[i], tos); v_equ(&qout[temp2], cand[i], dim); temp1 = melpe_add(temp1, tos); temp2 = melpe_add(temp2, dim); } v_free(cand_target); }