END_TEST START_TEST(test_pixel_to_metric) { /* input */ double xc = 0.0; // [mm] double yc = 0.0; // [mm] control_par cpar; /* output */ double xp, yp; cpar.imx = 1024; cpar.imy = 1008; cpar.pix_x = 0.01; cpar.pix_y = 0.01; cpar.chfield = 0; /* compare the xc, yc to the original */ double xc1, yc1; metric_to_pixel (&xp, &yp, xc, yc, &cpar); pixel_to_metric (&xc1, &yc1, xp, yp, &cpar); ck_assert_msg( fabs(xc1 - xc) < EPS && fabs(yc1 - yc) < EPS, "Expected %f, %f but got %f %f\n", xc, yc, xc1, yc1); xc = 1.0; yc = 0.0; metric_to_pixel (&xp, &yp, xc, yc, &cpar); pixel_to_metric (&xc1, &yc1, xp, yp, &cpar); ck_assert_msg( fabs(xc1 - xc) < EPS && fabs(yc1 - yc) < EPS, "Expected %f, %f, but got %f %f\n", xc,yc,xc1, yc1); xc = 0.0; yc = -1.0; metric_to_pixel (&xp, &yp, xc, yc, &cpar); pixel_to_metric (&xc1, &yc1, xp, yp, &cpar); ck_assert_msg( fabs(xc1 - xc) < EPS && fabs(yc1 - yc) < EPS, "Expected %f, %f, but got %f %f\n", xc,yc,xc1, yc1); }
void searchquader(double X, double Y, double Z, double xr[4], double xl[4], double yd[4], double yu[4]) { int k, i; double x, y, xz, yz; coord_3d quader[8], point; /* project quader in image space to define search area */ for (k=0; k<8; k++) { quader[k].pnr=k; } /* calculation of quader points */ point.pnr=0; point.x=X; point.y=Y; point.z=Z; quader[0].x=X+tpar.dvxmin; quader[0].y=Y+tpar.dvymin; quader[0].z=Z+tpar.dvzmin; /* --- */ quader[1].x=X+tpar.dvxmax; quader[1].y=Y+tpar.dvymin; quader[1].z=Z+tpar.dvzmin; /* +-- */ quader[2].x=X+tpar.dvxmin; quader[2].y=Y+tpar.dvymax; quader[2].z=Z+tpar.dvzmin; /* -+- */ quader[3].x=X+tpar.dvxmin; quader[3].y=Y+tpar.dvymin; quader[3].z=Z+tpar.dvzmax; /* --+ */ //changed by Beat and JuliAn Nov 2008 quader[4].x=X+tpar.dvxmax; quader[4].y=Y+tpar.dvymax; quader[4].z=Z+tpar.dvzmin; /* ++- */ quader[5].x=X+tpar.dvxmax; quader[5].y=Y+tpar.dvymin; quader[5].z=Z+tpar.dvzmax; /* +-+ */ quader[6].x=X+tpar.dvxmin; quader[6].y=Y+tpar.dvymax; quader[6].z=Z+tpar.dvzmax; /* -++ */ quader[7].x=X+tpar.dvxmax; quader[7].y=Y+tpar.dvymax; quader[7].z=Z+tpar.dvzmax; /* +++ */ /* calculation of search area */ for (i=0; i<n_img; i++) { xr[i]=0; xl[i]=imx; yd[i]=0; yu[i]=imy; img_coord (point.x, point.y, point.z, Ex[i], I[i], G[i], ap[i], mmp, &xz,&yz); metric_to_pixel (xz,yz, imx,imy, pix_x,pix_y, &xz,&yz, chfield); for (k=0; k<8; k++) { img_coord (quader[k].x, quader[k].y, quader[k].z, Ex[i], I[i], G[i], ap[i], mmp, &x,&y); metric_to_pixel (x,y, imx,imy, pix_x,pix_y, &x,&y, chfield); if (x <xl[i] ) xl[i]=x; if (y <yu[i] ) yu[i]=y; if (x >xr[i] ) xr[i]=x; if (y >yd[i] ) yd[i]=y; } if (xl[i] < 0 ) xl[i]=0; if (yu[i] < 0 ) yu[i]=0; if (xr[i] > imx) xr[i]=imx; if (yd[i] > imy) yd[i]=imy; xr[i]=xr[i]-xz; xl[i]=xz-xl[i]; yd[i]=yd[i]-yz; yu[i]=yz-yu[i]; } return; }
END_TEST START_TEST(test_metric_to_pixel) { /* input */ double xc = 0.0; // [mm] double yc = 0.0; // [mm] control_par cpar; /* output */ double xp, yp; cpar.imx = 1024; cpar.imy = 1008; cpar.pix_x = 0.01; cpar.pix_y = 0.01; cpar.chfield = 0; metric_to_pixel (&xp, &yp, xc, yc, &cpar); ck_assert_msg( fabs(xp - 512.0) < EPS && fabs(yp - 504.0) < EPS, "Expected 512.0, 504.0, but got %f %f\n", xp, yp); xc = 1.0; yc = 0.0; metric_to_pixel (&xp, &yp, xc, yc, &cpar); ck_assert_msg( fabs(xp - 612.0) < EPS && fabs(yp - 504.0) < EPS, "Expected 612.0, 504.0, but got %f %f\n", xp, yp); xc = 0.0; yc = -1.0; metric_to_pixel (&xp, &yp, xc, yc, &cpar); ck_assert_msg( fabs(xp - 512.0) < EPS && fabs(yp - 604.0) < EPS, "Expected 512.0, 604.0, but got %f %f\n", xp, yp); }
/* point_to_pixel is just a shortcut to two lines that repeat every so in track loop * img_coord (from 3d point to a 2d vector in metric units), followed by * metric_to_pixel (from 2d vector in metric units to the pixel position in the camera) * Arguments: * vec3d point in 3D space * Calibration *cal parameters * Control parameters (num cams, multimedia parameters, cpar->mm, etc.) * Returns (as a first argument): * vec2d with pixel positions (x,y) in the camera. */ void point_to_pixel (vec2d v1, vec3d point, Calibration *cal, control_par *cpar){ img_coord(point, cal, cpar->mm, &v1[0], &v1[1]); metric_to_pixel(&v1[0], &v1[1], v1[0], v1[1], cpar); }
/* track backwards */ void trackback_c () { char buf[256]; int i, j, h, k, step, invol=0; int zaehler1, philf[4][MAX_CANDS]; int count1=0, count2=0, zusatz=0; int quali=0; double x2[4], y2[4], angle, acc, lmax, dl; double xr[4], xl[4], yd[4], yu[4]; vec3d diff_pos, X[7]; /* 7 reference points used in the algorithm, TODO: check if can reuse some */ double xn[4], yn[4]; double rr, Ymin=0, Ymax=0; double npart=0, nlinks=0; foundpix *w, p16[4*MAX_CANDS]; sequence_par *seq_par; track_par *tpar; volume_par *vpar; control_par *cpar; framebuf *fb; /* Shortcuts to inside current frame */ P *curr_path_inf, *ref_path_inf; corres *ref_corres; target **ref_targets; int _ix; /* For use in any of the complex index expressions below */ int _frame_parts; /* number of particles in a frame */ display = 1; /* read data */ seq_par = read_sequence_par("parameters/sequence.par", 4); tpar = read_track_par("parameters/track.par"); vpar = read_volume_par("parameters/criteria.par"); cpar = read_control_par("parameters/ptv.par"); fb = (framebuf *) malloc(sizeof(framebuf)); fb_init(fb, 4, cpar->num_cams, MAX_TARGETS, "res/rt_is", "res/ptv_is", "res/added", seq_par->img_base_name); /* Prime the buffer with first frames */ for (step = seq_par->last; step > seq_par->last - 4; step--) { fb_read_frame_at_end(fb, step, 1); fb_next(fb); } fb_prev(fb); lmax = norm((tpar->dvxmin - tpar->dvxmax), (tpar->dvymin - tpar->dvymax), (tpar->dvzmin - tpar->dvzmax)); volumedimension (glob_cal, &(vpar->X_lay[1]), &(vpar->X_lay[0]), &Ymax, &Ymin, &(vpar->Zmax_lay[1]), &(vpar->Zmin_lay[0]), cpar); /* sequence loop */ for (step = seq_par->last - 1; step > seq_par->first; step--) { sprintf (buf, "Time step: %d, seqnr: %d, Particle info:", step - seq_par->first, step); for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); /* We try to find link only if the forward search failed to. */ if ((curr_path_inf->next < 0) || (curr_path_inf->prev != -1)) continue; for (j = 0; j < 7; j++) vec_init(X[j]); curr_path_inf->inlist = 0; reset_foundpix_array(p16, 16, fb->num_cams); /* 3D-position of current particle */ vec_copy(X[1], curr_path_inf->x); /* use information from previous to locate new search position and to calculate values for search area */ ref_path_inf = &(fb->buf[0]->path_info[curr_path_inf->next]); vec_copy(X[0], ref_path_inf->x); search_volume_center_moving(ref_path_inf->x, curr_path_inf->x, X[2]); for (j=0; j < fb->num_cams; j++) { img_coord (j, X[2][0], X[2][1], X[2][2], Ex[j],I[j], G[j], ap[j], *(cpar->mm), &xn[j], &yn[j]); metric_to_pixel(&xn[j], &yn[j], xn[j], yn[j], cpar); } /* calculate searchquader and reprojection in image space */ searchquader(X[2][0], X[2][1], X[2][2], xr, xl, yd, yu, tpar, cpar); for (j = 0; j < fb->num_cams; j++) { zaehler1 = candsearch_in_pix ( fb->buf[2]->targets[j], fb->buf[2]->num_targets[j], xn[j], yn[j], xl[j], xr[j], yu[j], yd[j], philf[j], cpar); for(k=0; k<4; k++) { if( zaehler1>0) { if (philf[j][k] == -999){ p16[j*4+k].ftnr=-1; } else { p16[j*4+k].ftnr=fb->buf[3]->targets[j][philf[j][k]].tnr; p16[j*4+k].whichcam[j]=1; } } } } /* search in pix for candidates in next time step */ //for (j = 0; j < fb->num_cams; j++) { // register_closest_neighbs(fb->buf[2]->targets[j], // fb->buf[2]->num_targets[j], j, xn[j], yn[j], // xl[j], xr[j], yu[j], yd[j], &p16[j*MAX_CANDS]); //} /* fill and sort candidate struct */ sortwhatfound(p16, &zaehler1, fb->num_cams); w = (foundpix *) calloc (zaehler1, sizeof (foundpix)); /*end of candidate struct */ if (zaehler1 > 0) count2++; copy_foundpix_array(w, p16, zaehler1, fb->num_cams); for (i = 0; i < zaehler1; i++) { ref_path_inf = &(fb->buf[2]->path_info[w[i].ftnr]); vec_copy(X[3], ref_path_inf->x); vec_subt(X[1], X[3], diff_pos); if (pos3d_in_bounds(diff_pos, tpar)) { angle_acc(X[1], X[2], X[3], &angle, &acc); /* *********************check link *****************************/ if ((acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10)) { dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) )/2; quali=w[i].freq; rr = (dl/lmax + acc/tpar->dacc + angle/tpar->dangle)/quali; register_link_candidate(curr_path_inf, rr, w[i].ftnr); } } } free(w); /******************/ quali=0; /* reset img coord because num_cams < 4 */ for (j=0;j<4;j++) { x2[j]=-1e10; y2[j]=-1e10;} /* if old wasn't found try to create new particle position from rest */ if (tpar->add) { if ( curr_path_inf->inlist == 0) { for (j = 0; j < fb->num_cams; j++) { /* use fix distance to define xl, xr, yu, yd instead of searchquader */ xl[j]= xr[j]= yu[j]= yd[j] = 3.0; zaehler1 = candsearch_in_pixrest (fb->buf[2]->targets[j], fb->buf[2]->num_targets[j], xn[j], yn[j], xl[j], xr[j], yu[j], yd[j], philf[j], cpar); if(zaehler1 > 0) { _ix = philf[j][0]; x2[j] = fb->buf[2]->targets[j][_ix].x; y2[j] = fb->buf[2]->targets[j][_ix].y; } } for (j = 0; j < fb->num_cams; j++) { if (x2[j] !=-1e10 && y2[j] != -1e10) { pixel_to_metric(&x2[j], &y2[j], x2[j],y2[j], cpar); quali++; } } if (quali>=2) { vec_copy(X[3], X[2]); invol=0; det_lsq_3d (glob_cal, *(cpar->mm), x2[0], y2[0], x2[1], y2[1], x2[2], y2[2], x2[3], y2[3], &(X[3][0]), &(X[3][1]), &(X[3][2]), fb->num_cams); /* volume check */ if ( vpar->X_lay[0] < X[3][0] && X[3][0] < vpar->X_lay[1] && Ymin < X[3][1] && X[3][1] < Ymax && vpar->Zmin_lay[0] < X[3][2] && X[3][2] < vpar->Zmax_lay[1]) {invol = 1;} vec_subt(X[1], X[3], diff_pos); if (invol == 1 && pos3d_in_bounds(diff_pos, tpar)) { angle_acc(X[1], X[2], X[3], &angle, &acc); if ( (acc<tpar->dacc && angle<tpar->dangle) || \ (acc<tpar->dacc/10) ) { dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) )/2; rr =(dl/lmax+acc/tpar->dacc + angle/tpar->dangle)/(quali); ref_path_inf = &(fb->buf[2]->path_info[ fb->buf[2]->num_parts]); vec_copy(ref_path_inf->x, X[3]); reset_links(ref_path_inf); _frame_parts = fb->buf[2]->num_parts; register_link_candidate(curr_path_inf, rr, _frame_parts); ref_corres = &(fb->buf[2]->correspond[_frame_parts]); ref_targets = fb->buf[2]->targets; for (j = 0;j < fb->num_cams; j++) { ref_corres->p[j]=-1; if(philf[j][0]!=-999) { _ix = philf[j][0]; ref_targets[j][_ix].tnr = _frame_parts; ref_corres->p[j] = _ix; ref_corres->nr = _frame_parts; } } fb->buf[2]->num_parts++; } } invol=0; } } } /* end of if old wasn't found try to create new particle position from rest */ } /* end of h-loop */ for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); if(curr_path_inf->inlist > 0 ) { sort(curr_path_inf->inlist, (float *)curr_path_inf->decis, curr_path_inf->linkdecis); } } /* create links with decision check */ count1=0; zusatz=0; for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); if (curr_path_inf->inlist > 0 ) { /* if old/new and unused prev == -1 and next == -2 link is created */ ref_path_inf = &(fb->buf[2]->path_info[curr_path_inf->linkdecis[0]]); if ( ref_path_inf->prev == PREV_NONE && \ ref_path_inf->next == NEXT_NONE ) { curr_path_inf->finaldecis = curr_path_inf->decis[0]; curr_path_inf->prev = curr_path_inf->linkdecis[0]; fb->buf[2]->path_info[curr_path_inf->prev].next = h; zusatz++; } /* old which link to prev has to be checked */ if ((ref_path_inf->prev != PREV_NONE) && \ (ref_path_inf->next == NEXT_NONE) ) { vec_copy(X[0], fb->buf[0]->path_info[curr_path_inf->next].x); vec_copy(X[1], curr_path_inf->x); vec_copy(X[3], ref_path_inf->x); vec_copy(X[4], fb->buf[3]->path_info[ref_path_inf->prev].x); for (j = 0; j < 3; j++) X[5][j] = 0.5*(5.0*X[3][j] - 4.0*X[1][j] + X[0][j]); angle_acc(X[3], X[4], X[5], &angle, &acc); if ( (acc<tpar->dacc && angle<tpar->dangle) || (acc<tpar->dacc/10) ) { curr_path_inf->finaldecis = curr_path_inf->decis[0]; curr_path_inf->prev = curr_path_inf->linkdecis[0]; fb->buf[2]->path_info[curr_path_inf->prev].next = h; zusatz++; } } } if (curr_path_inf->prev != -1 ) count1++; } /* end of creation of links with decision check */ sprintf (buf, "step: %d, curr: %d, next: %d, links: %d, lost: %d, add: %d", step, fb->buf[1]->num_parts, fb->buf[2]->num_parts, count1, fb->buf[1]->num_parts - count1, zusatz); /* for the average of particles and links */ npart = npart + fb->buf[1]->num_parts; nlinks = nlinks + count1; fb_next(fb); fb_write_frame_from_start(fb, step); if(step > seq_par->first + 2) { fb_read_frame_at_end(fb, step - 3, 1); } } /* end of sequence loop */ /* average of all steps */ npart /= (seq_par->last - seq_par->first - 1); nlinks /= (seq_par->last - seq_par->first - 1); printf ("Average over sequence, particles: %5.1f, links: %5.1f, lost: %5.1f\n", npart, nlinks, npart-nlinks); fb_next(fb); fb_write_frame_from_start(fb, step); fb_free(fb); free(fb); free(tpar); /* reset of display flag */ display = 1; }
void trackcorr_c_loop (tracking_run *run_info, int step, int display) { /* sequence loop */ char val[256], buf[256]; int j, h, k, mm, kk, invol=0; int zaehler1, zaehler2, philf[4][MAX_CANDS]; int count1=0, count2=0, count3=0, zusatz=0; int intx0, intx1, inty0, inty1; int intx2, inty2; int quali=0; vec3d diff_pos, X[7]; /* 7 reference points used in the algorithm, TODO: check if can reuse some */ double x1[4], y1[4], x2[4], y2[4], angle, acc, angle0, acc0, dl; double xr[4], xl[4], yd[4], yu[4], angle1, acc1; double xp[4], yp[4], xc[4], yc[4], xn[4], yn[4]; double rr; int flag_m_tr=0; /* Shortcuts to inside current frame */ P *curr_path_inf, *ref_path_inf; corres *curr_corres, *ref_corres; target **curr_targets, **ref_targets; int _ix; /* For use in any of the complex index expressions below */ int _frame_parts; /* number of particles in a frame */ /* Shortcuts into the tracking_run struct */ framebuf *fb; track_par *tpar; volume_par *vpar; control_par *cpar; /* Remaining globals: all those in trackcorr_c_init. calibration globals. */ foundpix *w, *wn, p16[4*MAX_CANDS]; sprintf (buf, "Time step: %d, seqnr: %d, Particle info:", step - run_info->seq_par->first, step); count1=0; zusatz=0; fb = run_info->fb; tpar = run_info->tpar; vpar = run_info->vpar; cpar = run_info->cpar; curr_targets = fb->buf[1]->targets; /* try to track correspondences from previous 0 - corp, variable h */ for (h = 0; h < fb->buf[1]->num_parts; h++) { for (j = 0; j < 7; j++) vec_init(X[j]); curr_path_inf = &(fb->buf[1]->path_info[h]); curr_corres = &(fb->buf[1]->correspond[h]); curr_path_inf->inlist = 0; reset_foundpix_array(p16, 16, fb->num_cams); /* 3D-position */ vec_copy(X[1], curr_path_inf->x); /* use information from previous to locate new search position and to calculate values for search area */ if (curr_path_inf->prev >= 0) { ref_path_inf = &(fb->buf[0]->path_info[curr_path_inf->prev]); vec_copy(X[0], ref_path_inf->x); search_volume_center_moving(ref_path_inf->x, curr_path_inf->x, X[2]); for (j = 0; j < fb->num_cams; j++) { img_coord (j, X[2][0], X[2][1], X[2][2], Ex[j],I[j], G[j], ap[j], *(cpar->mm), &xn[j], &yn[j]); metric_to_pixel(&x1[j], &y1[j], xn[j], yn[j], cpar); } } else { vec_copy(X[2], X[1]); for (j=0; j < fb->num_cams; j++) { if (curr_corres->p[j] == -1) { img_coord (j, X[2][0], X[2][1], X[2][2], Ex[j],I[j], G[j], ap[j], *(cpar->mm), &xn[j], &yn[j]); metric_to_pixel(&x1[j], &y1[j], xn[j], yn[j], cpar); } else { _ix = curr_corres->p[j]; x1[j] = curr_targets[j][_ix].x; y1[j] = curr_targets[j][_ix].y; } } } /* calculate searchquader and reprojection in image space */ searchquader(X[2][0], X[2][1], X[2][2], xr, xl, yd, yu, tpar, cpar); /* search in pix for candidates in next time step */ for (j = 0; j < fb->num_cams; j++) { register_closest_neighbs(fb->buf[2]->targets[j], fb->buf[2]->num_targets[j], j, x1[j], y1[j], xl[j], xr[j], yu[j], yd[j], &p16[j*MAX_CANDS], cpar); } /* fill and sort candidate struct */ sortwhatfound(p16, &zaehler1, fb->num_cams); w = (foundpix *) calloc (zaehler1, sizeof (foundpix)); if (zaehler1 > 0) count2++; copy_foundpix_array(w, p16, zaehler1, fb->num_cams); /*end of candidate struct */ /* check for what was found */ for (mm=0; mm<zaehler1;mm++) { /* zaehler1-loop */ /* search for found corr of current the corr in next with predicted location */ reset_foundpix_array(p16, 16, fb->num_cams); /* found 3D-position */ ref_path_inf = &(fb->buf[2]->path_info[w[mm].ftnr]); vec_copy(X[3], ref_path_inf->x); if (curr_path_inf->prev >= 0) { for (j = 0; j < 3; j++) X[5][j] = 0.5*(5.0*X[3][j] - 4.0*X[1][j] + X[0][j]); } else { search_volume_center_moving(X[1], X[3], X[5]); } searchquader(X[5][0], X[5][1], X[5][2], xr, xl, yd, yu, tpar, cpar); for (j = 0; j < fb->num_cams; j++) { img_coord (j, X[5][0], X[5][1], X[5][2], Ex[j],I[j], G[j], ap[j], *(cpar->mm), &xn[j], &yn[j]); metric_to_pixel(&x2[j], &y2[j], xn[j], yn[j], cpar); } /* search for candidates in next time step */ for (j=0; j < fb->num_cams; j++) { zaehler2 = candsearch_in_pix (fb->buf[3]->targets[j], fb->buf[3]->num_targets[j], x1[j], y1[j], xl[j], xr[j], yu[j], yd[j], philf[j], cpar); for(k = 0; k < 4; k++) { if (philf[j][k] == -999) { p16[j*4+k].ftnr=-1; } else { if (fb->buf[3]->targets[j][philf[j][k]].tnr != -1) { _ix = philf[j][k]; p16[j*4+k].ftnr = fb->buf[3]->targets[j][_ix].tnr; p16[j*4+k].whichcam[j] = 1; } } } } /* end of search in pix */ /* fill and sort candidate struct */ sortwhatfound(p16, &zaehler2, fb->num_cams); wn = (foundpix *) calloc (zaehler2, sizeof (foundpix)); if (zaehler2 > 0) count3++; copy_foundpix_array(wn, p16, zaehler2, fb->num_cams); /*end of candidate struct */ /* ************************************************ */ for (kk=0; kk < zaehler2; kk++) { /* zaehler2-loop */ ref_path_inf = &(fb->buf[3]->path_info[wn[kk].ftnr]); vec_copy(X[4], ref_path_inf->x); vec_subt(X[4], X[3], diff_pos); if ( pos3d_in_bounds(diff_pos, tpar)) { angle_acc(X[3], X[4], X[5], &angle1, &acc1); if (curr_path_inf->prev >= 0) { angle_acc(X[1], X[2], X[3], &angle0, &acc0); } else { acc0=acc1; angle0=angle1; } acc=(acc0+acc1)/2; angle=(angle0+angle1)/2; quali=wn[kk].freq+w[mm].freq; if ((acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10)) { dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[4], X[3]) )/2; rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle)/(quali); register_link_candidate(curr_path_inf, rr, w[mm].ftnr); } } } /* end of zaehler2-loop */ /* creating new particle position */ /* *************************************************************** */ for (j = 0;j < fb->num_cams; j++) { img_coord (j, X[5][0], X[5][1], X[5][2], Ex[j],I[j], G[j], ap[j], *(cpar->mm), &xn[j], &yn[j]); metric_to_pixel(&xn[j], &yn[j], xn[j], yn[j], cpar); } /* reset img coord because of num_cams < 4 */ for (j=0;j < fb->num_cams; j++) { x2[j]=-1e10; y2[j]=-1e10; } /* search for unused candidates in next time step */ for (j = 0;j < fb->num_cams; j++) { /* use fix distance to define xl, xr, yu, yd instead of searchquader */ xl[j]= xr[j]= yu[j]= yd[j] = 3.0; zaehler2 = candsearch_in_pixrest (fb->buf[3]->targets[j], fb->buf[3]->num_targets[j], xn[j], yn[j], xl[j], xr[j], yu[j], yd[j], philf[j], cpar); if(zaehler2>0 ) { _ix = philf[j][0]; x2[j] = fb->buf[3]->targets[j][_ix].x; y2[j] = fb->buf[3]->targets[j][_ix].y; } } quali=0; for (j = 0; j < fb->num_cams; j++) { if (x2[j] != -1e10 && y2[j] != -1e10) { pixel_to_metric(&x2[j],&y2[j], x2[j],y2[j], cpar); quali++; } } if ( quali >= 2) { vec_copy(X[4], X[5]); invol=0; det_lsq_3d (glob_cal, *(cpar->mm), x2[0], y2[0], x2[1], y2[1], x2[2], y2[2], x2[3], y2[3], &(X[4][0]), &(X[4][1]), &(X[4][2]), fb->num_cams); /* volume check */ if ( vpar->X_lay[0] < X[4][0] && X[4][0] < vpar->X_lay[1] && run_info->ymin < X[4][1] && X[4][1] < run_info->ymax && vpar->Zmin_lay[0] < X[4][2] && X[4][2] < vpar->Zmax_lay[1]) {invol=1;} vec_subt(X[3], X[4], diff_pos); if ( invol == 1 && pos3d_in_bounds(diff_pos, tpar) ) { angle_acc(X[3], X[4], X[5], &angle, &acc); if ((acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10)) { dl=(vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[4], X[3]) )/2; rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle) / (quali+w[mm].freq); register_link_candidate(curr_path_inf, rr, w[mm].ftnr); if (tpar->add) { ref_path_inf = &(fb->buf[3]->path_info[ fb->buf[3]->num_parts]); vec_copy(ref_path_inf->x, X[4]); reset_links(ref_path_inf); _frame_parts = fb->buf[3]->num_parts; ref_corres = &(fb->buf[3]->correspond[_frame_parts]); ref_targets = fb->buf[3]->targets; for (j = 0; j < fb->num_cams; j++) { ref_corres->p[j]=-1; if(philf[j][0]!=-999) { _ix = philf[j][0]; ref_targets[j][_ix].tnr = _frame_parts; ref_corres->p[j] = _ix; ref_corres->nr = _frame_parts; } } fb->buf[3]->num_parts++; zusatz++; } } } invol=0; } quali=0; /* end of creating new particle position */ /* *************************************************************** */ /* try to link if kk is not found/good enough and prev exist */ if ( curr_path_inf->inlist == 0 && curr_path_inf->prev >= 0 ) { vec_subt(X[3], X[1], diff_pos); if (pos3d_in_bounds(diff_pos, tpar)) { angle_acc(X[1], X[2], X[3], &angle, &acc); if ( (acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10) ) { quali = w[mm].freq; dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) )/2; rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle)/(quali); register_link_candidate(curr_path_inf, rr, w[mm].ftnr); } } } free(wn); } /* end of zaehler1-loop */ /* begin of inlist still zero */ if (tpar->add) { if ( curr_path_inf->inlist == 0 && curr_path_inf->prev >= 0 ) { for (j = 0; j < fb->num_cams; j++) { img_coord (j, X[2][0], X[2][1], X[2][2], Ex[j],I[j], G[j], ap[j], *(cpar->mm), &xn[j], &yn[j]); metric_to_pixel(&xn[j], &yn[j], xn[j], yn[j], cpar); x2[j]=-1e10; y2[j]=-1e10; } /* search for unused candidates in next time step */ for (j = 0; j < fb->num_cams; j++) { /*use fix distance to define xl, xr, yu, yd instead of searchquader */ xl[j]= xr[j]= yu[j]= yd[j] = 3.0; zaehler2 = candsearch_in_pixrest (fb->buf[2]->targets[j], fb->buf[2]->num_targets[j], xn[j], yn[j], xl[j], xr[j], yu[j], yd[j], philf[j], cpar); if(zaehler2 > 0) { _ix = philf[j][0]; x2[j] = fb->buf[2]->targets[j][_ix].x; y2[j] = fb->buf[2]->targets[j][_ix].y; } } quali=0; for (j = 0; j < fb->num_cams; j++) { if (x2[j] !=-1e10 && y2[j] != -1e10) { pixel_to_metric(&x2[j], &y2[j], x2[j], y2[j], cpar); quali++; } } if (quali>=2) { vec_copy(X[3], X[2]); invol=0; det_lsq_3d (glob_cal, *(cpar->mm), x2[0], y2[0], x2[1], y2[1], x2[2], y2[2], x2[3], y2[3], &(X[3][0]), &(X[3][1]), &(X[3][2]), fb->num_cams); /* in volume check */ if ( vpar->X_lay[0] < X[3][0] && X[3][0] < vpar->X_lay[1] && run_info->ymin < X[3][1] && X[3][1] < run_info->ymax && vpar->Zmin_lay[0] < X[3][2] && X[3][2] < vpar->Zmax_lay[1]) {invol = 1;} vec_subt(X[2], X[3], diff_pos); if ( invol == 1 && pos3d_in_bounds(diff_pos, tpar) ) { angle_acc(X[1], X[2], X[3], &angle, &acc); if ( (acc < tpar->dacc && angle < tpar->dangle) || \ (acc < tpar->dacc/10) ) { dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) )/2; rr = (dl/run_info->lmax + acc/tpar->dacc + angle/tpar->dangle)/(quali); ref_path_inf = &(fb->buf[2]->path_info[ fb->buf[2]->num_parts]); vec_copy(ref_path_inf->x, X[3]); reset_links(ref_path_inf); _frame_parts = fb->buf[2]->num_parts; register_link_candidate(curr_path_inf, rr, _frame_parts); ref_corres = &(fb->buf[2]->correspond[_frame_parts]); ref_targets = fb->buf[2]->targets; for (j = 0;j < fb->num_cams; j++) { ref_corres->p[j]=-1; if(philf[j][0]!=-999) { _ix = philf[j][0]; ref_targets[j][_ix].tnr = _frame_parts; ref_corres->p[j] = _ix; ref_corres->nr = _frame_parts; } } fb->buf[2]->num_parts++; zusatz++; } } invol=0; } // if quali >= 2 } } /* end of inlist still zero */ /***********************************/ free(w); } /* end of h-loop */ /* sort decis and give preliminary "finaldecis" */ for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); if(curr_path_inf->inlist > 0 ) { sort(curr_path_inf->inlist, (float *) curr_path_inf->decis, curr_path_inf->linkdecis); curr_path_inf->finaldecis = curr_path_inf->decis[0]; curr_path_inf->next = curr_path_inf->linkdecis[0]; } } /* create links with decision check */ for (h = 0;h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); if(curr_path_inf->inlist > 0 ) { ref_path_inf = &(fb->buf[2]->path_info[curr_path_inf->next]); if (ref_path_inf->prev == -1) { /* best choice wasn't used yet, so link is created */ ref_path_inf->prev = h; } else { /* best choice was already used by mega[2][mega[1][h].next].prev */ /* check which is the better choice */ if ( fb->buf[1]->path_info[ref_path_inf->prev].finaldecis > \ curr_path_inf->finaldecis) { /* remove link with prev */ fb->buf[1]->path_info[ref_path_inf->prev].next = NEXT_NONE; ref_path_inf->prev = h; } else { curr_path_inf->next = NEXT_NONE; } } } if (curr_path_inf->next != -2 ) count1++; } /* end of creation of links with decision check */ /* ******** Draw links now ******** */ m1_tr = 0; if (display) { for (h = 0; h < fb->buf[1]->num_parts; h++) { curr_path_inf = &(fb->buf[1]->path_info[h]); curr_corres = &(fb->buf[1]->correspond[h]); ref_corres = &(fb->buf[2]->correspond[curr_path_inf->next]); if (curr_path_inf->next != -2 ) { strcpy(buf,""); sprintf(buf ,"green"); for (j = 0; j < fb->num_cams; j++) { if (curr_corres->p[j] > 0 && ref_corres->p[j] > 0) { flag_m_tr=1; xp[j] = curr_targets[j][curr_corres->p[j]].x; yp[j] = curr_targets[j][curr_corres->p[j]].y; xc[j] = fb->buf[2]->targets[j][ref_corres->p[j]].x; yc[j] = fb->buf[2]->targets[j][ref_corres->p[j]].y; predict (xp[j], yp[j], xc[j], yc[j], &xn[j], &yn[j]); if ( ( fabs(xp[j]-zoom_x[j]) < cpar->imx/(2*zoom_f[j])) && (fabs(yp[j]-zoom_y[j]) < cpar->imy/(2*zoom_f[j]))) { strcpy(val,""); sprintf(val ,"orange"); intx0 = (int)(cpar->imx/2 + zoom_f[j]*(xp[j] - zoom_x[j])); inty0 = (int)(cpar->imy/2 + zoom_f[j]*(yp[j] - zoom_y[j])); intx1 = (int)(cpar->imx/2 + zoom_f[j]*(xc[j] - zoom_x[j])); inty1 = (int)(cpar->imy/2 + zoom_f[j]*(yc[j] - zoom_y[j])); intx2 = (int)(cpar->imx/2 + zoom_f[j]*(xn[j] - zoom_x[j])); inty2 = (int)(cpar->imy/2 + zoom_f[j]*(yn[j] - zoom_y[j])); intx0_tr[j][m1_tr]=intx0; inty0_tr[j][m1_tr]=inty0; intx1_tr[j][m1_tr]=intx1; inty1_tr[j][m1_tr]=inty1; intx2_tr[j][m1_tr]=intx2; inty2_tr[j][m1_tr]=inty2; pnr1_tr[j][m1_tr]=-1; pnr2_tr[j][m1_tr]=-1; pnr3_tr[j][m1_tr]=-1; if (curr_path_inf->finaldecis > 0.2) { pnr1_tr[j][m1_tr] = h; pnr2_tr[j][m1_tr] = curr_path_inf->next; pnr3_tr[j][m1_tr] = curr_path_inf->finaldecis; } } } if (flag_m_tr==0) { intx0_tr[j][m1_tr]=0; inty0_tr[j][m1_tr]=0; intx1_tr[j][m1_tr]=0; inty1_tr[j][m1_tr]=0; intx2_tr[j][m1_tr]=0; inty2_tr[j][m1_tr]=0; pnr1_tr[j][m1_tr]=-1; pnr2_tr[j][m1_tr]=-1; pnr3_tr[j][m1_tr]=-1; } flag_m_tr=0; } m1_tr++; } } } /* ******** End of Draw links now ******** */ sprintf (buf, "step: %d, curr: %d, next: %d, links: %d, lost: %d, add: %d", step, fb->buf[1]->num_parts, fb->buf[2]->num_parts, count1, fb->buf[1]->num_parts - count1, zusatz); /* for the average of particles and links */ npart = npart + fb->buf[1]->num_parts; nlinks = nlinks + count1; fb_next(fb); fb_write_frame_from_start(fb, step); if(step < run_info->seq_par->last - 2) { fb_read_frame_at_end(fb, step + 3, 0); } } /* end of sequence loop */
int flow_demo_c (ClientData clientData, Tcl_Interp* interp, int argc, const char** argv) { int i, i_seq, nr, j,pft_version=3,num_points,dumy; char name[128]; unsigned char *imgf; Tk_PhotoHandle img_handle; Tk_PhotoImageBlock img_block; nr = atoi(argv[1]); fpp = fopen_r ("parameters/sequence.par"); for (i=0; i<4; i++) fscanf (fpp, "%s\n", seq_name[i]); /* name of sequence */ fscanf (fpp,"%d\n", &seq_first); fscanf (fpp,"%d\n", &seq_last); fclose (fpp); /* allocate memory */ imgf = (unsigned char *) calloc (imgsize, 1); fpp = fopen ("parameters/pft_version.par", "r"); if (fpp){ fscanf (fpp, "%d\n", &pft_version); pft_version=pft_version+3; fclose (fpp); } else{ fpp = fopen ("parameters/pft_version.par", "w"); fprintf(fpp,"%d\n", 0); fclose(fpp); } /* load and display images */ for (i_seq=seq_first; i_seq<=seq_last; i_seq++){ compose_name_plus_nr (seq_name[nr], "", i_seq, name); fp1 = fopen_r (name); if (! fp1) return TCL_OK; sprintf (buf, "display camera %d, image %d", nr+1, i_seq); Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); Tcl_Eval(interp, ".text delete 2"); Tcl_Eval(interp, ".text insert 2 $tbuf"); read_image (interp, name, imgf); fclose (fp1); img_handle = Tk_FindPhoto( interp, "temp"); Tk_PhotoGetImage (img_handle, &img_block); sprintf(buf, "newimage %d", nr+1); Tcl_Eval(interp, buf); if(pft_version==4){ sprintf (filename, "%s%s", name,"_targets"); /* read targets of camera nr*/ nt4[3][nr]=0; fp1= fopen (filename, "r"); if (! fp1) printf("Can't open ascii file: %s\n", filename); fscanf (fp1, "%d\n", &nt4[3][nr]); for (j=0; j<nt4[3][nr]; j++){ fscanf (fp1, "%4d %lf %lf %d %d %d %d %d\n", &pix[nr][j].pnr, &pix[nr][j].x, &pix[nr][j].y, &pix[nr][j].n , &pix[nr][j].nx ,&pix[nr][j].ny, &pix[nr][j].sumg, &pix[nr][j].tnr); } fclose (fp1); num[nr] = nt4[3][nr]; if (display){ for (j=0; j<num[nr]; j++){ drawcross (interp, (int) pix[nr][j].x, (int) pix[nr][j].y,cr_sz, nr, "blue"); } printf ("drawing %d 2d ", num[nr]); } sprintf (filename, "res/rt_is.%d", i_seq); fp1= fopen (filename, "r"); if (fp1){ fscanf (fp1, "%d\n", &num_points); for (j=0; j<num_points; j++){ if (n_img==4){ fscanf(fp1, "%d %lf %lf %lf %d %d %d %d\n", &dumy, &fix[j].x, &fix[j].y, &fix[j].z, &geo[0][j].pnr, &geo[1][j].pnr, &geo[2][j].pnr, &geo[3][j].pnr); } if (n_img==3){ fscanf(fp1, "%d %lf %lf %lf %d %d %d %d\n", &dumy, &fix[j].x, &fix[j].y, &fix[j].z, &geo[0][j].pnr, &geo[1][j].pnr, &geo[2][j].pnr); } if (n_img==2){ // Alex's patch. 24.09.09. Working on Wesleyan data of 2 cameras only fscanf(fp1, "%d %lf %lf %lf %d %d %d %d\n", &dumy, &fix[j].x, &fix[j].y, &fix[j].z, &geo[0][j].pnr, &geo[1][j].pnr); } } fclose (fp1); if (display){ for (j=0; j<num_points; j++){ img_coord (fix[j].x,fix[j].y,fix[j].z, Ex[nr], I[nr], G[nr], ap[nr], mmp, &pix[nr][j].x,&pix[nr][j].y); metric_to_pixel (pix[nr][j].x,pix[nr][j].y, imx,imy, pix_x,pix_y, &pix[nr][j].x,&pix[nr][j].y, chfield); //if(geo[nr][j].pnr>-1){ // drawcross (interp, (int) pix[nr][geo[nr][j].pnr].x, (int) pix[nr][geo[nr][j].pnr].y,cr_sz, nr, "yellow"); //} drawcross (interp, (int) pix[nr][j].x, (int) pix[nr][j].y,cr_sz, nr, "red"); } printf ("and %d corresponding 3d positions for frame %d\n", num_points,i_seq); } } } Tcl_Eval(interp, "update idletasks"); } printf ("done\n\n"); free (imgf); return TCL_OK; }
int trajectories_c(ClientData clientData, Tcl_Interp* interp, int argc, const char** argv) /* draws crosses for detected points in a displayed image */ { int k, intx1, inty1, intx2, inty2; int i, anz1, anz2, m, j; FILE *fp1; char val[256]; vector *line1, *line2; double color; coord_2d p1[4], p2[4]; cr_sz = atoi(Tcl_GetVar2(interp, "mp", "pcrossize", TCL_GLOBAL_ONLY)); fpp = fopen_r ("parameters/sequence.par"); /* name of sequence */ fscanf (fpp,"%d\n", &seq_first); fscanf (fpp,"%d\n", &seq_last); fclose (fpp); sprintf (buf, "Show trajectories "); puts (buf); Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); Tcl_Eval(interp, ".text delete 2"); Tcl_Eval(interp, ".text insert 2 $tbuf"); for (i=seq_first; i<seq_last;i++) { if (i < 10) sprintf (val, "res/ptv_is.%1d", i); else if (i < 100) sprintf (val, "res/ptv_is.%2d", i); else sprintf (val, "res/ptv_is.%3d", i); fp1 = fopen (val, "r"); color = ((double)(i-seq_first))/((double)(seq_last-2-seq_first)); fscanf (fp1,"%d\n", &anz1); line1 = (vector *) malloc (anz1 * sizeof (vector)); for (j=0;j<anz1;j++) { fscanf (fp1, "%d\n", &line1[j].p); fscanf (fp1, "%d\n", &line1[j].n); fscanf (fp1, "%lf\n", &line1[j].x1); fscanf (fp1, "%lf\n", &line1[j].y1); fscanf (fp1, "%lf\n", &line1[j].z1); } strcpy(val, ""); fclose (fp1); /* read next time step */ if (i+1 < 10) sprintf (val, "res/ptv_is.%1d", i+1); else if (i+1 < 100) sprintf (val, "res/ptv_is.%2d", i+1); else sprintf (val, "res/ptv_is.%3d", i+1); fp1 = fopen (val, "r"); fscanf (fp1,"%d\n", &anz2); line2 = (vector *) calloc (anz2, sizeof (vector)); for (j=0;j<anz2;j++) { fscanf (fp1, "%d\n", &line2[j].p); fscanf (fp1, "%d\n", &line2[j].n); fscanf (fp1, "%lf\n", &line2[j].x1); fscanf (fp1, "%lf\n", &line2[j].y1); fscanf (fp1, "%lf\n", &line2[j].z1); } fclose (fp1); for(j=0;j<anz1;j++) { m = line1[j].n; if (m >= 0) { for (k=0; k<n_img; k++) { img_coord (line1[j].x1, line1[j].y1, line1[j].z1, Ex[k],I[k], G[k], ap[k], mmp, &p1[k].x, &p1[k].y); metric_to_pixel (p1[k].x, p1[k].y, imx,imy, pix_x,pix_y, &p1[k].x, &p1[k].y, chfield); img_coord (line2[m].x1, line2[m].y1, line2[m].z1, Ex[k],I[k], G[k], ap[k], mmp, &p2[k].x, &p2[k].y); metric_to_pixel (p2[k].x, p2[k].y, imx,imy, pix_x,pix_y, &p2[k].x, &p2[k].y, chfield); if ( fabs( p2[k].x-zoom_x[k]) < imx/(2*zoom_f[k]) && ( fabs(p2[k].y-zoom_y[k]) < imy/(2*zoom_f[k])) ) { intx1 = (int)(imx/2+zoom_f[k]*(p1[k].x-zoom_x[k])); inty1 = (int)(imy/2+zoom_f[k]*(p1[k].y-zoom_y[k])); intx2 = (int)(imx/2+zoom_f[k]*(p2[k].x-zoom_x[k])); inty2 = (int)(imy/2+zoom_f[k]*(p2[k].y-zoom_y[k])); drawcross ( interp, intx1, inty1, cr_sz+1, k, "blue"); drawcross ( interp, intx2, inty2, cr_sz+1, k, "red"); drawvector (interp, intx1, inty1, intx2, inty2, 2, k, "green"); } } } } Tcl_Eval(interp, "update idletasks"); strcpy(val, ""); free(line1); free(line2); } /* end of sequence loop */ sprintf(val, "...done"); Tcl_SetVar(interp, "tbuf", val, TCL_GLOBAL_ONLY); Tcl_Eval(interp, ".text delete 3"); Tcl_Eval(interp, ".text insert 3 $tbuf"); return TCL_OK; }
int calibration_proc_c (/*ClientData clientData, Tcl_Interp* interp,*/ int argc, const char** argv) { int i, j, sel, i_img, k, n, sup; int intx1, inty1, intx2, inty2; coord_2d apfig1[11][11]; /* regular grid for ap figures */ coord_2d apfig2[11][11]; /* ap figures */ coord_3d fix4[4]; /* object points for preorientation */ coord_2d crd0[4][4]; /* image points for preorientation */ char filename[256], val[256]; const char *valp; //Tk_PhotoHandle img_handle; //Tk_PhotoImageBlock img_block; /* read support of unsharp mask */ fp1 = fopen ("parameters/unsharp_mask.par", "r"); if (! fp1) sup = 12; else { fscanf (fp1, "%d\n", &sup); fclose (fp1); } /* Get Selection value from TclTk */ // ChrisB: what does this do?? Set a value...... //valp = Tcl_GetVar(interp, "sel", TCL_GLOBAL_ONLY); //sel = atoi (valp); sel = 1; // set a value.... switch (sel) { case 1: /* read calibration parameter file */ fp1 = fopen_r ("parameters/cal_ori.par"); fscanf (fp1,"%s\n", fixp_name); for (i=0; i<4; i++) { fscanf (fp1, "%s\n", img_name[i]); fscanf (fp1, "%s\n", img_ori0[i]); } fscanf (fpp, "%d\n", &tiff_flag); fscanf (fp1, "%d\n", &chfield); fclose (fp1); /* create file names */ for (i=0; i<n_img; i++) { strcpy (img_ori[i], img_name[i]); strcat (img_ori[i], ".ori"); strcpy (img_addpar0[i], img_name[i]); strcat (img_addpar0[i], ".addpar0"); strcpy (img_addpar[i], img_name[i]); strcat (img_addpar[i], ".addpar"); strcpy (img_hp_name[i], img_name[i]); strcat (img_hp_name[i], "_hp"); } for (i=0; i<n_img; i++) { zoom_x[i] = imx/2, zoom_y[i] = imy/2, zoom_f[i] = 1; read_image (/*interp,*/ img_name[i], img[i]); sprintf(val, "camcanvas %d", i+1); //Tcl_Eval(interp, val); //img_handle = Tk_FindPhoto( interp, "temp"); //Tk_PhotoGetImage (img_handle, &img_block); //tclimg2cimg (interp, img[i], &img_block); sprintf(val, "newimage %d", i+1); //Tcl_Eval(interp, val); } break; case 2: puts ("Detection procedure"); strcpy(val,""); /* Highpass Filtering */ pre_processing_c (/*clientData, interp,*/ argc, argv); /* reset zoom values */ for (i=0; i<n_img; i++) { zoom_x[i] = imx/2; zoom_y[i] = imy/2; zoom_f[i] = 1; } /* copy images because the target recognition will set greyvalues to zero */ for (i=0; i<n_img; i++) { copy_images (img[i], img0[i]); } /* target recognition */ for (i=0; i<n_img; i++) { targ_rec (/*interp,*/ img[i], img0[i], "parameters/detect_plate.par", 0, imx, 1, imy, pix[i], i, &num[i]); sprintf (buf,"image %d: %d, ", i+1, num[i]); strcat(val, buf); if (num[i] > nmax) exit (1); } /* save pixel coord as approx. for template matching */ if (examine) for (i=0; i<n_img; i++) { sprintf (filename, "%s_pix", img_name[i]); fp1 = fopen (filename, "w"); for (j=0; j<num[i]; j++) fprintf (fp1, "%4d %8.3f %8.3f\n", pix[i][j].pnr, pix[i][j].x, pix[i][j].y); fclose (fp1); } sprintf(buf,"Number of detected targets, interaction enabled"); //Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); //Tcl_Eval(interp, ".text delete 2"); //Tcl_Eval(interp, ".text insert 2 $tbuf"); //Tcl_SetVar(interp, "tbuf", val, TCL_GLOBAL_ONLY); //Tcl_Eval(interp, ".text delete 3"); //Tcl_Eval(interp, ".text insert 3 $tbuf"); break; case 3: pp1=0; pp2=0; pp3=0; pp4=0; for (i=0; i<n_img; i++) { sprintf (buf, "%d targets remain", num[i]); puts (buf); } fp1 = fopen_r ("parameters/man_ori.par"); for (i=0; i<n_img; i++) { fscanf (fp1, "%d %d %d %d\n", &nr[i][0], &nr[i][1], &nr[i][2], &nr[i][3]); } fclose (fp1); for (i=0; i<n_img; i++) { sprintf(val, "measure %d %d %d %d %d", nr[i][0], nr[i][1], nr[i][2], nr[i][3], i+1); //Tcl_Eval(interp, val); #if 0 // ChrisB: do we need this? valp = Tcl_GetVar(interp, "px0", TCL_GLOBAL_ONLY); pix0[i][0].x = atoi (valp); valp = Tcl_GetVar(interp, "py0", TCL_GLOBAL_ONLY); pix0[i][0].y = atoi (valp); valp = Tcl_GetVar(interp, "px1", TCL_GLOBAL_ONLY); pix0[i][1].x = atoi (valp); valp = Tcl_GetVar(interp, "py1", TCL_GLOBAL_ONLY); pix0[i][1].y = atoi (valp); valp = Tcl_GetVar(interp, "px2", TCL_GLOBAL_ONLY); pix0[i][2].x = atoi (valp); valp = Tcl_GetVar(interp, "py2", TCL_GLOBAL_ONLY); pix0[i][2].y = atoi (valp); valp = Tcl_GetVar(interp, "px3", TCL_GLOBAL_ONLY); pix0[i][3].x = atoi (valp); valp = Tcl_GetVar(interp, "py3", TCL_GLOBAL_ONLY); pix0[i][3].y = atoi (valp); #endif } /* write measured coordinates to file for next trial */ fp1 = fopen ("man_ori.dat", "w"); for (i=0; i<n_img; i++) for (j=0; j<4; j++) fprintf (fp1, "%f %f\n", pix0[i][j].x, pix0[i][j].y); fclose (fp1); break; case 4: /* read pixel coordinates of older pre-orientation */ /* read point numbers of pre-clicked points */ fp1 = fopen_r ("parameters/man_ori.par"); for (i=0; i<n_img; i++) { fscanf (fp1, "%d %d %d %d\n", &nr[i][0], &nr[i][1], &nr[i][2], &nr[i][3]); } fclose (fp1); /* read coordinates of pre-clicked points */ fp1 = fopen ("man_ori.dat", "r"); if (! fp1) break; for (i_img=0; i_img<n_img; i_img++) for (i=0; i<4; i++) { #if 0 fscanf (fp1, "%lf %lf\n", &pix0[i_img][i].x, &pix0[i_img][i].y); drawcross (interp, (int) pix0[i_img][i].x, (int) pix0[i_img][i].y, cr_sz+2, i_img, "red"); draw_pnr (interp, (int) pix0[i_img][i].x, (int) pix0[i_img][i].y, nr[i_img][i], i_img, "red"); #endif } fclose (fp1); break; case 5: puts ("Sort grid points"); for (i=0; i<n_img; i++) { /* read control point coordinates for man_ori points */ fp1 = fopen_r (fixp_name); k = 0; while ( fscanf (fp1, "%d %lf %lf %lf", &fix[k].pnr, &fix[k].x, &fix[k].y, &fix[k].z) != EOF) k++; fclose (fp1); nfix = k; /* take clicked points from control point data set */ for (j=0; j<4; j++) for (k=0; k<nfix; k++) { if (fix[k].pnr == nr[i][j]) fix4[j] = fix[k]; } /* get approx for orientation and ap */ read_ori (&Ex[i], &I[i], img_ori0[i]); fp1 = fopen (img_addpar0[i], "r"); if (! fp1) fp1 = fopen ("addpar.raw", "r"); if (fp1) { fscanf (fp1, "%lf %lf %lf %lf %lf %lf %lf", &ap[i].k1,&ap[i].k2,&ap[i].k3, &ap[i].p1,&ap[i].p2, &ap[i].scx,&ap[i].she); fclose (fp1);} else { printf("no addpar.raw\n"); ap[i].k1=ap[i].k2=ap[i].k3=ap[i].p1=ap[i].p2=ap[i].she=0.0; ap[i].scx=1.0; } /* transform clicked points */ for (j=0; j<4; j++) { pixel_to_metric (pix0[i][j].x, pix0[i][j].y, imx,imy, pix_x, pix_y, &crd0[i][j].x, &crd0[i][j].y, chfield); correct_brown_affin (crd0[i][j].x, crd0[i][j].y, ap[i], &crd0[i][j].x, &crd0[i][j].y); } /* raw orientation with 4 points */ raw_orient (Ex[i], I[i], ap[i], mmp, 4, fix4, crd0[i], &Ex[i]); sprintf (filename, "raw%d.ori", i); write_ori (Ex[i], I[i], filename); /* sorting of detected points by back-projection */ sortgrid_man (/*interp,*/ Ex[i], I[i], ap[i], mmp, imx,imy, pix_x,pix_y, nfix, fix, num[i], pix[i], chfield, i); /* adapt # of detected points */ num[i] = nfix; for (j=0; j<nfix; j++) { #if 0 if (pix[i][j].pnr < 0) continue; intx1 = (int) pix[i][j].x ; inty1 = (int) pix[i][j].y ; drawcross (interp, intx1, inty1, cr_sz, i, "white"); draw_pnr (interp, intx1, inty1, fix[j].pnr, i, "white"); #endif } } /* dump dataset for rdb */ if (examine == 4) { /* create filename for dumped dataset */ sprintf (filename, "dump_for_rdb"); fp1 = fopen (filename, "w"); /* write # of points to file */ fprintf (fp1, "%d\n", nfix); /* write point and image coord to file */ for (i=0; i<nfix; i++) { fprintf (fp1, "%4d %10.3f %10.3f %10.3f %d ", fix[i].pnr, fix[i].x, fix[i].y, fix[i].z, 0); for (i_img=0; i_img<n_img; i_img++) { if (pix[i_img][i].pnr >= 0) { /* transform pixel coord to metric */ pixel_to_metric (pix[i_img][i].x, pix[i_img][i].y, imx,imy, pix_x, pix_y, &crd[i_img][i].x, &crd[i_img][i].y, chfield); fprintf (fp1, "%4d %8.5f %8.5f ", pix[i_img][i].pnr, crd[i_img][i].x, crd[i_img][i].y); } else { fprintf (fp1, "%4d %8.5f %8.5f ", pix[i_img][i].pnr, 0.0, 0.0); } } fprintf (fp1, "\n"); } fclose (fp1); printf ("dataset dumped into %s\n", filename); } break; case 6: puts ("Orientation"); strcpy(buf, ""); for (i_img=0; i_img<n_img; i_img++) { for (i=0; i<nfix ; i++) { pixel_to_metric (pix[i_img][i].x, pix[i_img][i].y, imx,imy, pix_x, pix_y, &crd[i_img][i].x, &crd[i_img][i].y, chfield); crd[i_img][i].pnr = pix[i_img][i].pnr; } /* save data for special use of resection routine */ if (examine == 4) { printf ("try write resection data to disk\n"); /* point coordinates */ sprintf (filename, "resect_%s.fix", img_name[i_img]); write_ori (Ex[i_img], I[i_img], img_ori[i_img]); fp1 = fopen (filename, "w"); for (i=0; i<nfix; i++) fprintf (fp1, "%3d %10.5f %10.5f %10.5f\n", fix[i].pnr, fix[i].x, fix[i].y, fix[i].z); fclose (fp1); /* metric image coordinates */ sprintf (filename, "resect_%s.crd", img_name[i_img]); fp1 = fopen (filename, "w"); for (i=0; i<nfix; i++) fprintf (fp1, "%3d %9.5f %9.5f\n", crd[i_img][i].pnr, crd[i_img][i].x, crd[i_img][i].y); fclose (fp1); /* orientation and calibration approx data */ write_ori (Ex[i_img], I[i_img], "resect.ori0"); fp1 = fopen ("resect.ap0", "w"); fprintf (fp1, "%f %f %f %f %f %f %f", ap[i_img].k1, ap[i_img].k2, ap[i_img].k3, ap[i_img].p1, ap[i_img].p2, ap[i_img].scx, ap[i_img].she); fclose (fp1); printf ("resection data written to disk\n"); } /* resection routine */ /* ================= */ if (examine != 4) orient (/*interp,*/ Ex[i_img], I[i_img], ap[i_img], mmp, nfix, fix, crd[i_img], &Ex[i_img], &I[i_img], &ap[i_img], i_img); /* ================= */ /* resection with dumped datasets */ if (examine == 4) { printf("Resection with dumped datasets? (y/n)"); scanf("%s",buf); if (buf[0] != 'y') continue; strcpy (buf, ""); /* read calibration frame datasets */ for (n=0, nfix=0, dump_for_rdb=0; n<100; n++) { sprintf (filename, "resect.fix%d", n); fp1 = fopen (filename, "r"); if (! fp1) continue; printf("reading file: %s\n", filename); printf ("reading dumped resect data #%d\n", n); k = 0; while ( fscanf (fp1, "%d %lf %lf %lf", &fix[nfix+k].pnr, &fix[nfix+k].x, &fix[nfix+k].y, &fix[nfix+k].z) != EOF) k++; fclose (fp1); /* read metric image coordinates */ sprintf (filename, "resect_%d.crd%d", i_img, n); printf("reading file: %s\n", filename); fp1 = fopen (filename, "r"); for (i=nfix; i<nfix+k; i++) fscanf (fp1, "%d %lf %lf", &crd[i_img][i].pnr, &crd[i_img][i].x, &crd[i_img][i].y); nfix += k; } /* resection */ orient (/*interp,*/ Ex[i_img], I[i_img], ap[i_img], mmp, nfix, fix, crd[i_img], &Ex[i_img], &I[i_img], &ap[i_img], i_img); } /* save orientation and additional parameters */ write_ori (Ex[i_img], I[i_img], img_ori[i_img]); fp1 = fopen (img_addpar[i_img], "w"); fprintf (fp1, "%f %f %f %f %f %f %f", ap[i_img].k1, ap[i_img].k2, ap[i_img].k3, ap[i_img].p1, ap[i_img].p2, ap[i_img].scx, ap[i_img].she); fclose (fp1); } //Tcl_Eval(interp, ".text delete 3"); //Tcl_Eval(interp, ".text delete 1"); //Tcl_Eval(interp, ".text insert 1 \"Orientation and self calibration \""); //Tcl_Eval(interp, ".text delete 2"); //Tcl_Eval(interp, ".text insert 2 \"...done, sigma0 for each image -> \""); //Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); //Tcl_Eval(interp, ".text insert 3 $tbuf"); break; case 7: checkpoint_proc (/*interp*/); #if 0 sprintf(val,"blue: planimetry, yellow: height"); Tcl_SetVar(interp, "tbuf", val, TCL_GLOBAL_ONLY); Tcl_Eval(interp, ".text delete 2"); Tcl_Eval(interp, ".text insert 2 $tbuf"); Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); Tcl_Eval(interp, ".text delete 3"); Tcl_Eval(interp, ".text insert 3 $tbuf"); #endif break; case 8: /* draw additional parameter figures */ //Tcl_Eval(interp, "clearcam"); /* read orientation and additional parameters */ for (i=0; i<n_img; i++) read_ori (&Ex[i], &I[i], img_ori[i]); for (i=0; i<n_img; i++) { fp1 = fopen_r (img_addpar[i]); fscanf (fp1,"%lf %lf %lf %lf %lf %lf %lf", &ap[i].k1, &ap[i].k2, &ap[i].k3, &ap[i].p1, &ap[i].p2, &ap[i].scx, &ap[i].she); fclose (fp1); } for (i_img=0; i_img<n_img; i_img++) { /* create undistorted grid */ for (i=0; i<11; i++) for (j=0; j<11; j++) { apfig1[i][j].x = i * imx/10; apfig1[i][j].y = j * imy/10; } /* draw undistorted grid */ for (i=0; i<10; i++) for (j=0; j<10; j++) { intx1 = (int) apfig1[i][j].x; inty1 = (int) apfig1[i][j].y; intx2 = (int) apfig1[i+1][j].x; inty2 = (int) apfig1[i][j+1].y; //drawvector (interp, intx1, inty1, intx2, inty1, 1, i_img, "black"); //drawvector (interp, intx1, inty1, intx1, inty2, 1, i_img, "black"); } for (j=0; j<10; j++) { intx1 = (int) apfig1[10][j].x; inty1 = (int) apfig1[10][j].y; inty2 = (int) apfig1[10][j+1].y; //drawvector (interp, intx1, inty1, intx1, inty2, 1, i_img, "black"); } for (i=0; i<10; i++) { intx1 = (int) apfig1[i][10].x; inty1 = (int) apfig1[i][10].y; intx2 = (int) apfig1[i+1][10].x; //drawvector (interp, intx1, inty1, intx2, inty1, 1, i_img, "black"); } /* distort grid */ for (i=0; i<11; i++) for (j=0; j<11; j++) { /* transform to metric, distort and re-transform */ pixel_to_metric (apfig1[i][j].x, apfig1[i][j].y, imx,imy, pix_x,pix_y, &apfig2[i][j].x, &apfig2[i][j].y, chfield); distort_brown_affin (apfig2[i][j].x, apfig2[i][j].y, ap[i_img], &apfig2[i][j].x, &apfig2[i][j].y); metric_to_pixel (apfig2[i][j].x, apfig2[i][j].y, imx,imy, pix_x,pix_y, &apfig2[i][j].x, &apfig2[i][j].y, chfield); /* exaggerate distortion by factor 5 */ apfig2[i][j].x = 5*apfig2[i][j].x - 4*apfig1[i][j].x; apfig2[i][j].y = 5*apfig2[i][j].y - 4*apfig1[i][j].y; } /* draw distorted grid */ for (i=0; i<10; i++) for (j=0; j<10; j++) { intx1 = (int) apfig2[i][j].x; inty1 = (int) apfig2[i][j].y; intx2 = (int) apfig2[i+1][j].x; inty2 = (int) apfig2[i+1][j].y; //drawvector (interp, intx1, inty1, intx2, inty2, 3, i_img, "magenta"); intx2 = (int) apfig2[i][j+1].x ; inty2 = (int) apfig2[i][j+1].y ; //drawvector (interp, intx1, inty1, intx2, inty2, 3, i_img, "magenta"); } for (j=0; j<10; j++) { intx1 = (int) apfig2[10][j].x; inty1 = (int) apfig2[10][j].y; intx2 = (int) apfig2[10][j+1].x; inty2 = (int) apfig2[10][j+1].y; //drawvector (interp, intx1, inty1, intx2, inty2, 3, i_img, "magenta"); } for (i=0; i<10; i++) { intx1 = (int) apfig2[i][10].x; inty1 = (int) apfig2[i][10].y; intx2 = (int) apfig2[i+1][10].x; inty2 = (int) apfig2[i+1][10].y ; //drawvector (interp, intx1, inty1, intx2, inty2, 3, i_img, "magenta"); } } break; } return TCL_OK; }
int mouse_proc_c (int click_x, int click_y, int kind, int num_image, volume_par *vpar, \ control_par *cpar){ int i, j, n, zf; double x, y; double xa12, xb12, ya12, yb12; int k, pt1, intx1, inty1, count, intx2, inty2, pt2; candidate cand[maxcand]; printf("entered mouse_proc_c \n"); if (zoom_f[0] == 1) {zf = 2;} else { zf = zoom_f[0];} n=num_image; if (examine) zf *= 2; switch (kind) { /* -------------------------- MIDDLE MOUSE BUTTON ---------------------------------- */ case 3: /* generate epipolar line segments */ /* get geometric coordinates of nearest point in img[n] */ x = (float) (click_x - cpar->imx/2)/zoom_f[n] + zoom_x[n]; y = (float) (click_y - cpar->imy/2)/zoom_f[n] + zoom_y[n]; pixel_to_metric (&x, &y, x,y, cpar); x -= I[n].xh; y -= I[n].yh; correct_brown_affin (x, y, ap[n], &x, &y); k = nearest_neighbour_geo (geo[n], num[n], x, y, 0.05); if (k == -999){ printf ("No point near click coord! Click again! \n"); return -1; } pt1 = geo[n][k].pnr; intx1 = (int) ( cpar->imx/2 + zoom_f[n] * (pix[n][pt1].x-zoom_x[n])); inty1 = (int) ( cpar->imy/2 + zoom_f[n] * (pix[n][pt1].y-zoom_y[n])); rclick_points_intx1=intx1; rclick_points_inty1=inty1; //drawcross (interp, intx1, inty1, cr_sz+2, n, "BlueViolet"); printf ( "pt1,nx,ny,n,sumg: %d %d %d %d %d\n", pt1, pix[n][pt1].nx, pix[n][pt1].ny, pix[n][pt1].n, pix[n][pt1].sumg); for (i = 0; i < cpar->num_cams; i++) if (i != n) { /* calculate epipolar band in img[i] */ epi_mm (i, geo[n][k].x,geo[n][k].y, Ex[n],I[n], G[n], Ex[i],I[i], G[i], *(cpar->mm), vpar, &xa12, &ya12, &xb12, &yb12); /* search candidate in img[i] */ printf("\ncandidates in img: %d\n", i); find_candidate_plus_msg (geo[i], pix[i], num[i], xa12, ya12, xb12, yb12, pix[n][pt1].n, pix[n][pt1].nx, pix[n][pt1].ny, pix[n][pt1].sumg, cand, &count, i, vpar, cpar); distort_brown_affin (xa12,ya12, ap[i], &xa12,&ya12); distort_brown_affin (xb12,yb12, ap[i], &xb12,&yb12); xa12 += I[i].xh; ya12 += I[i].yh; xb12 += I[i].xh; yb12 += I[i].yh; metric_to_pixel(&xa12, &ya12, xa12, ya12, cpar); metric_to_pixel(&xb12, &yb12, xb12, yb12, cpar); intx1 = (int) ( cpar->imx/2 + zoom_f[i] * (xa12 - zoom_x[i])); inty1 = (int) ( cpar->imy/2 + zoom_f[i] * (ya12 - zoom_y[i])); intx2 = (int) ( cpar->imx/2 + zoom_f[i] * (xb12 - zoom_x[i])); inty2 = (int) ( cpar->imy/2 + zoom_f[i] * (yb12 - zoom_y[i])); rclick_intx1[i]=intx1; rclick_inty1[i]=inty1; rclick_intx2[i]=intx2; rclick_inty2[i]=inty2; // drawvector ( interp, intx1, inty1, intx2, inty2, 1, i, val); rclick_count[i]=count; for (j=0; j<count; j++) { pt2 = cand[j].pnr; intx2 = (int) ( cpar->imx/2 + zoom_f[i] * (pix[i][pt2].x - zoom_x[i])); inty2 = (int) ( cpar->imy/2 + zoom_f[i] * (pix[i][pt2].y - zoom_y[i])); rclick_points_x1[i][j]=intx2; rclick_points_y1[i][j]=inty2; //drawcross (interp, intx2, inty2, cr_sz+2, i, "orange"); } } break; case 4: /* delete points, which should not be used for orientation */ j = kill_in_list (n, num[n], click_x, click_y); if (j != -1) { num[n] -= 1; printf ("point %d deleted", j); } else { printf ("no point near click coord !"); } break; } printf("finished mouse_proc_c \n"); return 0; }