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