/* 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_diff_norm)
{
int i;
vec3d vec1 = {1., 2., 3.}, vec2 = {4., 5., 6.};
fail_unless(vec_diff_norm(vec1, vec2) == sqrt(3)*3);
}
/* 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));
}
}
/* 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 */
/* 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 */
