Exemplo n.º 1
0
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);
}
Exemplo n.º 2
0
/* 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));
    }
}
Exemplo n.º 3
0
/*  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;
}
Exemplo n.º 4
0
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));
}
Exemplo n.º 5
0
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);
}
Exemplo n.º 6
0
/*  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));
}
Exemplo n.º 7
0
/*     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;
}
Exemplo n.º 8
0
/* 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 */
Exemplo n.º 9
0
/* 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);

}
Exemplo n.º 10
0
// 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);
}
Exemplo n.º 11
0
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);
}
Exemplo n.º 12
0
/*     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;
}
Exemplo n.º 13
0
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 */
Exemplo n.º 14
0
int vec_ort(double a[], double b[]) {
    double proj[3];
    vec_proj_wc(proj,a,b);
    vec_subt(a,proj);
}