END_TEST START_TEST(test_point_position) { /* Generate target information based on a known point. In the simple case, calculated point must equal known point and average ray distance == 0. Then jigg the cameras symmetrically to get the same points again but with an analytically derivable ray distance. */ int cam, num_cams = 4; vec2d targs_plain[4], targs_jigged[4]; Calibration *calib[4]; char ori_tmpl[] = "cal/sym_cam%d.tif.ori"; char ori_name[25]; mm_np media_par = {1, 1., {1., 0., 0.}, {1., 0., 0.}, 1.}; vec3d point = {17, 42, 0}; /* Something in the FOV, non-symmetric. */ vec3d res, jigged; double jigg_amp = 0.5, skew_dist, jigged_correct; /* Target generation requires an existing calibration. */ chdir("testing_fodder/"); /* Four cameras on 4 quadrants. */ for (cam = 0; cam < num_cams; cam++) { sprintf(ori_name, ori_tmpl, cam + 1); calib[cam] = read_calibration(ori_name, "cal/cam1.tif.addpar", NULL); img_coord(point, calib[cam], &media_par, &(targs_plain[cam][0]), &(targs_plain[cam][1])); vec_copy(jigged, point); jigged[1] += ((cam % 2) ? jigg_amp : -jigg_amp); img_coord(jigged, calib[cam], &media_par, &(targs_jigged[cam][0]), &(targs_jigged[cam][1])); } skew_dist = point_position(targs_plain, num_cams, &media_par, calib, res); fail_unless(skew_dist < 1e-10); vec_subt(point, res, res); fail_unless(vec_norm(res) < 1e-10); skew_dist = point_position(targs_jigged, num_cams, &media_par, calib, res); jigged_correct = 4*(2*jigg_amp)/6; /* two thirds, but because of details: each disagreeing pair (4 out of 6) has a 2*jigg_amp error because the cameras are moved in opposite directions. */ fail_unless(fabs(skew_dist - jigged_correct) < 0.05); vec_subt(point, res, res); fail_unless(vec_norm(res) < 0.01); }
/* angle_acc() calculates the angle between the (1st order) numerical velocity vectors to the predicted next position and to the candidate actual position. The angle is calculated in [gon], see [1]. The predicted position is the position if the particle continued at current velocity. Arguments: vec3d start, pred, cand - the particle start position, predicted position, and possible actual position, respectively. double *angle - output variable, the angle between the two velocity vectors, [gon] double *acc - output variable, the 1st-order numerical acceleration embodied in the deviation from prediction. */ void angle_acc(vec3d start, vec3d pred, vec3d cand, double *angle, double *acc) { vec3d v0, v1; vec_subt(pred, start, v0); vec_subt(cand, start, v1); *acc = vec_diff_norm(v0, v1); if ((v0[0] == -v1[0]) && (v0[1] == -v1[1]) && (v0[2] == -v1[2])) { *angle = 200; } else { *angle = (200./M_PI) * acos(vec_dot(v0, v1) / vec_norm(v0) \ / vec_norm(v1)); } }
/* skew_midpoint() finds the middle of the minimal distance segment between skew rays. Undefined for parallel rays. Reference for algorithm: docs/skew_midpoint.pdf Arguments: vec3d vert1, direct1 - vertex and direction unit vector of one ray. vec3d vert2, direct2 - vertex and direction unit vector of other ray. vec3d res - output buffer for midpoint result. Returns: The minimal distance between the skew lines. */ double skew_midpoint(vec3d vert1, vec3d direct1, vec3d vert2, vec3d direct2, vec3d res) { vec3d perp_both, sp_diff, on1, on2, temp; double scale; /* vector between starting points */ vec_subt(vert2, vert1, sp_diff); /* The shortest distance is on a line perpendicular to both rays. */ vec_cross(direct1, direct2, perp_both); scale = vec_dot(perp_both, perp_both); /* position along each ray */ vec_cross(sp_diff, direct2, temp); vec_scalar_mul(direct1, vec_dot(perp_both, temp)/scale, temp); vec_add(vert1, temp, on1); vec_cross(sp_diff, direct1, temp); vec_scalar_mul(direct2, vec_dot(perp_both, temp)/scale, temp); vec_add(vert2, temp, on2); /* Distance: */ scale = vec_diff_norm(on1, on2); /* Average */ vec_add(on1, on2, res); vec_scalar_mul(res, 0.5, res); return scale; }
END_TEST START_TEST(test_vec_subt) { vec3d sub = {1., 2., 3.}, from = {4., 5., 6.}, res = {3., 3., 3.}, out; vec_subt(from, sub, out); fail_unless(vec_cmp(out, res)); }
double lines_distance_3d(double a1[3], double a2[3], double b1[3], double b2[3]) { double cp1[3], cp2[3], len[3]; line_segment_closest_points(a1, a2, b1, b2, cp1, cp2); vec_subt(len, cp2, cp1); return vec_dot(len, len); }
/* weighted_dumbbell_precision() Gives a weighted sum of dumbbell precision measures: ray convergence, and dumbbell length. The weight of the first is 1, and the later is weighted by a user defined value. Arguments: (vec2d **) targets - 2D array of targets, so order 3 tensor of shape (num_targs,num_cams,2). Each target is the 2D metric, flat, centred coordinates of one identified point. int num_targs - the number of known targets, assumed to be the same in all cameras. int num_cams - number of cameras. mm_np *multimed_pars - multimedia parameters struct for ray tracing through several layers. Calibration* cals[] - each camera's calibration object. int db_length - distance between two consecutive targets (assuming the targets list is a 2 by 2 list of dumbbell frames). double db_weight - the weight of the average length error compared to the average ray convergence error. */ double weighted_dumbbell_precision(vec2d** targets, int num_targs, int num_cams, mm_np *multimed_pars, Calibration* cals[], int db_length, double db_weight) { int pt; double dtot = 0, len_err_tot = 0, dist; vec3d res[2], *res_current; for (pt = 0; pt < num_targs; pt++) { res_current = &(res[pt % 2]); dtot += point_position(targets[pt], num_cams, multimed_pars, cals, *res_current); if (pt % 2 == 1) { vec_subt(res[0], res[1], res[0]); dist = vec_norm(res[0]); len_err_tot += 1 - ((dist > db_length) ? (db_length/dist) : dist/db_length); } } /* note half as many pairs as targets is assumed */ return (dtot / num_targs + db_weight*len_err_tot/(0.5*num_targs)); }
/* track backwards */ double trackback_c (tracking_run *run_info, int step) { int i, j, h, in_volume = 0; int philf[4][MAX_CANDS]; int count1 = 0, count2 = 0, num_added = 0; int quali = 0; double angle, acc, dl; vec3d diff_pos, X[6]; /* 6 reference points used in the algorithm */ vec2d n[4], v2[4]; // replaces xn,yn, x2[4], y2[4], double rr, Ymin = 0, Ymax = 0; double npart = 0, nlinks = 0; foundpix *w; sequence_par *seq_par; track_par *tpar; volume_par *vpar; control_par *cpar; framebuf *fb; Calibration **cal; /* Shortcuts to inside current frame */ P *curr_path_inf, *ref_path_inf; /* shortcuts */ cal = run_info->cal; seq_par = run_info->seq_par; tpar = run_info->tpar; vpar = run_info->vpar; cpar = run_info->cpar; fb = run_info->fb; /* Prime the buffer with first frames */ for (step = seq_par->last; step > seq_par->last - 4; step--) { fb_read_frame_at_end(fb, step, 1); fb_next(fb); } fb_prev(fb); /* sequence loop */ for (step = seq_par->last - 1; step > seq_par->first; step--) { printf ("Time step: %d, seqnr: %d:\n", step - seq_par->first, step); for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); /* We try to find link only if the forward search failed to. */ if ((curr_path_inf->next < 0) || (curr_path_inf->prev != -1)) continue; for (j = 0; j < 6; j++) vec_init(X[j]); curr_path_inf->inlist = 0; /* 3D-position of current particle */ vec_copy(X[1], curr_path_inf->x); /* use information from previous to locate new search position and to calculate values for search area */ ref_path_inf = &(fb->buf[0]->path_info[curr_path_inf->next]); vec_copy(X[0], ref_path_inf->x); search_volume_center_moving(ref_path_inf->x, curr_path_inf->x, X[2]); for (j = 0; j < fb->num_cams; j++) { point_to_pixel (n[j], X[2], cal[j], cpar); } /* calculate searchquader and reprojection in image space */ w = sorted_candidates_in_volume(X[2], n, fb->buf[2], run_info); if (w != NULL) { count2++; i = 0; while (w[i].ftnr != TR_UNUSED) { ref_path_inf = &(fb->buf[2]->path_info[w[i].ftnr]); vec_copy(X[3], ref_path_inf->x); vec_subt(X[1], X[3], diff_pos); if (pos3d_in_bounds(diff_pos, tpar)) { angle_acc(X[1], X[2], X[3], &angle, &acc); /* *********************check link *****************************/ if ((acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10)) { dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) )/2; quali = w[i].freq; rr = (dl/run_info->lmax + acc/tpar->dacc + \ angle/tpar->dangle)/quali; register_link_candidate(curr_path_inf, rr, w[i].ftnr); } } i++; } } free(w); /* if old wasn't found try to create new particle position from rest */ if (tpar->add) { if ( curr_path_inf->inlist == 0) { quali = assess_new_position(X[2], v2, philf, fb->buf[2], run_info); if (quali>=2) { //vec_copy(X[3], X[2]); in_volume = 0; point_position(v2, fb->num_cams, cpar->mm, cal, X[3]); /* volume check */ if ( vpar->X_lay[0] < X[3][0] && X[3][0] < vpar->X_lay[1] && Ymin < X[3][1] && X[3][1] < Ymax && vpar->Zmin_lay[0] < X[3][2] && X[3][2] < vpar->Zmax_lay[1]) {in_volume = 1;} vec_subt(X[1], X[3], diff_pos); if (in_volume == 1 && pos3d_in_bounds(diff_pos, tpar)) { angle_acc(X[1], X[2], X[3], &angle, &acc); if ( (acc<tpar->dacc && angle<tpar->dangle) || \ (acc<tpar->dacc/10) ) { dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) )/2; rr = (dl/run_info->lmax+acc/tpar->dacc + angle/tpar->dangle)/(quali); register_link_candidate(curr_path_inf, rr, fb->buf[2]->num_parts); add_particle(fb->buf[2], X[3], philf); } } in_volume = 0; } } } /* end of if old wasn't found try to create new particle position from rest */ } /* end of h-loop */ for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); if(curr_path_inf->inlist > 0 ) { sort(curr_path_inf->inlist, (float *)curr_path_inf->decis, curr_path_inf->linkdecis); } } /* create links with decision check */ count1 = 0; num_added = 0; for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); if (curr_path_inf->inlist > 0 ) { /* if old/new and unused prev == -1 and next == -2 link is created */ ref_path_inf = &(fb->buf[2]->path_info[curr_path_inf->linkdecis[0]]); if ( ref_path_inf->prev == PREV_NONE && \ ref_path_inf->next == NEXT_NONE ) { curr_path_inf->finaldecis = curr_path_inf->decis[0]; curr_path_inf->prev = curr_path_inf->linkdecis[0]; fb->buf[2]->path_info[curr_path_inf->prev].next = h; num_added++; } /* old which link to prev has to be checked */ if ((ref_path_inf->prev != PREV_NONE) && \ (ref_path_inf->next == NEXT_NONE) ) { vec_copy(X[0], fb->buf[0]->path_info[curr_path_inf->next].x); vec_copy(X[1], curr_path_inf->x); vec_copy(X[3], ref_path_inf->x); vec_copy(X[4], fb->buf[3]->path_info[ref_path_inf->prev].x); for (j = 0; j < 3; j++) X[5][j] = 0.5*(5.0*X[3][j] - 4.0*X[1][j] + X[0][j]); angle_acc(X[3], X[4], X[5], &angle, &acc); if ( (acc<tpar->dacc && angle<tpar->dangle) || (acc<tpar->dacc/10) ) { curr_path_inf->finaldecis = curr_path_inf->decis[0]; curr_path_inf->prev = curr_path_inf->linkdecis[0]; fb->buf[2]->path_info[curr_path_inf->prev].next = h; num_added++; } } } if (curr_path_inf->prev != PREV_NONE ) count1++; } /* end of creation of links with decision check */ printf ("step: %d, curr: %d, next: %d, links: %d, lost: %d, add: %d", step, fb->buf[1]->num_parts, fb->buf[2]->num_parts, count1, fb->buf[1]->num_parts - count1, num_added); /* for the average of particles and links */ npart = npart + fb->buf[1]->num_parts; nlinks = nlinks + count1; fb_next(fb); fb_write_frame_from_start(fb, step); if(step > seq_par->first + 2) { fb_read_frame_at_end(fb, step - 3, 1); } } /* end of sequence loop */ /* average of all steps */ npart /= (seq_par->last - seq_par->first - 1); nlinks /= (seq_par->last - seq_par->first - 1); printf ("Average over sequence, particles: %5.1f, links: %5.1f, lost: %5.1f\n", npart, nlinks, npart-nlinks); fb_next(fb); fb_write_frame_from_start(fb, step); fb_free(fb); free(fb); free(tpar); return nlinks; }
/* trackcorr_c_loop is the main tracking subroutine that scans the 3D particle position * data from rt_is.* files and the 2D particle positions in image space in _targets and * constructs trajectories (links) of the particles in 3D in time. * the basic concepts of the tracking procedure are from the following publication by * Jochen Willneff: "A New Spatio-Temporal Matching Algorithm For 3D-Particle Tracking Velocimetry" * https://www.mendeley.com/catalog/new-spatiotemporal-matching-algorithm-3dparticle-tracking-velocimetry/ * or http://e-collection.library.ethz.ch/view/eth:26978 * this method is an extension of the previously used tracking method described in details in * Malik et al. 1993: "Particle tracking velocimetry in three-dimensional flows: Particle tracking" * http://mnd.ly/2dCt3um * * Arguments: * tracking_run *run_info pointer to the (sliding) frame dataset of 4 frames of particle positions * and all the needed parameters underneath: control, volume, etc. * integer step number or the frame number from the sequence * Note: step is not really setting up the step to track, the buffer provided to the trackcoor_c_loop * is already preset by 4 frames buf[0] to buf[3] and we track particles in buf[1], i.e. one "previous" * one present and two future frames. * * Returns: function does not return an argument, the tracks are updated within the run_info dataset */ void trackcorr_c_loop (tracking_run *run_info, int step) { /* sequence loop */ int j, h, mm, kk, in_volume = 0; int philf[4][MAX_CANDS]; int count1 = 0, count2 = 0, count3 = 0, num_added = 0; int quali = 0; vec3d diff_pos, X[6]; /* 7 reference points used in the algorithm, TODO: check if can reuse some */ double angle, acc, angle0, acc0, dl; double angle1, acc1; vec2d v1[4], v2[4]; /* volume center projection on cameras */ double rr; /* Shortcuts to inside current frame */ P *curr_path_inf, *ref_path_inf; corres *curr_corres; target **curr_targets; int _ix; /* For use in any of the complex index expressions below */ int orig_parts; /* avoid infinite loop with particle addition set */ /* Shortcuts into the tracking_run struct */ Calibration **cal; framebuf *fb; track_par *tpar; volume_par *vpar; control_par *cpar; foundpix *w, *wn; count1 = 0; num_added = 0; fb = run_info->fb; cal = run_info->cal; tpar = run_info->tpar; vpar = run_info->vpar; cpar = run_info->cpar; curr_targets = fb->buf[1]->targets; /* try to track correspondences from previous 0 - corp, variable h */ orig_parts = fb->buf[1]->num_parts; for (h = 0; h < orig_parts; h++) { for (j = 0; j < 6; j++) vec_init(X[j]); curr_path_inf = &(fb->buf[1]->path_info[h]); curr_corres = &(fb->buf[1]->correspond[h]); curr_path_inf->inlist = 0; /* 3D-position */ vec_copy(X[1], curr_path_inf->x); /* use information from previous to locate new search position and to calculate values for search area */ if (curr_path_inf->prev >= 0) { ref_path_inf = &(fb->buf[0]->path_info[curr_path_inf->prev]); vec_copy(X[0], ref_path_inf->x); search_volume_center_moving(ref_path_inf->x, curr_path_inf->x, X[2]); for (j = 0; j < fb->num_cams; j++) { point_to_pixel (v1[j], X[2], cal[j], cpar); } } else { vec_copy(X[2], X[1]); for (j = 0; j < fb->num_cams; j++) { if (curr_corres->p[j] == -1) { point_to_pixel (v1[j], X[2], cal[j], cpar); } else { _ix = curr_corres->p[j]; v1[j][0] = curr_targets[j][_ix].x; v1[j][1] = curr_targets[j][_ix].y; } } } /* calculate search cuboid and reproject it to the image space */ w = sorted_candidates_in_volume(X[2], v1, fb->buf[2], run_info); if (w == NULL) continue; /* Continue to find candidates for the candidates. */ count2++; mm = 0; while (w[mm].ftnr != TR_UNUSED) { /* counter1-loop */ /* search for found corr of current the corr in next with predicted location */ /* found 3D-position */ ref_path_inf = &(fb->buf[2]->path_info[w[mm].ftnr]); vec_copy(X[3], ref_path_inf->x); if (curr_path_inf->prev >= 0) { for (j = 0; j < 3; j++) X[5][j] = 0.5*(5.0*X[3][j] - 4.0*X[1][j] + X[0][j]); } else { search_volume_center_moving(X[1], X[3], X[5]); } for (j = 0; j < fb->num_cams; j++) { point_to_pixel (v1[j], X[5], cal[j], cpar); } /* end of search in pix */ wn = sorted_candidates_in_volume(X[5], v1, fb->buf[3], run_info); if (wn != NULL) { count3++; kk = 0; while (wn[kk].ftnr != TR_UNUSED) { ref_path_inf = &(fb->buf[3]->path_info[wn[kk].ftnr]); vec_copy(X[4], ref_path_inf->x); vec_subt(X[4], X[3], diff_pos); if ( pos3d_in_bounds(diff_pos, tpar)) { angle_acc(X[3], X[4], X[5], &angle1, &acc1); if (curr_path_inf->prev >= 0) { angle_acc(X[1], X[2], X[3], &angle0, &acc0); } else { acc0 = acc1; angle0 = angle1; } acc = (acc0+acc1)/2; angle = (angle0+angle1)/2; quali = wn[kk].freq+w[mm].freq; if ((acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10)) { dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[4], X[3]) )/2; rr = (dl/run_info->lmax + acc/tpar->dacc + \ angle/tpar->dangle)/(quali); register_link_candidate( curr_path_inf, rr, w[mm].ftnr); } } kk++; } /* End of searching 2nd-frame candidates. */ } /* creating new particle position, * reset img coord because of num_cams < 4 * fix distance of 3 pixels to define xl,xr,yu,yd instead of searchquader * and search for unused candidates in next time step */ quali = assess_new_position(X[5], v2, philf, fb->buf[3], run_info); /* quali >=2 means at least in two cameras * we found a candidate */ if ( quali >= 2) { in_volume = 0; //inside volume dl = point_position(v2, cpar->num_cams, cpar->mm, cal, X[4]); /* volume check */ if ( vpar->X_lay[0] < X[4][0] && X[4][0] < vpar->X_lay[1] && run_info->ymin < X[4][1] && X[4][1] < run_info->ymax && vpar->Zmin_lay[0] < X[4][2] && X[4][2] < vpar->Zmax_lay[1]) { in_volume = 1; } vec_subt(X[3], X[4], diff_pos); if ( in_volume == 1 && pos3d_in_bounds(diff_pos, tpar) ) { angle_acc(X[3], X[4], X[5], &angle, &acc); if ((acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10)) { dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[4], X[3]) )/2; rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle) / (quali+w[mm].freq); register_link_candidate(curr_path_inf, rr, w[mm].ftnr); if (tpar->add) { add_particle(fb->buf[3], X[4], philf); num_added++; } } } in_volume = 0; } quali = 0; /* end of creating new particle position */ /* *************************************************************** */ /* try to link if kk is not found/good enough and prev exist */ if ( curr_path_inf->inlist == 0 && curr_path_inf->prev >= 0 ) { vec_subt(X[3], X[1], diff_pos); if (pos3d_in_bounds(diff_pos, tpar)) { angle_acc(X[1], X[2], X[3], &angle, &acc); if ( (acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10) ) { quali = w[mm].freq; dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) )/2; rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle)/(quali); register_link_candidate(curr_path_inf, rr, w[mm].ftnr); } } } free(wn); mm++; } /* end of loop over first-frame candidates. */ /* begin of inlist still zero */ if (tpar->add) { if ( curr_path_inf->inlist == 0 && curr_path_inf->prev >= 0 ) { quali = assess_new_position(X[2], v2, philf, fb->buf[2], run_info); if (quali>=2) { vec_copy(X[3], X[2]); in_volume = 0; dl = point_position(v2, fb->num_cams, cpar->mm, cal, X[3]); /* in volume check */ if ( vpar->X_lay[0] < X[3][0] && X[3][0] < vpar->X_lay[1] && run_info->ymin < X[3][1] && X[3][1] < run_info->ymax && vpar->Zmin_lay[0] < X[3][2] && X[3][2] < vpar->Zmax_lay[1]) { in_volume = 1; } vec_subt(X[2], X[3], diff_pos); if ( in_volume == 1 && pos3d_in_bounds(diff_pos, tpar) ) { angle_acc(X[1], X[2], X[3], &angle, &acc); if ( (acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10) ) { dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) )/2; rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle)/(quali); register_link_candidate(curr_path_inf, rr, fb->buf[2]->num_parts); add_particle(fb->buf[2], X[3], philf); num_added++; } } in_volume = 0; } // if quali >= 2 } } /* end of inlist still zero */ /***********************************/ free(w); } /* end of h-loop */ /* sort decis and give preliminary "finaldecis" */ for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); if(curr_path_inf->inlist > 0 ) { sort(curr_path_inf->inlist, (float *) curr_path_inf->decis, curr_path_inf->linkdecis); curr_path_inf->finaldecis = curr_path_inf->decis[0]; curr_path_inf->next = curr_path_inf->linkdecis[0]; } } /* create links with decision check */ for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); if(curr_path_inf->inlist > 0 ) { ref_path_inf = &(fb->buf[2]->path_info[curr_path_inf->next]); if (ref_path_inf->prev == PREV_NONE) { /* best choice wasn't used yet, so link is created */ ref_path_inf->prev = h; } else { /* best choice was already used by mega[2][mega[1][h].next].prev */ /* check which is the better choice */ if ( fb->buf[1]->path_info[ref_path_inf->prev].finaldecis > \ curr_path_inf->finaldecis) { /* remove link with prev */ fb->buf[1]->path_info[ref_path_inf->prev].next = NEXT_NONE; ref_path_inf->prev = h; } else { curr_path_inf->next = NEXT_NONE; } } } if (curr_path_inf->next != NEXT_NONE ) count1++; } /* end of creation of links with decision check */ printf ("step: %d, curr: %d, next: %d, links: %d, lost: %d, add: %d\n", step, fb->buf[1]->num_parts, fb->buf[2]->num_parts, count1, fb->buf[1]->num_parts - count1, num_added); /* for the average of particles and links */ run_info->npart = run_info->npart + fb->buf[1]->num_parts; run_info->nlinks = run_info->nlinks + count1; fb_next(fb); fb_write_frame_from_start(fb, step); if(step < run_info->seq_par->last - 2) { fb_read_frame_at_end(fb, step + 3, 0); } } /* end of sequence loop */
/* search_volume_center_moving() finds the position of the center of the search * volume for a moving particle using the velocity of last step. * * Arguments: * vec3d prev_pos - previous position * vec3d curr_pos - current position * vec3d *output - output variable, for the calculated * position. */ void search_volume_center_moving(vec3d prev_pos, vec3d curr_pos, vec3d output) { vec_scalar_mul(curr_pos, 2, output); vec_subt(output, prev_pos, output); }
// This function is adapted from geom.cpp in ODE, which // is copyright Russell Smith. // given two line segments A and B with endpoints a1-a2 and b1-b2, return the // points on A and B that are closest to each other (in cp1 and cp2). // in the case of parallel lines where there are multiple solutions, a // solution involving the endpoint of at least one line will be returned. // this will work correctly for zero length lines, e.g. if a1==a2 and/or // b1==b2. // // the algorithm works by applying the voronoi clipping rule to the features // of the line segments. the three features of each line segment are the two // endpoints and the line between them. the voronoi clipping rule states that, // for feature X on line A and feature Y on line B, the closest points PA and // PB between X and Y are globally the closest points if PA is in V(Y) and // PB is in V(X), where V(X) is the voronoi region of X. void line_segment_closest_points(double a1[3], double a2[3], double b1[3], double b2[3], double cp1[3], double cp2[3]) { double la, lb, k, da1, da2, da3, da4, db1, db2, db3, db4, det; double a1a2[3], b1b2[3], a1b1[3], a1b2[3], a2b1[3], a2b2[3], n[3], tmp[3]; // check vertex-vertex features vec_subt(a1a2, a2, a1); vec_subt(b1b2, b2, b1); vec_subt(a1b1, b1, a1); da1 = vec_dot(a1a2, a1b1); db1 = vec_dot(b1b2, a1b1); if ((da1 <= 0) && (db1 >= 0)) { vec_copy(cp1, a1); vec_copy(cp2, b1); return; } vec_subt(a1b2, b2, a1); da2 = vec_dot(a1a2, a1b2); db2 = vec_dot(b1b2,a1b2); if ((da2 <= 0) && (db2 <= 0)) { vec_copy(cp1, a1); vec_copy(cp2, b2); return; } vec_subt(a2b1, b1, a2); da3 = vec_dot(a1a2, a2b1); db3 = vec_dot(b1b2, a2b1); if ((da3 >= 0) && (db3 >= 0)) { vec_copy(cp1, a2); vec_copy(cp2, b1); return; } vec_subt(a2b2, b2, a2); da4 = vec_dot(a1a2, a2b2); db4 = vec_dot(b1b2, a2b2); if ((da4 >= 0) && (db4 <= 0)) { vec_copy(cp1, a2); vec_copy(cp2, b2); return; } // check edge-vertex features. // if one or both of the lines has zero length, we will never get to here, // so we do not have to worry about the following divisions by zero. la = vec_dot(a1a2, a1a2); if ((da1 >= 0) && (da3 <= 0.0)) { k = da1 / la; vsv_mult(tmp, k, a1a2); vec_subt(n, a1b1, tmp); if (vec_dot(b1b2, n) >= 0.0) { vec_plus(cp1, a1, tmp); vec_copy(cp2, b1); return; } } if ((da2 >= 0) && (da4 <= 0)) { k = da2 / la; vsv_mult(tmp, k, a1a2); vec_subt(n, a1b2, tmp); if (vec_dot(b1b2, n) <= 0.0) { vec_plus(cp1, a1, tmp); vec_copy(cp2, b2); return; } } lb = vec_dot(b1b2, b1b2); if ((db1 <= 0) && (db2 >= 0.0)) { k = -db1 / lb; vsv_mult(tmp, k, b1b2); vec_neg(a1b1); vec_subt(n, a1b1, tmp); if (vec_dot(a1a2, n) >= 0.0) { vec_copy(cp1,a1); vec_plus(cp2, b1, tmp); return; } } if ((db3 <= 0.0) && (db4 >= 0.0)) { k = -db3 / lb; vsv_mult(tmp, k, b1b2); vec_neg(a2b1); vec_subt(n, a2b1, tmp); if (vec_dot(a1a2, n) <= 0.0) { vec_copy(cp1,a2); vec_plus(cp2, b1, tmp); return; } } // it must be edge-edge k = vec_dot(a1a2, b1b2); det = la*lb - k*k; if (det <= 0.0) { // this should never happen, but just in case... vec_copy(cp1, a1); vec_copy(cp2, b1); return; } det = 1.0/det; double alpha = (lb*da1 - k*db1) * det; double beta = ( k*da1 - la*db1) * det; vs_mult(a1a2, alpha); vec_plus(cp1, a1, a1a2); vs_mult(b1b2, beta); vec_plus(cp2, b1, b1b2); }
double line_segment_distance(double seg1_a[3], double seg1_b[3], double seg2_a[3], double seg2_b[3]) { double sc, sd, sn, tc, td, tn; double uu, uv, vv, uw, vw, det; double u[3], v[3], w[3]; vec_subt(u, seg1_b, seg1_a); vec_subt(v, seg2_b, seg2_a); vec_subt(w, seg1_a, seg2_a); uu = vec_dot(u, u); uv = vec_dot(u, v); vv = vec_dot(v, v); uw = vec_dot(u, w); vw = vec_dot(v, w); det = uu*vv - sqr(uv); sd = det; td = det; if (det < ALMOST_ZERO) { sn = 0.0; sd = 1.0; tn = vw; td = vv; } else { sn = uv*vw - vv*uw; tn = uu*vw - uv*uw; if (sn < 0.0) { sn = 0.0; tn = vw; td = vv; } else if (sn > sd) { sn = sd; tn = vw + uv; td = vv; } } if (tn < 0.0) { tn = 0.0; if (-uw < 0.0) { sn = 0.0; } else if (-uw > uu) { sn = sd; } else { sn = -uw; sd = uu; } } else if (tn > td) { tn = td; if (uv-uw < 0.0) { sn = 0.0; } else if (uv-uw > uu) { sn = sd; } else { sn = uv-uw; sd = uu; } } sc = (absv(sn) < ALMOST_ZERO ? 0.0 : sn / sd); tc = (absv(tn) < ALMOST_ZERO ? 0.0 : tn / td); vs_mult(u, sc); vs_mult(v, tc); vec_plus(w, w, u); vec_subt(w, w, v); return vec_dot(w, w); }
/* track backwards */ void trackback_c () { char buf[256]; int i, j, h, k, step, invol=0; int zaehler1, philf[4][MAX_CANDS]; int count1=0, count2=0, zusatz=0; int quali=0; double x2[4], y2[4], angle, acc, lmax, dl; double xr[4], xl[4], yd[4], yu[4]; vec3d diff_pos, X[7]; /* 7 reference points used in the algorithm, TODO: check if can reuse some */ double xn[4], yn[4]; double rr, Ymin=0, Ymax=0; double npart=0, nlinks=0; foundpix *w, p16[4*MAX_CANDS]; sequence_par *seq_par; track_par *tpar; volume_par *vpar; control_par *cpar; framebuf *fb; /* Shortcuts to inside current frame */ P *curr_path_inf, *ref_path_inf; corres *ref_corres; target **ref_targets; int _ix; /* For use in any of the complex index expressions below */ int _frame_parts; /* number of particles in a frame */ display = 1; /* read data */ seq_par = read_sequence_par("parameters/sequence.par", 4); tpar = read_track_par("parameters/track.par"); vpar = read_volume_par("parameters/criteria.par"); cpar = read_control_par("parameters/ptv.par"); fb = (framebuf *) malloc(sizeof(framebuf)); fb_init(fb, 4, cpar->num_cams, MAX_TARGETS, "res/rt_is", "res/ptv_is", "res/added", seq_par->img_base_name); /* Prime the buffer with first frames */ for (step = seq_par->last; step > seq_par->last - 4; step--) { fb_read_frame_at_end(fb, step, 1); fb_next(fb); } fb_prev(fb); lmax = norm((tpar->dvxmin - tpar->dvxmax), (tpar->dvymin - tpar->dvymax), (tpar->dvzmin - tpar->dvzmax)); volumedimension (glob_cal, &(vpar->X_lay[1]), &(vpar->X_lay[0]), &Ymax, &Ymin, &(vpar->Zmax_lay[1]), &(vpar->Zmin_lay[0]), cpar); /* sequence loop */ for (step = seq_par->last - 1; step > seq_par->first; step--) { sprintf (buf, "Time step: %d, seqnr: %d, Particle info:", step - seq_par->first, step); for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); /* We try to find link only if the forward search failed to. */ if ((curr_path_inf->next < 0) || (curr_path_inf->prev != -1)) continue; for (j = 0; j < 7; j++) vec_init(X[j]); curr_path_inf->inlist = 0; reset_foundpix_array(p16, 16, fb->num_cams); /* 3D-position of current particle */ vec_copy(X[1], curr_path_inf->x); /* use information from previous to locate new search position and to calculate values for search area */ ref_path_inf = &(fb->buf[0]->path_info[curr_path_inf->next]); vec_copy(X[0], ref_path_inf->x); search_volume_center_moving(ref_path_inf->x, curr_path_inf->x, X[2]); for (j=0; j < fb->num_cams; j++) { img_coord (j, X[2][0], X[2][1], X[2][2], Ex[j],I[j], G[j], ap[j], *(cpar->mm), &xn[j], &yn[j]); metric_to_pixel(&xn[j], &yn[j], xn[j], yn[j], cpar); } /* calculate searchquader and reprojection in image space */ searchquader(X[2][0], X[2][1], X[2][2], xr, xl, yd, yu, tpar, cpar); for (j = 0; j < fb->num_cams; j++) { zaehler1 = candsearch_in_pix ( fb->buf[2]->targets[j], fb->buf[2]->num_targets[j], xn[j], yn[j], xl[j], xr[j], yu[j], yd[j], philf[j], cpar); for(k=0; k<4; k++) { if( zaehler1>0) { if (philf[j][k] == -999){ p16[j*4+k].ftnr=-1; } else { p16[j*4+k].ftnr=fb->buf[3]->targets[j][philf[j][k]].tnr; p16[j*4+k].whichcam[j]=1; } } } } /* search in pix for candidates in next time step */ //for (j = 0; j < fb->num_cams; j++) { // register_closest_neighbs(fb->buf[2]->targets[j], // fb->buf[2]->num_targets[j], j, xn[j], yn[j], // xl[j], xr[j], yu[j], yd[j], &p16[j*MAX_CANDS]); //} /* fill and sort candidate struct */ sortwhatfound(p16, &zaehler1, fb->num_cams); w = (foundpix *) calloc (zaehler1, sizeof (foundpix)); /*end of candidate struct */ if (zaehler1 > 0) count2++; copy_foundpix_array(w, p16, zaehler1, fb->num_cams); for (i = 0; i < zaehler1; i++) { ref_path_inf = &(fb->buf[2]->path_info[w[i].ftnr]); vec_copy(X[3], ref_path_inf->x); vec_subt(X[1], X[3], diff_pos); if (pos3d_in_bounds(diff_pos, tpar)) { angle_acc(X[1], X[2], X[3], &angle, &acc); /* *********************check link *****************************/ if ((acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10)) { dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) )/2; quali=w[i].freq; rr = (dl/lmax + acc/tpar->dacc + angle/tpar->dangle)/quali; register_link_candidate(curr_path_inf, rr, w[i].ftnr); } } } free(w); /******************/ quali=0; /* reset img coord because num_cams < 4 */ for (j=0;j<4;j++) { x2[j]=-1e10; y2[j]=-1e10;} /* if old wasn't found try to create new particle position from rest */ if (tpar->add) { if ( curr_path_inf->inlist == 0) { for (j = 0; j < fb->num_cams; j++) { /* use fix distance to define xl, xr, yu, yd instead of searchquader */ xl[j]= xr[j]= yu[j]= yd[j] = 3.0; zaehler1 = candsearch_in_pixrest (fb->buf[2]->targets[j], fb->buf[2]->num_targets[j], xn[j], yn[j], xl[j], xr[j], yu[j], yd[j], philf[j], cpar); if(zaehler1 > 0) { _ix = philf[j][0]; x2[j] = fb->buf[2]->targets[j][_ix].x; y2[j] = fb->buf[2]->targets[j][_ix].y; } } for (j = 0; j < fb->num_cams; j++) { if (x2[j] !=-1e10 && y2[j] != -1e10) { pixel_to_metric(&x2[j], &y2[j], x2[j],y2[j], cpar); quali++; } } if (quali>=2) { vec_copy(X[3], X[2]); invol=0; det_lsq_3d (glob_cal, *(cpar->mm), x2[0], y2[0], x2[1], y2[1], x2[2], y2[2], x2[3], y2[3], &(X[3][0]), &(X[3][1]), &(X[3][2]), fb->num_cams); /* volume check */ if ( vpar->X_lay[0] < X[3][0] && X[3][0] < vpar->X_lay[1] && Ymin < X[3][1] && X[3][1] < Ymax && vpar->Zmin_lay[0] < X[3][2] && X[3][2] < vpar->Zmax_lay[1]) {invol = 1;} vec_subt(X[1], X[3], diff_pos); if (invol == 1 && pos3d_in_bounds(diff_pos, tpar)) { angle_acc(X[1], X[2], X[3], &angle, &acc); if ( (acc<tpar->dacc && angle<tpar->dangle) || \ (acc<tpar->dacc/10) ) { dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) )/2; rr =(dl/lmax+acc/tpar->dacc + angle/tpar->dangle)/(quali); ref_path_inf = &(fb->buf[2]->path_info[ fb->buf[2]->num_parts]); vec_copy(ref_path_inf->x, X[3]); reset_links(ref_path_inf); _frame_parts = fb->buf[2]->num_parts; register_link_candidate(curr_path_inf, rr, _frame_parts); ref_corres = &(fb->buf[2]->correspond[_frame_parts]); ref_targets = fb->buf[2]->targets; for (j = 0;j < fb->num_cams; j++) { ref_corres->p[j]=-1; if(philf[j][0]!=-999) { _ix = philf[j][0]; ref_targets[j][_ix].tnr = _frame_parts; ref_corres->p[j] = _ix; ref_corres->nr = _frame_parts; } } fb->buf[2]->num_parts++; } } invol=0; } } } /* end of if old wasn't found try to create new particle position from rest */ } /* end of h-loop */ for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); if(curr_path_inf->inlist > 0 ) { sort(curr_path_inf->inlist, (float *)curr_path_inf->decis, curr_path_inf->linkdecis); } } /* create links with decision check */ count1=0; zusatz=0; for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); if (curr_path_inf->inlist > 0 ) { /* if old/new and unused prev == -1 and next == -2 link is created */ ref_path_inf = &(fb->buf[2]->path_info[curr_path_inf->linkdecis[0]]); if ( ref_path_inf->prev == PREV_NONE && \ ref_path_inf->next == NEXT_NONE ) { curr_path_inf->finaldecis = curr_path_inf->decis[0]; curr_path_inf->prev = curr_path_inf->linkdecis[0]; fb->buf[2]->path_info[curr_path_inf->prev].next = h; zusatz++; } /* old which link to prev has to be checked */ if ((ref_path_inf->prev != PREV_NONE) && \ (ref_path_inf->next == NEXT_NONE) ) { vec_copy(X[0], fb->buf[0]->path_info[curr_path_inf->next].x); vec_copy(X[1], curr_path_inf->x); vec_copy(X[3], ref_path_inf->x); vec_copy(X[4], fb->buf[3]->path_info[ref_path_inf->prev].x); for (j = 0; j < 3; j++) X[5][j] = 0.5*(5.0*X[3][j] - 4.0*X[1][j] + X[0][j]); angle_acc(X[3], X[4], X[5], &angle, &acc); if ( (acc<tpar->dacc && angle<tpar->dangle) || (acc<tpar->dacc/10) ) { curr_path_inf->finaldecis = curr_path_inf->decis[0]; curr_path_inf->prev = curr_path_inf->linkdecis[0]; fb->buf[2]->path_info[curr_path_inf->prev].next = h; zusatz++; } } } if (curr_path_inf->prev != -1 ) count1++; } /* end of creation of links with decision check */ sprintf (buf, "step: %d, curr: %d, next: %d, links: %d, lost: %d, add: %d", step, fb->buf[1]->num_parts, fb->buf[2]->num_parts, count1, fb->buf[1]->num_parts - count1, zusatz); /* for the average of particles and links */ npart = npart + fb->buf[1]->num_parts; nlinks = nlinks + count1; fb_next(fb); fb_write_frame_from_start(fb, step); if(step > seq_par->first + 2) { fb_read_frame_at_end(fb, step - 3, 1); } } /* end of sequence loop */ /* average of all steps */ npart /= (seq_par->last - seq_par->first - 1); nlinks /= (seq_par->last - seq_par->first - 1); printf ("Average over sequence, particles: %5.1f, links: %5.1f, lost: %5.1f\n", npart, nlinks, npart-nlinks); fb_next(fb); fb_write_frame_from_start(fb, step); fb_free(fb); free(fb); free(tpar); /* reset of display flag */ display = 1; }
void trackcorr_c_loop (tracking_run *run_info, int step, int display) { /* sequence loop */ char val[256], buf[256]; int j, h, k, mm, kk, invol=0; int zaehler1, zaehler2, philf[4][MAX_CANDS]; int count1=0, count2=0, count3=0, zusatz=0; int intx0, intx1, inty0, inty1; int intx2, inty2; int quali=0; vec3d diff_pos, X[7]; /* 7 reference points used in the algorithm, TODO: check if can reuse some */ double x1[4], y1[4], x2[4], y2[4], angle, acc, angle0, acc0, dl; double xr[4], xl[4], yd[4], yu[4], angle1, acc1; double xp[4], yp[4], xc[4], yc[4], xn[4], yn[4]; double rr; int flag_m_tr=0; /* Shortcuts to inside current frame */ P *curr_path_inf, *ref_path_inf; corres *curr_corres, *ref_corres; target **curr_targets, **ref_targets; int _ix; /* For use in any of the complex index expressions below */ int _frame_parts; /* number of particles in a frame */ /* Shortcuts into the tracking_run struct */ framebuf *fb; track_par *tpar; volume_par *vpar; control_par *cpar; /* Remaining globals: all those in trackcorr_c_init. calibration globals. */ foundpix *w, *wn, p16[4*MAX_CANDS]; sprintf (buf, "Time step: %d, seqnr: %d, Particle info:", step - run_info->seq_par->first, step); count1=0; zusatz=0; fb = run_info->fb; tpar = run_info->tpar; vpar = run_info->vpar; cpar = run_info->cpar; curr_targets = fb->buf[1]->targets; /* try to track correspondences from previous 0 - corp, variable h */ for (h = 0; h < fb->buf[1]->num_parts; h++) { for (j = 0; j < 7; j++) vec_init(X[j]); curr_path_inf = &(fb->buf[1]->path_info[h]); curr_corres = &(fb->buf[1]->correspond[h]); curr_path_inf->inlist = 0; reset_foundpix_array(p16, 16, fb->num_cams); /* 3D-position */ vec_copy(X[1], curr_path_inf->x); /* use information from previous to locate new search position and to calculate values for search area */ if (curr_path_inf->prev >= 0) { ref_path_inf = &(fb->buf[0]->path_info[curr_path_inf->prev]); vec_copy(X[0], ref_path_inf->x); search_volume_center_moving(ref_path_inf->x, curr_path_inf->x, X[2]); for (j = 0; j < fb->num_cams; j++) { img_coord (j, X[2][0], X[2][1], X[2][2], Ex[j],I[j], G[j], ap[j], *(cpar->mm), &xn[j], &yn[j]); metric_to_pixel(&x1[j], &y1[j], xn[j], yn[j], cpar); } } else { vec_copy(X[2], X[1]); for (j=0; j < fb->num_cams; j++) { if (curr_corres->p[j] == -1) { img_coord (j, X[2][0], X[2][1], X[2][2], Ex[j],I[j], G[j], ap[j], *(cpar->mm), &xn[j], &yn[j]); metric_to_pixel(&x1[j], &y1[j], xn[j], yn[j], cpar); } else { _ix = curr_corres->p[j]; x1[j] = curr_targets[j][_ix].x; y1[j] = curr_targets[j][_ix].y; } } } /* calculate searchquader and reprojection in image space */ searchquader(X[2][0], X[2][1], X[2][2], xr, xl, yd, yu, tpar, cpar); /* search in pix for candidates in next time step */ for (j = 0; j < fb->num_cams; j++) { register_closest_neighbs(fb->buf[2]->targets[j], fb->buf[2]->num_targets[j], j, x1[j], y1[j], xl[j], xr[j], yu[j], yd[j], &p16[j*MAX_CANDS], cpar); } /* fill and sort candidate struct */ sortwhatfound(p16, &zaehler1, fb->num_cams); w = (foundpix *) calloc (zaehler1, sizeof (foundpix)); if (zaehler1 > 0) count2++; copy_foundpix_array(w, p16, zaehler1, fb->num_cams); /*end of candidate struct */ /* check for what was found */ for (mm=0; mm<zaehler1;mm++) { /* zaehler1-loop */ /* search for found corr of current the corr in next with predicted location */ reset_foundpix_array(p16, 16, fb->num_cams); /* found 3D-position */ ref_path_inf = &(fb->buf[2]->path_info[w[mm].ftnr]); vec_copy(X[3], ref_path_inf->x); if (curr_path_inf->prev >= 0) { for (j = 0; j < 3; j++) X[5][j] = 0.5*(5.0*X[3][j] - 4.0*X[1][j] + X[0][j]); } else { search_volume_center_moving(X[1], X[3], X[5]); } searchquader(X[5][0], X[5][1], X[5][2], xr, xl, yd, yu, tpar, cpar); for (j = 0; j < fb->num_cams; j++) { img_coord (j, X[5][0], X[5][1], X[5][2], Ex[j],I[j], G[j], ap[j], *(cpar->mm), &xn[j], &yn[j]); metric_to_pixel(&x2[j], &y2[j], xn[j], yn[j], cpar); } /* search for candidates in next time step */ for (j=0; j < fb->num_cams; j++) { zaehler2 = candsearch_in_pix (fb->buf[3]->targets[j], fb->buf[3]->num_targets[j], x1[j], y1[j], xl[j], xr[j], yu[j], yd[j], philf[j], cpar); for(k = 0; k < 4; k++) { if (philf[j][k] == -999) { p16[j*4+k].ftnr=-1; } else { if (fb->buf[3]->targets[j][philf[j][k]].tnr != -1) { _ix = philf[j][k]; p16[j*4+k].ftnr = fb->buf[3]->targets[j][_ix].tnr; p16[j*4+k].whichcam[j] = 1; } } } } /* end of search in pix */ /* fill and sort candidate struct */ sortwhatfound(p16, &zaehler2, fb->num_cams); wn = (foundpix *) calloc (zaehler2, sizeof (foundpix)); if (zaehler2 > 0) count3++; copy_foundpix_array(wn, p16, zaehler2, fb->num_cams); /*end of candidate struct */ /* ************************************************ */ for (kk=0; kk < zaehler2; kk++) { /* zaehler2-loop */ ref_path_inf = &(fb->buf[3]->path_info[wn[kk].ftnr]); vec_copy(X[4], ref_path_inf->x); vec_subt(X[4], X[3], diff_pos); if ( pos3d_in_bounds(diff_pos, tpar)) { angle_acc(X[3], X[4], X[5], &angle1, &acc1); if (curr_path_inf->prev >= 0) { angle_acc(X[1], X[2], X[3], &angle0, &acc0); } else { acc0=acc1; angle0=angle1; } acc=(acc0+acc1)/2; angle=(angle0+angle1)/2; quali=wn[kk].freq+w[mm].freq; if ((acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10)) { dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[4], X[3]) )/2; rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle)/(quali); register_link_candidate(curr_path_inf, rr, w[mm].ftnr); } } } /* end of zaehler2-loop */ /* creating new particle position */ /* *************************************************************** */ for (j = 0;j < fb->num_cams; j++) { img_coord (j, X[5][0], X[5][1], X[5][2], Ex[j],I[j], G[j], ap[j], *(cpar->mm), &xn[j], &yn[j]); metric_to_pixel(&xn[j], &yn[j], xn[j], yn[j], cpar); } /* reset img coord because of num_cams < 4 */ for (j=0;j < fb->num_cams; j++) { x2[j]=-1e10; y2[j]=-1e10; } /* search for unused candidates in next time step */ for (j = 0;j < fb->num_cams; j++) { /* use fix distance to define xl, xr, yu, yd instead of searchquader */ xl[j]= xr[j]= yu[j]= yd[j] = 3.0; zaehler2 = candsearch_in_pixrest (fb->buf[3]->targets[j], fb->buf[3]->num_targets[j], xn[j], yn[j], xl[j], xr[j], yu[j], yd[j], philf[j], cpar); if(zaehler2>0 ) { _ix = philf[j][0]; x2[j] = fb->buf[3]->targets[j][_ix].x; y2[j] = fb->buf[3]->targets[j][_ix].y; } } quali=0; for (j = 0; j < fb->num_cams; j++) { if (x2[j] != -1e10 && y2[j] != -1e10) { pixel_to_metric(&x2[j],&y2[j], x2[j],y2[j], cpar); quali++; } } if ( quali >= 2) { vec_copy(X[4], X[5]); invol=0; det_lsq_3d (glob_cal, *(cpar->mm), x2[0], y2[0], x2[1], y2[1], x2[2], y2[2], x2[3], y2[3], &(X[4][0]), &(X[4][1]), &(X[4][2]), fb->num_cams); /* volume check */ if ( vpar->X_lay[0] < X[4][0] && X[4][0] < vpar->X_lay[1] && run_info->ymin < X[4][1] && X[4][1] < run_info->ymax && vpar->Zmin_lay[0] < X[4][2] && X[4][2] < vpar->Zmax_lay[1]) {invol=1;} vec_subt(X[3], X[4], diff_pos); if ( invol == 1 && pos3d_in_bounds(diff_pos, tpar) ) { angle_acc(X[3], X[4], X[5], &angle, &acc); if ((acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10)) { dl=(vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[4], X[3]) )/2; rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle) / (quali+w[mm].freq); register_link_candidate(curr_path_inf, rr, w[mm].ftnr); if (tpar->add) { ref_path_inf = &(fb->buf[3]->path_info[ fb->buf[3]->num_parts]); vec_copy(ref_path_inf->x, X[4]); reset_links(ref_path_inf); _frame_parts = fb->buf[3]->num_parts; ref_corres = &(fb->buf[3]->correspond[_frame_parts]); ref_targets = fb->buf[3]->targets; for (j = 0; j < fb->num_cams; j++) { ref_corres->p[j]=-1; if(philf[j][0]!=-999) { _ix = philf[j][0]; ref_targets[j][_ix].tnr = _frame_parts; ref_corres->p[j] = _ix; ref_corres->nr = _frame_parts; } } fb->buf[3]->num_parts++; zusatz++; } } } invol=0; } quali=0; /* end of creating new particle position */ /* *************************************************************** */ /* try to link if kk is not found/good enough and prev exist */ if ( curr_path_inf->inlist == 0 && curr_path_inf->prev >= 0 ) { vec_subt(X[3], X[1], diff_pos); if (pos3d_in_bounds(diff_pos, tpar)) { angle_acc(X[1], X[2], X[3], &angle, &acc); if ( (acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10) ) { quali = w[mm].freq; dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) )/2; rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle)/(quali); register_link_candidate(curr_path_inf, rr, w[mm].ftnr); } } } free(wn); } /* end of zaehler1-loop */ /* begin of inlist still zero */ if (tpar->add) { if ( curr_path_inf->inlist == 0 && curr_path_inf->prev >= 0 ) { for (j = 0; j < fb->num_cams; j++) { img_coord (j, X[2][0], X[2][1], X[2][2], Ex[j],I[j], G[j], ap[j], *(cpar->mm), &xn[j], &yn[j]); metric_to_pixel(&xn[j], &yn[j], xn[j], yn[j], cpar); x2[j]=-1e10; y2[j]=-1e10; } /* search for unused candidates in next time step */ for (j = 0; j < fb->num_cams; j++) { /*use fix distance to define xl, xr, yu, yd instead of searchquader */ xl[j]= xr[j]= yu[j]= yd[j] = 3.0; zaehler2 = candsearch_in_pixrest (fb->buf[2]->targets[j], fb->buf[2]->num_targets[j], xn[j], yn[j], xl[j], xr[j], yu[j], yd[j], philf[j], cpar); if(zaehler2 > 0) { _ix = philf[j][0]; x2[j] = fb->buf[2]->targets[j][_ix].x; y2[j] = fb->buf[2]->targets[j][_ix].y; } } quali=0; for (j = 0; j < fb->num_cams; j++) { if (x2[j] !=-1e10 && y2[j] != -1e10) { pixel_to_metric(&x2[j], &y2[j], x2[j], y2[j], cpar); quali++; } } if (quali>=2) { vec_copy(X[3], X[2]); invol=0; det_lsq_3d (glob_cal, *(cpar->mm), x2[0], y2[0], x2[1], y2[1], x2[2], y2[2], x2[3], y2[3], &(X[3][0]), &(X[3][1]), &(X[3][2]), fb->num_cams); /* in volume check */ if ( vpar->X_lay[0] < X[3][0] && X[3][0] < vpar->X_lay[1] && run_info->ymin < X[3][1] && X[3][1] < run_info->ymax && vpar->Zmin_lay[0] < X[3][2] && X[3][2] < vpar->Zmax_lay[1]) {invol = 1;} vec_subt(X[2], X[3], diff_pos); if ( invol == 1 && pos3d_in_bounds(diff_pos, tpar) ) { angle_acc(X[1], X[2], X[3], &angle, &acc); if ( (acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10) ) { dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) )/2; rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle)/(quali); ref_path_inf = &(fb->buf[2]->path_info[ fb->buf[2]->num_parts]); vec_copy(ref_path_inf->x, X[3]); reset_links(ref_path_inf); _frame_parts = fb->buf[2]->num_parts; register_link_candidate(curr_path_inf, rr, _frame_parts); ref_corres = &(fb->buf[2]->correspond[_frame_parts]); ref_targets = fb->buf[2]->targets; for (j = 0;j < fb->num_cams; j++) { ref_corres->p[j]=-1; if(philf[j][0]!=-999) { _ix = philf[j][0]; ref_targets[j][_ix].tnr = _frame_parts; ref_corres->p[j] = _ix; ref_corres->nr = _frame_parts; } } fb->buf[2]->num_parts++; zusatz++; } } invol=0; } // if quali >= 2 } } /* end of inlist still zero */ /***********************************/ free(w); } /* end of h-loop */ /* sort decis and give preliminary "finaldecis" */ for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); if(curr_path_inf->inlist > 0 ) { sort(curr_path_inf->inlist, (float *) curr_path_inf->decis, curr_path_inf->linkdecis); curr_path_inf->finaldecis = curr_path_inf->decis[0]; curr_path_inf->next = curr_path_inf->linkdecis[0]; } } /* create links with decision check */ for (h = 0;h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); if(curr_path_inf->inlist > 0 ) { ref_path_inf = &(fb->buf[2]->path_info[curr_path_inf->next]); if (ref_path_inf->prev == -1) { /* best choice wasn't used yet, so link is created */ ref_path_inf->prev = h; } else { /* best choice was already used by mega[2][mega[1][h].next].prev */ /* check which is the better choice */ if ( fb->buf[1]->path_info[ref_path_inf->prev].finaldecis > \ curr_path_inf->finaldecis) { /* remove link with prev */ fb->buf[1]->path_info[ref_path_inf->prev].next = NEXT_NONE; ref_path_inf->prev = h; } else { curr_path_inf->next = NEXT_NONE; } } } if (curr_path_inf->next != -2 ) count1++; } /* end of creation of links with decision check */ /* ******** Draw links now ******** */ m1_tr = 0; if (display) { for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); curr_corres = &(fb->buf[1]->correspond[h]); ref_corres = &(fb->buf[2]->correspond[curr_path_inf->next]); if (curr_path_inf->next != -2 ) { strcpy(buf,""); sprintf(buf ,"green"); for (j = 0; j < fb->num_cams; j++) { if (curr_corres->p[j] > 0 && ref_corres->p[j] > 0) { flag_m_tr=1; xp[j] = curr_targets[j][curr_corres->p[j]].x; yp[j] = curr_targets[j][curr_corres->p[j]].y; xc[j] = fb->buf[2]->targets[j][ref_corres->p[j]].x; yc[j] = fb->buf[2]->targets[j][ref_corres->p[j]].y; predict (xp[j], yp[j], xc[j], yc[j], &xn[j], &yn[j]); if ( ( fabs(xp[j]-zoom_x[j]) < cpar->imx/(2*zoom_f[j])) && (fabs(yp[j]-zoom_y[j]) < cpar->imy/(2*zoom_f[j]))) { strcpy(val,""); sprintf(val ,"orange"); intx0 = (int)(cpar->imx/2 + zoom_f[j]*(xp[j] - zoom_x[j])); inty0 = (int)(cpar->imy/2 + zoom_f[j]*(yp[j] - zoom_y[j])); intx1 = (int)(cpar->imx/2 + zoom_f[j]*(xc[j] - zoom_x[j])); inty1 = (int)(cpar->imy/2 + zoom_f[j]*(yc[j] - zoom_y[j])); intx2 = (int)(cpar->imx/2 + zoom_f[j]*(xn[j] - zoom_x[j])); inty2 = (int)(cpar->imy/2 + zoom_f[j]*(yn[j] - zoom_y[j])); intx0_tr[j][m1_tr]=intx0; inty0_tr[j][m1_tr]=inty0; intx1_tr[j][m1_tr]=intx1; inty1_tr[j][m1_tr]=inty1; intx2_tr[j][m1_tr]=intx2; inty2_tr[j][m1_tr]=inty2; pnr1_tr[j][m1_tr]=-1; pnr2_tr[j][m1_tr]=-1; pnr3_tr[j][m1_tr]=-1; if (curr_path_inf->finaldecis > 0.2) { pnr1_tr[j][m1_tr] = h; pnr2_tr[j][m1_tr] = curr_path_inf->next; pnr3_tr[j][m1_tr] = curr_path_inf->finaldecis; } } } if (flag_m_tr==0) { intx0_tr[j][m1_tr]=0; inty0_tr[j][m1_tr]=0; intx1_tr[j][m1_tr]=0; inty1_tr[j][m1_tr]=0; intx2_tr[j][m1_tr]=0; inty2_tr[j][m1_tr]=0; pnr1_tr[j][m1_tr]=-1; pnr2_tr[j][m1_tr]=-1; pnr3_tr[j][m1_tr]=-1; } flag_m_tr=0; } m1_tr++; } } } /* ******** End of Draw links now ******** */ sprintf (buf, "step: %d, curr: %d, next: %d, links: %d, lost: %d, add: %d", step, fb->buf[1]->num_parts, fb->buf[2]->num_parts, count1, fb->buf[1]->num_parts - count1, zusatz); /* for the average of particles and links */ npart = npart + fb->buf[1]->num_parts; nlinks = nlinks + count1; fb_next(fb); fb_write_frame_from_start(fb, step); if(step < run_info->seq_par->last - 2) { fb_read_frame_at_end(fb, step + 3, 0); } } /* end of sequence loop */
int vec_ort(double a[], double b[]) { double proj[3]; vec_proj_wc(proj,a,b); vec_subt(a,proj); }