int mark_track_c(ClientData clientData, Tcl_Interp* interp, int argc, const char** argv)
/* draws crosses for detected points in a displayed image */
{
char seq_name[4][128];
int i_img, i_seq, h, intx, inty;
cr_sz = atoi(Tcl_GetVar2(interp, "mp", "pcrossize", TCL_GLOBAL_ONLY));
fpp = fopen_r ("parameters/sequence.par");
for (i_img=0; i_img<4; i_img++)
{
fscanf (fpp, "%s\n", seq_name[i_img]);
}
/* name of sequence */
fscanf (fpp,"%d\n", &seq_first);
fscanf (fpp,"%d\n", &seq_last);
fclose (fpp);
sprintf (buf, "Show detected particles "); puts (buf);
Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY);
Tcl_Eval(interp, ".text delete 2");
Tcl_Eval(interp, ".text insert 2 $tbuf");
/* track sequence */
for (i_seq=seq_first; i_seq<=seq_last; i_seq++)
{
read_ascii_data(i_seq);
/* treat the cameras one after the other */
for (i_img=0; i_img<n_img; i_img++)
{
for (h=0; h<nt4[3][i_img]; h++)
{
if ( ( fabs(t4[3][i_img][h].x-zoom_x[i_img]) < imx/(2*zoom_f[i_img]))
&& ( fabs(t4[3][i_img][h].y-zoom_y[i_img]) < imy/(2*zoom_f[i_img])) )
{
intx = (int)(imx/2+zoom_f[i_img]*(t4[3][i_img][h].x-zoom_x[i_img]));
inty = (int)(imy/2+zoom_f[i_img]*(t4[3][i_img][h].y-zoom_y[i_img]));
if (t4[3][i_img][h].tnr>-1)
{
drawcross ( interp, intx, inty, cr_sz+1, i_img, "green");
if (zoom_f[i_img] >= 6) {
draw_pnr ( interp, intx, inty+10, i_seq, i_img, "orange");
draw_pnr ( interp, intx, inty, t4[3][i_img][h].tnr, i_img, "green");
}
} else { drawcross ( interp, intx, inty, cr_sz, i_img, "blue"); }
}
}
Tcl_Eval(interp, "update idletasks");
}
}
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;
}
void mark_corr (Tcl_Interp* interp, int nr)
/* draws crosses and numbers for corresponding points in a displayed window */
{
int i,j, pnr, sum, intx, inty;
double x, y;
if (match == 0) return;
for (i=0; i<match; i++)
{
pnr = geo[nr][con[i].p[nr]].pnr;
if (pnr < 0|| con[i].p[nr] < 0) continue;
x = pix[nr][pnr].x; y = pix[nr][pnr].y;
intx = (int) ( imx/2 + zoom_f[nr] * (x-zoom_x[nr]));
inty = (int) ( imy/2 + zoom_f[nr] * (y-zoom_y[nr]));
/* check whether quadruplet, triplet or pair -> select color */
for (j=0, sum=0; j<4; j++) if (con[i].p[j] > 0) { sum++; }
if ( sum == 2 ) sprintf(buf ,"yellow");
if ( sum == 3 ) sprintf(buf ,"green");
if ( sum == 4 ) sprintf(buf ,"red");
/* i is the number of the established correspondence */
draw_pnr (interp, intx+5 , inty, i, nr, "white");
/*
hilfi = geo[nr][con[i].p[nr]].pnr;
drawcross (interp, intx, inty, cr_sz, nr, buf);
*/
/* hilfi is the number of the detected point, see in targetlist */
/*
draw_pnr (interp, intx+25 , inty-15, hilfi, nr, "red");
*/
}
}
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;
}
void correspondences_4 (Tcl_Interp* interp, const char** argv)
{
int i,j,k,l,m,n,o, i1,i2,i3;
int count, match0=0, match4=0, match3=0, match2=0, match1=0;
int p1,p2,p3,p4, p31, p41, p42;
int pt1;
int tim[4][nmax];
int intx, inty;
double xa12,ya12,xb12,yb12,X,Y,Z;
double corr;
candidate cand[maxcand];
n_tupel *con0;
correspond *list[4][4];
/* ----------------------------------------------------------------------- */
/* allocate memory for lists of correspondences */
for (i1=0; i1<n_img-1; i1++) for (i2=i1+1; i2<n_img; i2++)
list[i1][i2] = (correspond *) malloc (num[i1] * sizeof (correspond));
con0 = (n_tupel *) malloc (4*nmax * sizeof (n_tupel));
/* ----------------------------------------------------------------------- */
printf("in corres zmin0: %f, zmax0: %f\n", Zmin_lay[0],Zmax_lay[0] );
/* initialize ... */
sprintf (buf,"Establishing correspondences");
Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY);
Tcl_Eval(interp, ".text delete 2");
Tcl_Eval(interp, ".text insert 2 $tbuf");
match=0; match0=0; match2=0;
for (i1=0; i1<n_img-1; i1++)
for (i2=i1+1; i2<n_img; i2++)
for (i=0; i<num[i1]; i++)
{
list[i1][i2][i].p1 = 0;
list[i1][i2][i].n = 0;
}
for (i=0; i<nmax; i++)
{
for (j=0; j<4; j++) tim[j][i] = 0;
for (j=0; j<4; j++) con0[i].p[j] = -1; con0[i].corr = 0;
}
/* -------------if only one cam and 2D--------- */ //by Beat Lüthi June 2007
if(n_img==1){
if(res_name[0]==0){
sprintf (res_name, "rt_is");
}
fp1 = fopen (res_name, "w");
fprintf (fp1, "%4d\n", num[0]);
for (i=0; i<num[0]; i++){
o = epi_mm_2D (geo[0][i].x,geo[0][i].y,
Ex[0], I[0], G[0], mmp,
&X,&Y,&Z);
pix[0][geo[0][i].pnr].tnr=i;
fprintf (fp1, "%4d", i+1);
fprintf (fp1, " %9.3f %9.3f %9.3f", X, Y, Z);
fprintf (fp1, " %4d", geo[0][i].pnr);
fprintf (fp1, " %4d", -1);
fprintf (fp1, " %4d", -1);
fprintf (fp1, " %4d\n", -1);
}
fclose (fp1);
match1=num[0];
}
/* -------------end of only one cam and 2D ------------ */
/* matching 1 -> 2,3,4 + 2 -> 3,4 + 3 -> 4 */
for (i1=0; i1<n_img-1; i1++) for (i2=i1+1; i2<n_img; i2++)
{
sprintf (buf, "Establishing correspondences %d - %d", i1, i2);
puts (buf);
/* establish correspondences from num[i1] points of img[i1] to img[i2] */
for (i=0; i<num[i1]; i++) if (geo[i1][i].x != -999)
{
/*o = epi_mm (geo[i1][i].x,geo[i1][i].y,
Ex[i1], I[i1], G[i1], Ex[i2], I[i2], G[i2], mmp,
&xa12, &ya12, &xb12, &yb12);
o = epi_mm (xa12, ya12,
Ex[i2], I[i2], G[i2], Ex[i1], I[i1], G[i1], mmp,
&xa12, &ya12, &xb12, &yb12);*/
o = epi_mm (geo[i1][i].x,geo[i1][i].y,
Ex[i1], I[i1], G[i1], Ex[i2], I[i2], G[i2], mmp,
&xa12, &ya12, &xb12, &yb12);
/////ich glaube, da muss ich einsteigen, wenn alles erledigt ist.
///////mit bild_1 x,y Epipole machen und dann selber was schreiben um die Distanz zu messen.
///////zu Punkt in bild_2.
/* origin point in the list */
p1 = i; list[i1][i2][p1].p1 = p1; pt1 = geo[i1][p1].pnr;
/* search for a conjugate point in geo[i2] */
find_candidate_plus (geo[i2], pix[i2], num[i2],
xa12, ya12, xb12, yb12, eps0,
pix[i1][pt1].n,pix[i1][pt1].nx,pix[i1][pt1].ny,
pix[i1][pt1].sumg, cand, &count, i2,argv);
/* write all corresponding candidates to the preliminary list */
/* of correspondences */
if (count > maxcand) { count = maxcand; }
for (j=0; j<count; j++)
{
list[i1][i2][p1].p2[j] = cand[j].pnr;
list[i1][i2][p1].corr[j] = cand[j].corr;
list[i1][i2][p1].dist[j] = cand[j].tol;
}
list[i1][i2][p1].n = count;
}
}
/* repair memory fault (!?) */
for (j=0; j<4; j++) for (i=0; i<nmax; i++) tim[j][i] = 0;
/* ------------------------------------------------------------------ */
/* ------------------------------------------------------------------ */
/* search consistent quadruplets in the list */
if (n_img == 4)
{
puts ("Search consistent quadruplets");
for (i=0, match0=0; i<num[0]; i++)
{
p1 = list[0][1][i].p1;
for (j=0; j<list[0][1][i].n; j++)
for (k=0; k<list[0][2][i].n; k++)
for (l=0; l<list[0][3][i].n; l++)
{
p2 = list[0][1][i].p2[j];
p3 = list[0][2][i].p2[k];
p4 = list[0][3][i].p2[l];
for (m=0; m<list[1][2][p2].n; m++)
for (n=0; n<list[1][3][p2].n; n++)
{
p31 = list[1][2][p2].p2[m];
p41 = list[1][3][p2].p2[n];
if (p3 == p31 && p4 == p41)
for (o=0; o<list[2][3][p3].n; o++)
{
p42 = list[2][3][p3].p2[o];
if (p4 == p42)
{
corr = (list[0][1][i].corr[j]
+ list[0][2][i].corr[k]
+ list[0][3][i].corr[l]
+ list[1][2][p2].corr[m]
+ list[1][3][p2].corr[n]
+ list[2][3][p3].corr[o])
/ (list[0][1][i].dist[j]
+ list[0][2][i].dist[k]
+ list[0][3][i].dist[l]
+ list[1][2][p2].dist[m]
+ list[1][3][p2].dist[n]
+ list[2][3][p3].dist[o]);
if (corr > corrmin)
{
/* accept as preliminary match */
con0[match0].p[0] = p1;
con0[match0].p[1] = p2;
con0[match0].p[2] = p3;
con0[match0].p[3] = p4;
con0[match0++].corr = corr;
if (match0 == 4*nmax) /* security */
{
printf ("Overflow in correspondences:");
printf (" > %d matches\n", match0);
i = num[0];
}
}
}
}
}
}
}
/* -------------------------------------------------------------------- */
/* sort quadruplets for match quality (.corr) */
quicksort_con (con0, match0);
/* -------------------------------------------------------------------- */
/* take quadruplets from the top to the bottom of the sorted list */
/* only if none of the points has already been used */
for (i=0, match=0; i<match0; i++)
{
p1 = con0[i].p[0]; if (p1 > -1) if (++tim[0][p1] > 1) continue;
p2 = con0[i].p[1]; if (p2 > -1) if (++tim[1][p2] > 1) continue;
p3 = con0[i].p[2]; if (p3 > -1) if (++tim[2][p3] > 1) continue;
p4 = con0[i].p[3]; if (p4 > -1) if (++tim[3][p4] > 1) continue;
con[match++] = con0[i];
}
match4 = match;
sprintf (buf, "%d consistent quadruplets, ", match4); puts (buf);
}
/* ----------------------------------------------------------------------- */
/* ----------------------------------------------------------------------- */
/* search consistent triplets : 123, 124, 134, 234 */
if ((n_img ==4 && allCam_flag==0) || n_img ==3)
{
puts ("Search consistent triplets");
match0=0;
for (i1=0; i1<n_img-2; i1++)
for (i2=i1+1; i2<n_img-1; i2++)
for (i3=i2+1; i3<n_img; i3++)
for (i=0; i<num[i1]; i++)
{
p1 = list[i1][i2][i].p1;
if (p1 > nmax || tim[i1][p1] > 0) continue;
for (j=0; j<list[i1][i2][i].n; j++)
for (k=0; k<list[i1][i3][i].n; k++)
{
p2 = list[i1][i2][i].p2[j];
if (p2 > nmax || tim[i2][p2] > 0) continue;
p3 = list[i1][i3][i].p2[k];
if (p3 > nmax || tim[i3][p3] > 0) continue;
for (m=0; m<list[i2][i3][p2].n; m++)
{
p31 = list[i2][i3][p2].p2[m];
if (p3 == p31)
{
corr = (list[i1][i2][i].corr[j]
+ list[i1][i3][i].corr[k]
+ list[i2][i3][p2].corr[m])
/ (list[i1][i2][i].dist[j]
+ list[i1][i3][i].dist[k]
+ list[i2][i3][p2].dist[m]);
if (corr > corrmin)
{ for (n=0; n<n_img; n++) con0[match0].p[n] = -2;
con0[match0].p[i1] = p1;
con0[match0].p[i2] = p2;
con0[match0].p[i3] = p3;
con0[match0++].corr = corr;
}
}
}
}
}
/* ----------------------------------------------------------------------- */
/* sort triplets for match quality (.corr) */
quicksort_con (con0, match0);
/* ----------------------------------------------------------------------- */
/* pragmatic version: */
/* take triplets from the top to the bottom of the sorted list */
/* only if none of the points has already been used */
for (i=0; i<match0; i++)
{
p1 = con0[i].p[0]; if (p1 > -1) if (++tim[0][p1] > 1) continue;
p2 = con0[i].p[1]; if (p2 > -1) if (++tim[1][p2] > 1) continue;
p3 = con0[i].p[2]; if (p3 > -1) if (++tim[2][p3] > 1) continue;
p4 = con0[i].p[3]; if (p4 > -1 && n_img > 3) if (++tim[3][p4] > 1) continue;
con[match++] = con0[i];
}
match3 = match - match4;
sprintf (buf, "%d consistent quadruplets, %d triplets ", match4, match3);
puts (buf);
/* repair artifact (?) */
if (n_img == 3) for (i=0; i<match; i++) con[i].p[3] = -1;
}
/* ----------------------------------------------------------------------- */
/* ----------------------------------------------------------------------- */
/* search consistent pairs : 12, 13, 14, 23, 24, 34 */
/* only if an object model is available or if only 2 images are used */
if(1<2 && n_img>1 && allCam_flag==0){
puts ("Search pairs");
match0 = 0;
for (i1=0; i1<n_img-1; i1++)
//if ( n_img == 2 || (num[0] < 64 && num[1] < 64 && num[2] < 64 && num[3] < 64))
if ( n_img > 1)
for (i2=i1+1; i2<n_img; i2++)
for (i=0; i<num[i1]; i++)
{
p1 = list[i1][i2][i].p1;
if (p1 > nmax || tim[i1][p1] > 0) continue;
/* take only unambigous pairs */
if (list[i1][i2][i].n != 1) continue;
p2 = list[i1][i2][i].p2[0];
if (p2 > nmax || tim[i2][p2] > 0) continue;
corr = list[i1][i2][i].corr[0] / list[i1][i2][i].dist[0];
if (corr > corrmin)
{
con0[match0].p[i1] = p1;
con0[match0].p[i2] = p2;
con0[match0++].corr = corr;
}
}
/* ----------------------------------------------------------------------- */
/* sort pairs for match quality (.corr) */
quicksort_con (con0, match0);
/* ----------------------------------------------------------------------- */
/* take pairs from the top to the bottom of the sorted list */
/* only if none of the points has already been used */
for (i=0; i<match0; i++)
{
p1 = con0[i].p[0]; if (p1 > -1) if (++tim[0][p1] > 1) continue;
p2 = con0[i].p[1]; if (p2 > -1) if (++tim[1][p2] > 1) continue;
p3 = con0[i].p[2]; if (p3 > -1 && n_img > 2)
if (++tim[2][p3] > 1) continue;
p4 = con0[i].p[3]; if (p4 > -1 && n_img > 3)
if (++tim[3][p4] > 1) continue;
con[match++] = con0[i];
}
} //end pairs?
match2 = match-match4-match3;
if(n_img==1){
sprintf (buf, "determined %d points from 2D", match1);
puts (buf);
}
else{
sprintf (buf, "%d consistent quadruplets(red), %d triplets(green) and %d unambigous pairs",
match4, match3, match2);
puts (buf);
}
Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY);
Tcl_Eval(interp, ".text delete 3");
Tcl_Eval(interp, ".text insert 3 $tbuf");
/* ----------------------------------------------------------------------- */
/* give each used pix the correspondence number */
for (i=0; i<match; i++){
for (j=0; j<n_img; j++){
if (con[i].p[j] > -1){ //Bug, detected in Nov 2011 by Koni&Beat
p1 = geo[j][con[i].p[j]].pnr;
if (p1 > -1 && p1 < 1202590843){
pix[j][p1].tnr= i;
}
}
}
}
/* draw crosses on canvas */
if (display) {
for (i=0; i<match4; i++) /* red crosses for quadruplets */
{
for (j=0; j<n_img; j++)
{
p1 = geo[j][con[i].p[j]].pnr;
if (p1 > -1)
{
if ( (fabs(pix[j][p1].x-zoom_x[j]) < imx/(2*zoom_f[j]))
&& (fabs(pix[j][p1].y-zoom_y[j]) < imy/(2*zoom_f[j])))
{
intx = (int) ( imx/2 + zoom_f[j] * (pix[j][p1].x-zoom_x[j]));
inty = (int) ( imy/2 + zoom_f[j] * (pix[j][p1].y-zoom_y[j]));
drawcross (interp, intx, inty, cr_sz, j, "red");
if (zoom_f[j]>=2) draw_pnr (interp, intx+5 , inty+0, i, j, "white");
}
}
}
}
for (i=match4; i<match4+match3; i++) /* green crosses for triplets */
{
for (j=0; j<n_img; j++)
{
p1 = geo[j][con[i].p[j]].pnr;
if (p1 > -1 && con[i].p[j] > -1)
{
if ( (fabs(pix[j][p1].x-zoom_x[j]) < imx/(2*zoom_f[j]))
&& (fabs(pix[j][p1].y-zoom_y[j]) < imy/(2*zoom_f[j])))
{
intx = (int) ( imx/2 + zoom_f[j] * (pix[j][p1].x-zoom_x[j]));
inty = (int) ( imy/2 + zoom_f[j] * (pix[j][p1].y-zoom_y[j]));
drawcross ( interp, intx, inty, cr_sz, j, "green" );
if (zoom_f[j]>=2) draw_pnr (interp, intx+5 , inty+0, i, j, "white");/* number of triplet */
}
}
}
}
for (i=match4+match3; i<match4+match3+match2; i++)
{ /* yellow crosses for pairs */
for (j=0; j<n_img; j++)
{
p1 = geo[j][con[i].p[j]].pnr;
if (p1 > -1 && con[i].p[j] > -1)
{
if ( (fabs(pix[j][p1].x-zoom_x[j]) < imx/(2*zoom_f[j]))
&& (fabs(pix[j][p1].y-zoom_y[j]) < imy/(2*zoom_f[j])))
{
intx = (int) ( imx/2 + zoom_f[j] * (pix[j][p1].x-zoom_x[j]));
inty = (int) ( imy/2 + zoom_f[j] * (pix[j][p1].y-zoom_y[j]));
drawcross (interp, intx, inty, cr_sz, j, "yellow");
if (zoom_f[j]>=2) draw_pnr (interp, intx+5 , inty+0, i, j, "white"); /* number of triplet */
}
}
}
}
for (j=0; j<n_img; j++)
{
for (i=0; i<num[j]; i++)
{ /* blue crosses for unused detections */
p1 = pix[j][i].tnr;
if (p1 == -1 )
{
if ( (fabs(pix[j][i].x-zoom_x[j]) < imx/(2*zoom_f[j]))
&& (fabs(pix[j][i].y-zoom_y[j]) < imy/(2*zoom_f[j])))
{
intx = (int) ( imx/2 + zoom_f[j] * (pix[j][i].x-zoom_x[j]));
inty = (int) ( imy/2 + zoom_f[j] * (pix[j][i].y-zoom_y[j]));
drawcross (interp, intx, inty, cr_sz, j, "blue");
}
}
}
}
}
/* ----------------------------------------------------------------------- */
/* free memory for lists of correspondences */
for (i1=0; i1<n_img-1; i1++)
{
for (i2=i1+1; i2<n_img; i2++) free (list[i1][i2]);
}
free (con0);
sprintf (buf,"Correspondences done");
Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY);
Tcl_Eval(interp, ".text delete 2");
Tcl_Eval(interp, ".text insert 2 $tbuf");
}