static void kalman_correct(kalman_t *kf, float p, float v) { /* K = P * HT * inv(H * P * HT + R) */ m_mlt(kf->H, kf->P, kf->T0); mmtr_mlt(kf->T0, kf->H, kf->T1); m_add(kf->T1, kf->R, kf->T0); m_inverse(kf->T0, kf->T1); mmtr_mlt(kf->P, kf->H, kf->T0); m_mlt(kf->T0, kf->T1, kf->K); /* x = x + K * (z - H * x) */ mv_mlt(kf->H, kf->x, kf->t0); v_set_val(kf->z, 0, p); v_set_val(kf->z, 1, v); v_sub(kf->z, kf->t0, kf->t1); mv_mlt(kf->K, kf->t1, kf->t0); v_add(kf->x, kf->t0, kf->t1); v_copy(kf->t1, kf->x); /* P = (I - K * H) * P */ m_mlt(kf->K, kf->H, kf->T0); m_sub(kf->I, kf->T0, kf->T1); m_mlt(kf->T1, kf->P, kf->T0); m_copy(kf->T0, kf->P); }
MAT *makeHQ(MAT *H, VEC *diag, VEC *beta, MAT *Qout) #endif { int i, j, limit; STATIC VEC *tmp1 = VNULL, *tmp2 = VNULL; if ( H==(MAT *)NULL || diag==(VEC *)NULL || beta==(VEC *)NULL ) error(E_NULL,"makeHQ"); limit = H->m - 1; if ( diag->dim < limit || beta->dim < limit ) error(E_SIZES,"makeHQ"); if ( H->m != H->n ) error(E_SQUARE,"makeHQ"); Qout = m_resize(Qout,H->m,H->m); tmp1 = v_resize(tmp1,H->m); tmp2 = v_resize(tmp2,H->m); MEM_STAT_REG(tmp1,TYPE_VEC); MEM_STAT_REG(tmp2,TYPE_VEC); for ( i = 0; i < H->m; i++ ) { /* tmp1 = i'th basis vector */ for ( j = 0; j < H->m; j++ ) /* tmp1->ve[j] = 0.0; */ v_set_val(tmp1,j,0.0); /* tmp1->ve[i] = 1.0; */ v_set_val(tmp1,i,1.0); /* apply H/h transforms in reverse order */ for ( j = limit-1; j >= 0; j-- ) { get_col(H,(unsigned int)j,tmp2); /* tmp2->ve[j+1] = diag->ve[j]; */ v_set_val(tmp2,j+1,v_entry(diag,j)); hhtrvec(tmp2,beta->ve[j],j+1,tmp1,tmp1); } /* insert into Qout */ set_col(Qout,(unsigned int)i,tmp1); } #ifdef THREADSAFE V_FREE(tmp1); V_FREE(tmp2); #endif return (Qout); }
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); }
/* rot_vec -- apply Givens rotation to x's i & k components */ extern VEC *rot_vec(VEC *x, u_int i, u_int k, double c, double s, VEC *out) { Real temp; if ( x==VNULL ) error(E_NULL,"rot_vec"); if ( i >= x->dim || k >= x->dim ) error(E_RANGE,"rot_vec"); out = v_copy(x,out); /* temp = c*out->ve[i] + s*out->ve[k]; */ temp = c*v_entry(out,i) + s*v_entry(out,k); /* out->ve[k] = -s*out->ve[i] + c*out->ve[k]; */ v_set_val(out,k,-s*v_entry(out,i)+c*v_entry(out,k)); /* out->ve[i] = temp; */ v_set_val(out,i,temp); return (out); }
static void kalman_predict(kalman_t *kf, float a) { /* x = A * x + B * u */ v_set_val(kf->u, 0, a); mv_mlt(kf->A, kf->x, kf->t0); mv_mlt(kf->B, kf->u, kf->t1); v_add(kf->t0, kf->t1, kf->x); /* P = A * P * AT + Q */ m_mlt(kf->A, kf->P, kf->T0); mmtr_mlt(kf->T0, kf->A, kf->T1); m_add(kf->T1, kf->Q, kf->P); }
MAT *Hfactor(MAT *A, VEC *diag, VEC *beta) #endif { STATIC VEC *hh = VNULL, *w = VNULL; int k, limit; if ( ! A || ! diag || ! beta ) error(E_NULL,"Hfactor"); if ( diag->dim < A->m - 1 || beta->dim < A->m - 1 ) error(E_SIZES,"Hfactor"); if ( A->m != A->n ) error(E_SQUARE,"Hfactor"); limit = A->m - 1; hh = v_resize(hh,A->m); w = v_resize(w,A->n); MEM_STAT_REG(hh,TYPE_VEC); MEM_STAT_REG(w, TYPE_VEC); for ( k = 0; k < limit; k++ ) { /* compute the Householder vector hh */ get_col(A,(unsigned int)k,hh); /* printf("the %d'th column = "); v_output(hh); */ hhvec(hh,k+1,&beta->ve[k],hh,&A->me[k+1][k]); /* diag->ve[k] = hh->ve[k+1]; */ v_set_val(diag,k,v_entry(hh,k+1)); /* printf("H/h vector = "); v_output(hh); */ /* printf("from the %d'th entry\n",k+1); */ /* printf("beta = %g\n",beta->ve[k]); */ /* apply Householder operation symmetrically to A */ _hhtrcols(A,k+1,k+1,hh,v_entry(beta,k),w); hhtrrows(A,0 ,k+1,hh,v_entry(beta,k)); /* printf("A = "); m_output(A); */ } #ifdef THREADSAFE V_FREE(hh); V_FREE(w); #endif return (A); }
// 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; }
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); }
/* BKPsolve -- solves A.x = b where A has been factored a la BKPfactor() -- returns x, which is created if NULL */ extern VEC *BKPsolve(MAT *A, PERM *pivot, PERM *block, VEC *b, VEC *x) { static VEC *tmp=VNULL; /* dummy storage needed */ int i, j, n, onebyone; Real **A_me, a11, a12, a22, b1, b2, det, sum, *tmp_ve, tmp_diag; if ( ! A || ! pivot || ! block || ! b ) error(E_NULL,"BKPsolve"); if ( A->m != A->n ) error(E_SQUARE,"BKPsolve"); n = A->n; if ( b->dim != n || pivot->size != n || block->size != n ) error(E_SIZES,"BKPsolve"); x = v_resize(x,n); tmp = v_resize(tmp,n); MEM_STAT_REG(tmp,TYPE_VEC); A_me = A->me; tmp_ve = tmp->ve; px_vec(pivot,b,tmp); /* solve for lower triangular part */ for ( i = 0; i < n; i++ ) { sum = v_entry(tmp,i); if ( block->pe[i] < i ) for ( j = 0; j < i-1; j++ ) sum -= m_entry(A,i,j)*v_entry(tmp,j); else for ( j = 0; j < i; j++ ) sum -= m_entry(A,i,j)*v_entry(tmp,j); v_set_val(tmp,i,sum); } /* printf("# BKPsolve: solving L part: tmp =\n"); v_output(tmp); */ /* solve for diagonal part */ for ( i = 0; i < n; i = onebyone ? i+1 : i+2 ) { onebyone = ( block->pe[i] == i ); if ( onebyone ) { tmp_diag = m_entry(A,i,i); if ( tmp_diag == 0.0 ) error(E_SING,"BKPsolve"); /* tmp_ve[i] /= tmp_diag; */ v_set_val(tmp,i,v_entry(tmp,i) / tmp_diag); } else { a11 = m_entry(A,i,i); a22 = m_entry(A,i+1,i+1); a12 = m_entry(A,i+1,i); b1 = v_entry(tmp,i); b2 = v_entry(tmp,i+1); det = a11*a22-a12*a12; /* < 0 : see BKPfactor() */ if ( det == 0.0 ) error(E_SING,"BKPsolve"); det = 1/det; v_set_val(tmp,i,det*(a22*b1-a12*b2)); v_set_val(tmp,i+1,det*(a11*b2-a12*b1)); } } /* printf("# BKPsolve: solving D part: tmp =\n"); v_output(tmp); */ /* solve for transpose of lower traingular part */ for ( i = n-1; i >= 0; i-- ) { /* use symmetry of factored form to get stride 1 */ sum = v_entry(tmp,i); if ( block->pe[i] > i ) for ( j = i+2; j < n; j++ ) sum -= m_entry(A,i,j)*v_entry(tmp,j); else for ( j = i+1; j < n; j++ ) sum -= m_entry(A,i,j)*v_entry(tmp,j); v_set_val(tmp,i,sum); } /* printf("# BKPsolve: solving L^T part: tmp =\n");v_output(tmp); */ /* and do final permutation */ x = pxinv_vec(pivot,tmp,x); return x; }
MAT *Hfactor(MAT *A, VEC *diag, VEC *beta) #endif { char MatrixTempBuffer[ 2000 ]; /*STATIC */VEC *hh = VNULL, *w = VNULL; int k, limit; if ( ! A || ! diag || ! beta ) error(E_NULL,"Hfactor"); if ( diag->dim < A->m - 1 || beta->dim < A->m - 1 ) error(E_SIZES,"Hfactor"); if ( A->m != A->n ) error(E_SQUARE,"Hfactor"); limit = A->m - 1; if( SET_VEC_SIZE( A->m ) < 1000 ) { vec_get( &hh, (void *)MatrixTempBuffer, A->m ); } else { hh = v_get( A->m ); } if( SET_VEC_SIZE( A->n ) < 1000 ) { vec_get( &w, (void *)(MatrixTempBuffer + 1000), A->n ); } else { w = v_get( A->n ); } /*hh = v_resize(hh,A->m); w = v_resize(w,A->n); MEM_STAT_REG(hh,TYPE_VEC); MEM_STAT_REG(w, TYPE_VEC);*/ for ( k = 0; k < limit; k++ ) { /* compute the Householder vector hh */ get_col(A,(unsigned int)k,hh); /* printf("the %d'th column = "); v_output(hh); */ hhvec(hh,k+1,&beta->ve[k],hh,&A->me[k+1][k]); /* diag->ve[k] = hh->ve[k+1]; */ v_set_val(diag,k,v_entry(hh,k+1)); /* printf("H/h vector = "); v_output(hh); */ /* printf("from the %d'th entry\n",k+1); */ /* printf("beta = %g\n",beta->ve[k]); */ /* apply Householder operation symmetrically to A */ _hhtrcols(A,k+1,k+1,hh,v_entry(beta,k),w); hhtrrows(A,0 ,k+1,hh,v_entry(beta,k)); /* printf("A = "); m_output(A); */ } /* #ifdef THREADSAFE V_FREE(hh); V_FREE(w); #endif */ if( hh != (VEC *)(MatrixTempBuffer ) ) // память выделялась, надо освободить V_FREE(hh); if( w != (VEC *)(MatrixTempBuffer + 1000) ) // память выделялась, надо освободить V_FREE(w); return (A); }
MAT *makeHQ(MAT *H, VEC *diag, VEC *beta, MAT *Qout) #endif { unsigned int i,/* j,*/ limit; int j; char MatrixTempBuffer[ 2000 ]; /*STATIC */VEC *tmp1 = VNULL, *tmp2 = VNULL; if ( H==(MAT *)NULL || diag==(VEC *)NULL || beta==(VEC *)NULL ) error(E_NULL,"makeHQ"); limit = H->m - 1; if ( diag->dim < limit || beta->dim < limit ) error(E_SIZES,"makeHQ"); if ( H->m != H->n ) error(E_SQUARE,"makeHQ"); Qout = m_resize(Qout,H->m,H->m); if( SET_VEC_SIZE( H->m ) < 1000 ) { vec_get( &tmp1, (void *)MatrixTempBuffer, H->m ); } else { tmp1 = v_get( H->m ); } if( SET_VEC_SIZE( H->m ) < 1000 ) { vec_get( &tmp2, (void *)(MatrixTempBuffer + 1000), H->m ); } else { tmp2 = v_get( H->m); } /*tmp1 = v_resize(tmp1,H->m); tmp2 = v_resize(tmp2,H->m); MEM_STAT_REG(tmp1,TYPE_VEC); MEM_STAT_REG(tmp2,TYPE_VEC);*/ for ( i = 0; i < H->m; i++ ) { /* tmp1 = i'th basis vector */ for ( j = 0; j < (int)H->m; j++ ) /* tmp1->ve[j] = 0.0; */ v_set_val(tmp1,j,0.0); /* tmp1->ve[i] = 1.0; */ v_set_val(tmp1,i,1.0); /* apply H/h transforms in reverse order */ for ( j = limit-1; j >= 0; j-- ) { get_col(H,(unsigned int)j,tmp2); /* tmp2->ve[j+1] = diag->ve[j]; */ v_set_val(tmp2,j+1,v_entry(diag,j)); hhtrvec(tmp2,beta->ve[j],j+1,tmp1,tmp1); } /* insert into Qout */ set_col(Qout,(unsigned int)i,tmp1); } /* #ifdef THREADSAFE V_FREE(tmp1); V_FREE(tmp2); #endif */ if( tmp1 != (VEC *)(MatrixTempBuffer ) ) // память выделялась, надо освободить V_FREE(tmp1); if( tmp2 != (VEC *)(MatrixTempBuffer + 1000) ) // память выделялась, надо освободить V_FREE(tmp2); return (Qout); }