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