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);
}