diff --git a/PTV/src_c/change_parameter.c b/PTV/src_c/change_parameter.c index b69e1ad..09f2b2c 100644 --- a/PTV/src_c/change_parameter.c +++ b/PTV/src_c/change_parameter.c @@ -1,16 +1,16 @@ /******************************************************************** - Author/Copyright: Jochen Willneff + Author/Copyright: Jochen Willneff - Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich + Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich - Creation Date: October'97 + Creation Date: October'97 - Description: editing of parameter files + Description: editing of parameter files - Routines contained: parameter_panel_init, done_proc_c + Routines contained: parameter_panel_init, done_proc_c *********************************************************************/ @@ -24,16 +24,17 @@ See the file license.txt for copying permission. #include "ptv.h" /********************************************************************/ -/* parameter_panel_init : initializes the parameter panel */ +/* parameter_panel_init : initializes the parameter panel */ /********************************************************************/ int parameter_panel_init(Tcl_Interp* interp) { - char val[256]; + char val[256]; - /* read 20 parameters from ptvppar */ + /* read 21 parameters from ptv.par */ - fp1 = fopen_r ("parameters/ptv.par"); + fp1 = fopen_rp ("parameters/ptv.par"); // replaced fopen_r, ad holten 12-2012 + if (! fp1) return TCL_OK; fscanf (fp1, "%s", val); Tcl_SetVar2(interp, "mp", "ncam", val, TCL_GLOBAL_ONLY); @@ -79,9 +80,10 @@ int parameter_panel_init(Tcl_Interp* interp) Tcl_SetVar2(interp, "mp", "thicknessglass", val, TCL_GLOBAL_ONLY); fclose (fp1); - /* read 15 parameters from targ_rec.par */ + /* read 16 parameters from targ_rec.par */ - fp1 = fopen_r ("parameters/targ_rec.par"); + fp1 = fopen_rp ("parameters/targ_rec.par"); // replaced fopen_r, ad holten 12-2012 + if (! fp1) return TCL_OK; fscanf (fp1, "%s", val); Tcl_SetVar2(interp, "mp", "partgv1", val, TCL_GLOBAL_ONLY); @@ -118,15 +120,18 @@ int parameter_panel_init(Tcl_Interp* interp) fclose (fp1); - fp1 = fopen_r ("parameters/pft_version.par"); - fscanf (fp1, "%s", val); + fp1 = fopen_rp ("parameters/pft_version.par"); // replaced fopen_r, ad holten 12-2012 + if (! fp1) return TCL_OK; + + fscanf (fp1, "%s", val); Tcl_SetVar2(interp, "mp", "target", val, TCL_GLOBAL_ONLY); fclose (fp1); /* read 12 parameters from criteria.par */ - fp1 = fopen_r ("parameters/criteria.par"); + fp1 = fopen_rp ("parameters/criteria.par"); // replaced fopen_r, ad holten 12-2012 + if (! fp1) return TCL_OK; fscanf (fp1, "%s", val); Tcl_SetVar2(interp, "mp", "xmin", val, TCL_GLOBAL_ONLY); @@ -157,7 +162,8 @@ int parameter_panel_init(Tcl_Interp* interp) /* read 6 parameters from sequence.par */ - fp1 = fopen_r ("parameters/sequence.par"); + fp1 = fopen_rp ("parameters/sequence.par"); // replaced fopen_r, ad holten 12-2012 + if (! fp1) return TCL_OK; fscanf (fp1, "%s", val); Tcl_SetVar2(interp, "mp", "basename1", val, TCL_GLOBAL_ONLY); @@ -176,7 +182,8 @@ int parameter_panel_init(Tcl_Interp* interp) /* read 12 parameters from cal_ori.par */ - fp1 = fopen_r ("parameters/cal_ori.par"); + fp1 = fopen_rp ("parameters/cal_ori.par"); // replaced fopen_r, ad holten 12-2012 + if (! fp1) return TCL_OK; fscanf (fp1, "%s", val); Tcl_SetVar2(interp, "cp", "platecoord", val, TCL_GLOBAL_ONLY); @@ -207,7 +214,8 @@ int parameter_panel_init(Tcl_Interp* interp) /* read 13 parameters from detect_plate.par */ - fp1 = fopen_r ("parameters/detect_plate.par"); + fp1 = fopen_rp ("parameters/detect_plate.par"); // replaced fopen_r, ad holten 12-2012 + if (! fp1) return TCL_OK; fscanf (fp1, "%s", val); Tcl_SetVar2(interp, "cp", "partgv1", val, TCL_GLOBAL_ONLY); @@ -242,7 +250,8 @@ int parameter_panel_init(Tcl_Interp* interp) /* read 16 parameters from man_ori.par */ - fp1 = fopen_r ("parameters/man_ori.par"); + fp1 = fopen_rp ("parameters/man_ori.par"); // replaced fopen_r, ad holten 12-2012 + if (! fp1) return TCL_OK; fscanf (fp1, "%s", val); Tcl_SetVar2(interp, "cp", "p11", val, TCL_GLOBAL_ONLY); @@ -280,8 +289,9 @@ int parameter_panel_init(Tcl_Interp* interp) fclose (fp1); - fp1 = fopen_r ("parameters/shaking.par"); - + fp1 = fopen_rp ("parameters/shaking.par"); // replaced fopen_r, ad holten 12-2012 + if (! fp1) return TCL_OK; + fscanf (fp1, "%s", val); Tcl_SetVar2(interp, "cp", "first_shake", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); @@ -296,7 +306,8 @@ int parameter_panel_init(Tcl_Interp* interp) /* read 11 parameters from orient.par */ - fp1 = fopen_r ("parameters/orient.par"); + fp1 = fopen_rp ("parameters/orient.par"); // replaced fopen_r, ad holten 12-2012 + if (! fp1) return TCL_OK; fscanf (fp1, "%s", val); Tcl_SetVar2(interp, "cp", "pnrori", val, TCL_GLOBAL_ONLY); @@ -325,9 +336,10 @@ int parameter_panel_init(Tcl_Interp* interp) fclose (fp1); - - fp1 = fopen_r ("parameters/examine.par"); + fp1 = fopen_rp ("parameters/examine.par"); // replaced fopen_r, ad holten 12-2012 + if (! fp1) return TCL_OK; + fscanf (fp1, "%s", val); Tcl_SetVar2(interp, "cp", "examineFlag", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); @@ -335,9 +347,10 @@ int parameter_panel_init(Tcl_Interp* interp) fclose (fp1); - /* read 5 parameters from track.par */ + /* read 22 parameters from track.par */ - fp1 = fopen_r ("parameters/track.par"); + fp1 = fopen_rp ("parameters/track.par"); // replaced fopen_r, ad holten 12-2012 + if (! fp1) return TCL_OK; fscanf (fp1, "%s", val); Tcl_SetVar2(interp, "tp", "dvxmin", val, TCL_GLOBAL_ONLY); @@ -365,47 +378,43 @@ int parameter_panel_init(Tcl_Interp* interp) fscanf (fp1, "%s", val); Tcl_SetVar2(interp, "tp", "dnx", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); - Tcl_SetVar2(interp, "tp", "dny", val, TCL_GLOBAL_ONLY); + Tcl_SetVar2(interp, "tp", "dny", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); */ /* 5 textboxes */ - Tcl_SetVar2(interp, "tp", "maxnum", val, TCL_GLOBAL_ONLY); + Tcl_SetVar2(interp, "tp", "maxnum", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); - Tcl_SetVar2(interp, "tp", "deltat", val, TCL_GLOBAL_ONLY); + Tcl_SetVar2(interp, "tp", "deltat", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); - Tcl_SetVar2(interp, "tp", "vmax", val, TCL_GLOBAL_ONLY); + Tcl_SetVar2(interp, "tp", "vmax", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); - Tcl_SetVar2(interp, "tp", "linktol", val, TCL_GLOBAL_ONLY); + Tcl_SetVar2(interp, "tp", "linktol", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); - Tcl_SetVar2(interp, "tp", "jumptol", val, TCL_GLOBAL_ONLY); + Tcl_SetVar2(interp, "tp", "jumptol", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); /* 8 checkboxes */ - Tcl_SetVar2(interp, "tp", "gluing", val, TCL_GLOBAL_ONLY); + Tcl_SetVar2(interp, "tp", "gluing", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); - Tcl_SetVar2(interp, "tp", "track", val, TCL_GLOBAL_ONLY); + Tcl_SetVar2(interp, "tp", "track", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); - Tcl_SetVar2(interp, "tp", "iterate", val, TCL_GLOBAL_ONLY); + Tcl_SetVar2(interp, "tp", "iterate", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); - Tcl_SetVar2(interp, "tp", "remkin", val, TCL_GLOBAL_ONLY); + Tcl_SetVar2(interp, "tp", "remkin", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); - Tcl_SetVar2(interp, "tp", "wbin", val, TCL_GLOBAL_ONLY); + Tcl_SetVar2(interp, "tp", "wbin", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); - Tcl_SetVar2(interp, "tp", "wtraj", val, TCL_GLOBAL_ONLY); + Tcl_SetVar2(interp, "tp", "wtraj", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); - Tcl_SetVar2(interp, "tp", "output", val, TCL_GLOBAL_ONLY); + Tcl_SetVar2(interp, "tp", "output", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); - Tcl_SetVar2(interp, "tp", "camconfig", val, TCL_GLOBAL_ONLY); + Tcl_SetVar2(interp, "tp", "camconfig", val, TCL_GLOBAL_ONLY); fscanf (fp1, "%s", val); - - - fclose (fp1); /* changed parameters are read out all together not before "done_proc_c" */ - - return TCL_OK; + return TCL_OK; } @@ -420,9 +429,7 @@ int done_proc_c(ClientData clientData, Tcl_Interp* interp, int argc, const char* { const char *valp; - /* rewrite all parameter files */ - fp1 = fopen ("parameters/ptv.par", "w"); valp = Tcl_GetVar2(interp, "mp", "ncam", TCL_GLOBAL_ONLY); @@ -445,7 +452,7 @@ int done_proc_c(ClientData clientData, Tcl_Interp* interp, int argc, const char* fprintf (fp1, "%s\n", valp); valp = Tcl_GetVar2(interp, "mp", "highpass", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); - valp = Tcl_GetVar2(interp, "mp", "allCam", TCL_GLOBAL_ONLY); + valp = Tcl_GetVar2(interp, "mp", "allCam", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); valp = Tcl_GetVar2(interp, "mp", "tiff", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); @@ -465,10 +472,9 @@ int done_proc_c(ClientData clientData, Tcl_Interp* interp, int argc, const char* fprintf (fp1, "%s\n", valp); valp = Tcl_GetVar2(interp, "mp", "water", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); - valp = Tcl_GetVar2(interp, "mp", "thicknessglass", TCL_GLOBAL_ONLY); + valp = Tcl_GetVar2(interp, "mp", "thicknessglass", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); - fclose (fp1); @@ -496,7 +502,7 @@ int done_proc_c(ClientData clientData, Tcl_Interp* interp, int argc, const char* fprintf (fp1, "%s\n", valp); valp = Tcl_GetVar2(interp, "mp", "pmaxnpixy", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); - valp = Tcl_GetVar2(interp, "mp", "psumgv", TCL_GLOBAL_ONLY); + valp = Tcl_GetVar2(interp, "mp", "psumgv", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); valp = Tcl_GetVar2(interp, "mp", "pcrossize", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); @@ -510,13 +516,12 @@ int done_proc_c(ClientData clientData, Tcl_Interp* interp, int argc, const char* fclose (fp1); fp1 = fopen ("parameters/pft_version.par", "w"); - valp = Tcl_GetVar2(interp, "mp", "target", TCL_GLOBAL_ONLY); + valp = Tcl_GetVar2(interp, "mp", "target", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); fclose (fp1); fp1 = fopen ("parameters/criteria.par", "w"); - valp = Tcl_GetVar2(interp, "mp", "xmin", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); valp = Tcl_GetVar2(interp, "mp", "xminzmin", TCL_GLOBAL_ONLY); @@ -529,9 +534,9 @@ int done_proc_c(ClientData clientData, Tcl_Interp* interp, int argc, const char* fprintf (fp1, "%s\n", valp); valp = Tcl_GetVar2(interp, "mp", "xmaxzmax", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); - valp = Tcl_GetVar2(interp, "mp", "nx", TCL_GLOBAL_ONLY); + valp = Tcl_GetVar2(interp, "mp", "nx", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); - valp = Tcl_GetVar2(interp, "mp", "ny", TCL_GLOBAL_ONLY); + valp = Tcl_GetVar2(interp, "mp", "ny", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); valp = Tcl_GetVar2(interp, "mp", "npix", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); @@ -539,7 +544,7 @@ int done_proc_c(ClientData clientData, Tcl_Interp* interp, int argc, const char* fprintf (fp1, "%s\n", valp); valp = Tcl_GetVar2(interp, "mp", "mincorr", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); - valp = Tcl_GetVar2(interp, "mp", "tolepi", TCL_GLOBAL_ONLY); + valp = Tcl_GetVar2(interp, "mp", "tolepi", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); fclose (fp1); @@ -621,11 +626,10 @@ int done_proc_c(ClientData clientData, Tcl_Interp* interp, int argc, const char* fprintf (fp1, "%s\n", valp); valp = Tcl_GetVar2(interp, "cp", "ppcrossize", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); - fclose (fp1); - + fp1 = fopen ("parameters/man_ori.par", "w"); valp = Tcl_GetVar2(interp, "cp", "p11", TCL_GLOBAL_ONLY); @@ -715,7 +719,6 @@ int done_proc_c(ClientData clientData, Tcl_Interp* interp, int argc, const char* fclose (fp1); - fp1 = fopen ("parameters/track.par", "w"); valp = Tcl_GetVar2(interp, "tp", "dvxmin", TCL_GLOBAL_ONLY); @@ -775,7 +778,6 @@ int done_proc_c(ClientData clientData, Tcl_Interp* interp, int argc, const char* valp = Tcl_GetVar2(interp, "tp", "camconfig", TCL_GLOBAL_ONLY); fprintf (fp1, "%s\n", valp); - fclose (fp1); - return TCL_OK; + return TCL_OK; } diff --git a/PTV/src_c/checkpoints.c b/PTV/src_c/checkpoints.c index 138877b..0bfe096 100644 --- a/PTV/src_c/checkpoints.c +++ b/PTV/src_c/checkpoints.c @@ -1,19 +1,19 @@ /**************************************************************************** -Routine: checkpoints.c +Routine: checkpoints.c -Author/Copyright: Hans-Gerd Maas +Author/Copyright: Hans-Gerd Maas -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: 1988 +Creation Date: 1988 -Description: calibration quality control from check points - on calibration plate - -Routines contained: +Description: calibration quality control from check points + on calibration plate + +Routines contained: ****************************************************************************/ /* @@ -26,121 +26,120 @@ See the file license.txt for copying permission. void checkpoint_proc (Tcl_Interp* interp) { - int i, count1 = 0, useflag; - int intx1, inty1, intx2, inty2; - double X, Y, Z; - double sigmax1=0, sigmay1=0, sigmaz1=0; - FILE *fp1, *fp3; - coord_3d fix[256]; - - puts ("check points"); - - for (i=0; i 0 && crd[0][i].pnr != fix[i].pnr) continue; - if (n_img > 1 && crd[1][i].pnr != fix[i].pnr) continue; - if (n_img > 2 && crd[2][i].pnr != fix[i].pnr) continue; - if (n_img > 3 && crd[3][i].pnr != fix[i].pnr) continue; - - /* do not use the corner points of plate 85 */ - if (nfix == 85 && fix[i].pnr == 1) continue; - if (nfix == 85 && fix[i].pnr == 7) continue; - if (nfix == 85 && fix[i].pnr == 43) continue; - if (nfix == 85 && fix[i].pnr == 49) continue; - - switch (n_img) - { - case 2: det_lsq_2 (Ex, I, G, ap, mmp, - crd[0][i].x,crd[0][i].y, - crd[1][i].x,crd[1][i].y, - &X,&Y,&Z); break; - case 3: det_lsq_3 (Ex, I, G, ap, mmp, - crd[0][i].x,crd[0][i].y, - crd[1][i].x,crd[1][i].y, - crd[2][i].x,crd[2][i].y, - &X,&Y,&Z); break; - case 4: det_lsq_4 (Ex, I, G, ap, mmp, - crd[0][i].x,crd[0][i].y, - crd[1][i].x,crd[1][i].y, - crd[2][i].x,crd[2][i].y, - crd[3][i].x,crd[3][i].y, - &X,&Y,&Z); break; + + /* read calibration plate coordinates */ + fp3 = fopen_rp (fixp_name); // replaced fopen_r(), ad holten, 12-2012 + if (!fp3) return; + for (i=0; i 0 && crd[0][i].pnr != fix[i].pnr) continue; + if (n_img > 1 && crd[1][i].pnr != fix[i].pnr) continue; + if (n_img > 2 && crd[2][i].pnr != fix[i].pnr) continue; + if (n_img > 3 && crd[3][i].pnr != fix[i].pnr) continue; + + // commented out, ad holten, 14-2012 + // /* do not use the corner points of plate 85 */ + // if (nfix == 85 && fix[i].pnr == 1) continue; + // if (nfix == 85 && fix[i].pnr == 7) continue; + // if (nfix == 85 && fix[i].pnr == 43) continue; + // if (nfix == 85 && fix[i].pnr == 49) continue; + + switch (n_img) { + case 2: det_lsq_2 (Ex, I, G, ap, mmp, + crd[0][i].x,crd[0][i].y, + crd[1][i].x,crd[1][i].y, + &X,&Y,&Z); break; + case 3: det_lsq_3 (Ex, I, G, ap, mmp, + crd[0][i].x,crd[0][i].y, + crd[1][i].x,crd[1][i].y, + crd[2][i].x,crd[2][i].y, + &X,&Y,&Z); break; + case 4: det_lsq_4 (Ex, I, G, ap, mmp, + crd[0][i].x,crd[0][i].y, + crd[1][i].x,crd[1][i].y, + crd[2][i].x,crd[2][i].y, + crd[3][i].x,crd[3][i].y, + &X,&Y,&Z); break; + } + + /* compute deviations */ + sigmax1 += (X - fix[i].x) * (X - fix[i].x); + sigmay1 += (Y - fix[i].y) * (Y - fix[i].y); + sigmaz1 += (Z - fix[i].z) * (Z - fix[i].z); + printf ("%3d - %7.3f %7.3f %7.3f | %6.3f %6.3f %6.3f\n", + fix[i].pnr, X,Y,Z, X-fix[i].x, Y-fix[i].y, Z-fix[i].z); + + /* draw residual vectors into img0 */ + intx1 = (int) pix[0][i].x; + inty1 = (int) pix[0][i].y; + intx2 = (int)(intx1 + (fix[i].x - X)*5*Ex[0].z0/I[0].cc); + inty2 = (int)(inty1 + (fix[i].y - Y)*5*Ex[0].z0/I[0].cc); + drawvector (interp,intx1, inty1, intx2, inty2, 1, 0, "yellow"); + inty2 = (int)(inty1 + (fix[i].z - Z)*5*Ex[0].z0/I[0].cc); + drawvector (interp, intx1, inty1, intx1, inty2, 1, 0, "blue"); + + count1++; } - - /* compute deviations */ - sigmax1 += (X - fix[i].x) * (X - fix[i].x); - sigmay1 += (Y - fix[i].y) * (Y - fix[i].y); - sigmaz1 += (Z - fix[i].z) * (Z - fix[i].z); - printf ("%3d - %7.3f %7.3f %7.3f | %6.3f %6.3f %6.3f\n", - fix[i].pnr, X,Y,Z, X-fix[i].x, Y-fix[i].y, Z-fix[i].z); - - - /* draw residual vectors into img0 */ - intx1 = (int) pix[0][i].x; - inty1 = (int) pix[0][i].y; - intx2 = intx1 + (fix[i].x - X)*5*Ex[0].z0/I[0].cc; - inty2 = inty1 + (fix[i].y - Y)*5*Ex[0].z0/I[0].cc; - drawvector (interp,intx1, inty1, intx2, inty2, 1, 0, "yellow"); - inty2 = inty1 + (fix[i].z - Z)*5*Ex[0].z0/I[0].cc; - drawvector (interp, intx1, inty1, intx1, inty2, 1, 0, "blue"); - - count1++; - } - - - /* compute a priori RMS */ - rmsX = sqrt(rmsX/count1); - rmsY = sqrt(rmsY/count1); - rmsZ = sqrt(rmsZ/count1); - mean_sigma0 = sqrt (mean_sigma0/count1); - printf ("RMS from %d checkpoints:\n", count1); - printf ("a priori => sigma0 = %4.2f micron, RMS = %6.3f/%6.3f/%6.3f mm", - mean_sigma0*1000, rmsX, rmsY, rmsZ); - - - /* compute a posteriori RMS from check points */ - puts ("\n accuracies from check points:\n"); - sigmax1 = sqrt (sigmax1/count1); - sigmay1 = sqrt (sigmay1/count1); - sigmaz1 = sqrt (sigmaz1/count1); - printf ("a posteriori => RMS = %6.3f/%6.3f/%6.3f mm\n", - sigmax1, sigmay1, sigmaz1); - - - sprintf (buf, "%d check points => mx=%6.3f mm, my=%6.3f mm, mz=%6.3f mm", - count1, sigmax1, sigmay1, sigmaz1); puts (buf); + + /* compute a priori RMS */ + rmsX = sqrt(rmsX/count1); + rmsY = sqrt(rmsY/count1); + rmsZ = sqrt(rmsZ/count1); + mean_sigma0 = sqrt (mean_sigma0/count1); + printf ("RMS from %d checkpoints:\n", count1); + printf ("a priori => sigma0 = %4.2f micron, RMS = %6.3f/%6.3f/%6.3f mm", + mean_sigma0*1000, rmsX, rmsY, rmsZ); + + /* compute a posteriori RMS from check points */ + puts ("\n accuracies from check points:\n"); + sigmax1 = sqrt (sigmax1/count1); + sigmay1 = sqrt (sigmay1/count1); + sigmaz1 = sqrt (sigmaz1/count1); + printf ("a posteriori => RMS = %6.3f/%6.3f/%6.3f mm\n", + sigmax1, sigmay1, sigmaz1); + + sprintf (buf, "%d check points => mx=%6.3f mm, my=%6.3f mm, mz=%6.3f mm", + count1, sigmax1, sigmay1, sigmaz1); + puts (buf); } diff --git a/PTV/src_c/correspondences.c b/PTV/src_c/correspondences.c index 1d1658c..ee0de07 100644 --- a/PTV/src_c/correspondences.c +++ b/PTV/src_c/correspondences.c @@ -1,18 +1,19 @@ /**************************************************************************** -Routine: correspondences.c +Routine: correspondences.c -Author/Copyright: Hans-Gerd Maas +Author/Copyright: Hans-Gerd Maas -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: 1988/89 +Creation Date: 1988/89 -Description: establishment of correspondences for 2/3/4 cameras +Description: establishment of correspondences for 2/3/4 cameras ****************************************************************************/ + /* Copyright (c) 1990-2011 ETH Zurich @@ -24,476 +25,444 @@ See the file license.txt for copying permission. /* #define maxcand 100 */ + /****************************************************************************/ /*--------------- 4 camera model: consistent quadruplets -------------------*/ /****************************************************************************/ 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]; -/* ----------------------------------------------------------------------- */ + 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, pt1; + int tim[4][nmax], intx, inty; + double xa12, ya12, xb12, yb12, X, Y, Z, corr; + candidate cand[maxcand]; + n_tupel *con0; + correspond *list[4][4]; + /* ----------------------------------------------------------------------- */ + + /* allocate memory for lists of correspondences */ + for (i1=0; i1 2,3,4 + 2 -> 3,4 + 3 -> 4 */ + + for (i1=0; i1 maxcand) count = maxcand; + for (j=0; j>>>>>>>>> correspondences_4: ERROR on line 144, CHECK CODE <<<<<<<<<<<<<<<< \n"); + i = nmax; j = 4; + } - /* ----------------------------------------------------------------------- */ + /* ------------------------------------------------------------------ */ + /* ------------------------------------------------------------------ */ + + /* search consistent quadruplets in the list */ + if (n_img == 4) { + puts ("Search consistent quadruplets"); + for (i=0, match0=0; i 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) */ -printf("in corres zmin0: %f, zmax0: %f\n", Zmin_lay[0],Zmax_lay[0] ); + quicksort_con (con0, match0); - /* 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; + /* take quadruplets from the top to the bottom of the sorted list + only if none of the points has already been used */ + // simplified 'if (p1 > -1) if (++tim[0][p1] > 1)' to + // 'if (p1 > -1 && ++tim[0][p1] > 1)' and so on, ad holten 12-2012 + for (i=0, match=0; i -1 && ++tim[0][p1] > 1) continue; + p2 = con0[i].p[1]; if (p2 > -1 && ++tim[1][p2] > 1) continue; + p3 = con0[i].p[2]; if (p3 > -1 && ++tim[2][p3] > 1) continue; + p4 = con0[i].p[3]; if (p4 > -1 && ++tim[3][p4] > 1) continue; + con[match++] = con0[i]; + } - for (i1=0; i1 2,3,4 + 2 -> 3,4 + 3 -> 4 */ - - for (i1=0; i1 nmax || tim[i1][p1] > 0) continue; - o = epi_mm (xa12, ya12, - Ex[i2], I[i2], G[i2], Ex[i1], I[i1], G[i1], mmp, - &xa12, &ya12, &xb12, &yb12);*/ + for (j=0; j 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 corrmin) { + for (n=0; n maxcand) { count = maxcand; } - for (j=0; j -1 && ++tim[0][p1] > 1) continue; + p2 = con0[i].p[1]; if (p2 > -1 && ++tim[1][p2] > 1) continue; + p3 = con0[i].p[2]; if (p3 > -1 && ++tim[2][p3] > 1) continue; + p4 = con0[i].p[3]; if (p4 > -1 && n_img > 3 && ++tim[3][p4] > 1) continue; - /* ------------------------------------------------------------------ */ - /* ------------------------------------------------------------------ */ + con[match++] = con0[i]; + } - /* search consistent quadruplets in the list */ - if (n_img == 4) - { - puts ("Search consistent quadruplets"); - for (i=0, match0=0; i 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]; - } - } + match3 = match - match4; + sprintf (buf, "%d consistent quadruplets, %d triplets ", match4, match3); + puts (buf); + + ///* repair artifact (?) */ // commented out ad holten, 12-2012 + //if (n_img == 3) + // for (i=0; i>>>>>>>>>>>>> correspondences_4: ERROR on line 144, CHECK CODE <<<<<<<<<<<<<<<<< \n"); + con[i].p[3] = -1; } - } - } } } + /* ----------------------------------------------------------------------- */ + /* ----------------------------------------------------------------------- */ - /* -------------------------------------------------------------------- */ + // search consistent pairs : 12, 13, 14, 23, 24, 34 - /* sort quadruplets for match quality (.corr) */ - quicksort_con (con0, match0); + // only if an object model is available or if only 2 images are used + // if (1<2 && n_img>1 && allCam_flag==0) { // removed 1<2, ad holten, 12-2012 + if (n_img>1 && allCam_flag==0) { + puts ("Search pairs"); - /* -------------------------------------------------------------------- */ + match0 = 0; + for (i1=0; i1 1) { + for (i2=i1+1; i2 nmax || tim[i1][p1] > 0) continue; - /* 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 -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]; - } + /* take only unambigous pairs */ + if (list[i1][i2][i].n != 1) continue; - 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 nmax || tim[i1][p1] > 0) continue; - - for (j=0; j 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 corrmin) - { for (n=0; n 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); - /* sort triplets 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 -1 && ++tim[0][p1] > 1) continue; + p2 = con0[i].p[1]; if (p2 > -1 && ++tim[1][p2] > 1) continue; + p3 = con0[i].p[2]; if (p3 > -1 && n_img > 2 && ++tim[2][p3] > 1) continue; + p4 = con0[i].p[3]; if (p4 > -1 && n_img > 3 && ++tim[3][p4] > 1) continue; - /* 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 -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]; + } + } // end pairs - con[match++] = con0[i]; + match2 = match-match4-match3; + if (n_img == 1) { + sprintf (buf, "determined %d points from 2D", match1); + puts (buf); } - - match3 = match - match4; - sprintf (buf, "%d consistent quadruplets, %d triplets ", match4, match3); - puts (buf); - - /* repair artifact (?) */ - if (n_img == 3) for (i=0; i1 && allCam_flag==0){ - puts ("Search pairs"); - - - match0 = 0; - for (i1=0; i1 1) - for (i2=i1+1; i2 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 -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 -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 -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 -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 */ - } + 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 -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; + } - } - } - } - for (i=match4+match3; i -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 -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 -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 -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 (i=0; i-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 i, i_seq, nr, j,pft_version=3, num_points; + char name[128]; + unsigned char *imgf; + Tk_PhotoHandle img_handle; + Tk_PhotoImageBlock img_block; + + nr = atoi(argv[1]); + + fpp = fopen_rp ("parameters/sequence.par"); // replaced fopen_r, ad holten 12-2012 + if (!fpp) return TCL_OK; + 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_rp(name); // replaced fopen_r, ad holten 12-2012 + 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_rp(filename); // replaced fopen, ad holten 12-2012 + if (!fp1) return TCL_OK; + + fscanf (fp1, "%d\n", &nt4[3][nr]); + for (j=0; j=n_img; j--) geo[i][j].pnr = -1; + } + fclose (fp1); + if (display) { + for (j=0; j-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; } diff --git a/PTV/src_c/draw.c b/PTV/src_c/draw.c index da19df5..7d7fa03 100644 --- a/PTV/src_c/draw.c +++ b/PTV/src_c/draw.c @@ -1,327 +1,319 @@ /**************************************************************************** -Author/Copyright: Jochen Willneff +Author/Copyright: Jochen Willneff -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: end of 97 +Creation Date: end of 97 -Description: different drawing function to interact - with Tcl/Tk +Description: different drawing function to interact + with Tcl/Tk -Routines contained: drawcross, drawvector, draw_pnr, mark_detections - mark_correspondences, mark_corr, mark_track_c +Routines contained: drawcross, drawvector, draw_pnr, mark_detections + mark_correspondences, mark_corr, mark_track_c ****************************************************************************/ #include "ptv.h" -int drawcross (Tcl_Interp* interp, int x0, int y0, int size, int imgnr, char color[256]) +int drawcross (Tcl_Interp* interp, int x0, int y0, int size, int imgnr, char color[]) { - char val[256]; - sprintf(val, "markparticle %d %d %d %d %s", x0, y0, size, imgnr+1, color); - Tcl_Eval(interp, val ); - return TCL_OK; + char val[256]; + sprintf(val, "markparticle %d %d %d %d %s", x0, y0, size, imgnr+1, color); + Tcl_Eval(interp, val ); + return TCL_OK; } -int drawvector (Tcl_Interp* interp, int x0, int y0, int x1, int y1, int width, int imgnr, char color[256]) +int drawvector (Tcl_Interp* interp, int x0, int y0, int x1, int y1, int width, int imgnr, char color[]) { - char val[256]; - sprintf(val, "drawline %d %d %d %d %d %d %s", x0, y0, x1, y1, width, imgnr+1, color); - Tcl_Eval(interp, val ); - return TCL_OK; + char val[256]; + sprintf(val, "drawline %d %d %d %d %d %d %s", x0, y0, x1, y1, width, imgnr+1, color); + Tcl_Eval(interp, val ); + return TCL_OK; } -int draw_pnr (Tcl_Interp* interp, int x, int y, int pnr, int imgnr, char color[256]) +int draw_pnr (Tcl_Interp* interp, int x, int y, int pnr, int imgnr, char color[]) { - char val[256]; - sprintf (val, "%d", pnr); - sprintf(buf, "drawtext %d %d %s %d %s", x, y, val, imgnr+1, color); - Tcl_Eval(interp, buf ); - return TCL_OK; + char val[256]; + sprintf (val, "%d", pnr); + sprintf(buf, "drawtext %d %d %s %d %s", x, y, val, imgnr+1, color); + Tcl_Eval(interp, buf ); + return TCL_OK; } -int draw_value (Tcl_Interp* interp, int x, int y, double pnr, int imgnr, char color[256]) +int draw_value (Tcl_Interp* interp, int x, int y, double pnr, int imgnr, char color[]) { - char val[256]; - sprintf (val, "%5.3f", pnr); - sprintf(buf, "drawtext %d %d %s %d %s", x, y, val, imgnr+1, color); - Tcl_Eval(interp, buf ); - return TCL_OK; + char val[256]; + sprintf (val, "%5.3f", pnr); + sprintf(buf, "drawtext %d %d %s %d %s", x, y, val, imgnr+1, color); + Tcl_Eval(interp, buf ); + return TCL_OK; } void mark_detections (Tcl_Interp* interp, int nr) /* draws crosses for detected points in a displayed image */ { - int i,limx, limy, intx, inty; - - if (num[nr] == 0) - { - printf ("No points detected"); return; - } - limy = imy/(2*zoom_f[nr]); - limx = imx/(2*zoom_f[nr]); - for (i=0; i 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"); - - drawcross (interp, intx, inty, cr_sz, nr, buf); - - /* draw point number */ - if ( examine && zoom_f[nr] > 2 ) - { - /* number of established correspondence */ - /* - draw_pnr (interp, intx+3 , inty+3, i, nr, "white"); - */ - } - } - } -} + int i,j, pnr, lim, sum, intx, inty; + double x, y; + + if (match == 0) return; + + lim = imx/(2*zoom_f[nr]); + + for (i=0; i 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"); + drawcross (interp, intx, inty, cr_sz, nr, buf); + + /* draw point number */ + // if (examine && zoom_f[nr] > 2) { + // /* number of established correspondence */ + // draw_pnr (interp, intx+3 , inty+3, i, nr, "white"); + // } + } + } +} 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; + int i,j, pnr, sum, intx, inty; + double x, y; - if (match == 0) return; + if (match == 0) return; - for (i=0; i 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"); - */ - } + for (i=0; i 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 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-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"); } + 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_rp ("parameters/sequence.par"); // replaced fopen_r, ad holten 12-2012 + if (!fpp) return TCL_OK; + 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 -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"); } - } - 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"); - 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; + 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"); - + 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_rp ("parameters/sequence.par"); // replaced fopen_r, ad holten 12-2012 + if (!fpp) return TCL_OK; + + /* 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"); + + line1 = line2 = NULL; // added, ad holten, 12-2012 + for (i=seq_first; i= 0) { + for (k=0; k= 0) { - for (k=0; k O2 */ - vect1[0] = Ex2.x0 - Ex1.x0; - vect1[1] = Ex2.y0 - Ex1.y0; - vect1[2] = Ex2.z0 - Ex1.z0; + /* base O1 -> O2 */ + vect1[0] = Ex2.x0 - Ex1.x0; + vect1[1] = Ex2.y0 - Ex1.y0; + vect1[2] = Ex2.z0 - Ex1.z0; - /* coordinates of arbitrary point P1 in image1 */ - p1l[0] = xl; p1l[1] = yl; p1l[2] = - I1.cc; + /* coordinates of arbitrary point P1 in image1 */ + p1l[0] = xl; p1l[1] = yl; p1l[2] = -I1.cc; - /* beam O1 -> P in space */ - matmul (vect2, Ex1.dm, p1l, 3,3,1); + /* beam O1 -> P in space */ + matmul (vect2, Ex1.dm, p1l, 3,3,1); - /* normale to epipolar plane */ - crossprod (vect1,vect2,nk); + /* normale to epipolar plane */ + crossprod (vect1,vect2,nk); - /* normale to image2 */ - vect3[0] = 0; vect3[1] = 0; vect3[2] = - I2.cc; + /* normale to image2 */ + vect3[0] = 0; vect3[1] = 0; vect3[2] = -I2.cc; - /* normale to image 2, in space */ - matmul (n2, Ex2.dm, vect3, 3,3,1); + /* normale to image 2, in space */ + matmul (n2, Ex2.dm, vect3, 3,3,1); - /* epipolar line in image2, in space */ - crossprod (nk,n2,K2); + /* epipolar line in image2, in space */ + crossprod (nk,n2,K2); - /* epipolar line in image2 */ - for (i=0; i<3; i++) for (j=0; j<3; j++) D2t[i][j] = Ex2.dm[j][i]; - matmul (k2, D2t, K2, 3,3,1); - m2 = k2[1] / k2[0]; - return (m2); + /* epipolar line in image2 */ + for (i=0; i<3; i++) + for (j=0; j<3; j++) + D2t[i][j] = Ex2.dm[j][i]; + matmul (k2, D2t, K2, 3,3,1); + m2 = k2[1] / k2[0]; + return (m2); } - - - -int epi_mm (x1, y1, Ex1, I1, G1, Ex2, I2, G2, mmp, xmin, ymin, xmax, ymax) - -double x1, y1; /* input coord */ -Exterior Ex1, Ex2; /* orientation data */ -Interior I1, I2; /* orientation data */ -Glass G1, G2; /* glass data */ -mm_np mmp; /* multimed param. (layers) */ -double *xmin, *ymin, *xmax, *ymax; /* output search window */ - +#endif + + +int epi_mm (double x1, double y1, + Exterior Ex1, Interior I1, Glass G1, + Exterior Ex2, Interior I2, Glass G2, mm_np mmp, + double* xmin, double* ymin, double* xmax, double* ymax) + // double x1, y1 input coord + // Exterior Ex1, Ex2 orientation data + // Interior I1, I2 orientation data + // Glass G1, G2; glass data + // mm_np mmp; multimed param. (layers) + // double *xmin, *ymin, *xmax, *ymax output search window { - /* ray tracing gives the point of exit and the direction - cosines at the waterside of the glass; - min. and max. depth give window in object space, - which can be transformed into _2 image - (use img_xy_mm because of comparison with img_geo) */ + /* ray tracing gives the point of exit and the direction cosines + at the waterside of the glass; + min. and max. depth give window in object space, which can be + transformed into _2 image + (use img_xy_mm because of comparison with img_geo) */ - double a, b, c, xa,ya,xb,yb; - double X1,Y1,Z1, X, Y, Z; - double Zmin, Zmax; + double a, b, c, xa,ya,xb,yb; + double X1,Y1,Z1, X, Y, Z; + double Zmin, Zmax; - //ray_tracing (x1,y1, Ex1, I1, mmp, &X1, &Y1, &Z1, &a, &b, &c); - ray_tracing_v2 (x1,y1, Ex1, I1, G1, mmp, &X1, &Y1, &Z1, &a, &b, &c); + //ray_tracing (x1,y1, Ex1, I1, mmp, &X1, &Y1, &Z1, &a, &b, &c); + ray_tracing_v2 (x1,y1, Ex1, I1, G1, mmp, &X1, &Y1, &Z1, &a, &b, &c); - /* calculate min and max depth for position (valid only for one setup) */ - Zmin = Zmin_lay[0] - + (X1-X_lay[0]) * (Zmin_lay[1]-Zmin_lay[0]) / (X_lay[1]-X_lay[0]); - Zmax = Zmax_lay[0] - + (X1-X_lay[0]) * (Zmax_lay[1]-Zmax_lay[0]) / (X_lay[1]-X_lay[0]); + /* calculate min and max depth for position (valid only for one setup) */ + Zmin = Zmin_lay[0] + + (X1-X_lay[0]) * (Zmin_lay[1]-Zmin_lay[0]) / (X_lay[1]-X_lay[0]); + Zmax = Zmax_lay[0] + + (X1-X_lay[0]) * (Zmax_lay[1]-Zmax_lay[0]) / (X_lay[1]-X_lay[0]); - Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - //img_xy_mm_geo_old (X,Y,Z, Ex2, I2, mmp, &xa, &ya); - img_xy_mm_geo (X,Y,Z, Ex2, I2, G2, mmp, &xa, &ya); + Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + //img_xy_mm_geo_old (X,Y,Z, Ex2, I2, mmp, &xa, &ya); + img_xy_mm_geo (X,Y,Z, Ex2, I2, G2, mmp, &xa, &ya); - Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - //img_xy_mm_geo_old (X,Y,Z, Ex2, I2, mmp, &xb, &yb); - img_xy_mm_geo (X,Y,Z, Ex2, I2, G2, mmp, &xb, &yb); + Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + //img_xy_mm_geo_old (X,Y,Z, Ex2, I2, mmp, &xb, &yb); + img_xy_mm_geo (X,Y,Z, Ex2, I2, G2, mmp, &xb, &yb); - /* ==> window given by xa,ya,xb,yb */ + /* ==> window given by xa,ya,xb,yb */ - *xmin = xa; *ymin = ya; *xmax = xb; *ymax = yb; + *xmin = xa; *ymin = ya; *xmax = xb; *ymax = yb; - return (0); + return (0); } -int epi_mm_2D (x1, y1, Ex1, I1, G1, mmp, xp,yp,zp) - -double x1, y1; /* input coord */ -Exterior Ex1; /* orientation data */ -Interior I1; /* orientation data */ -Glass G1; /* glass data */ -mm_np mmp; /* multimed param. (layers) */ -double *xp, *yp, *zp; -//double *xmin, *ymin, *xmax, *ymax; /* output search window */ - +int epi_mm_2D (double x1, double y1, Exterior Ex1, Interior I1, Glass G1, + mm_np mmp, double *xp, double *yp, double *zp) + // double x1, y1 input coord + // Exterior Ex1 orientation data + // Interior I1 orientation data + // Glass G1 glass data + // mm_np mmp multimed param. (layers) + // double *xp, *yp, *zp; { - /* ray tracing gives the point of exit and the direction - cosines at the waterside of the glass; - min. and max. depth give window in object space, - which can be transformed into _2 image - (use img_xy_mm because of comparison with img_geo) */ + /* ray tracing gives the point of exit and the direction cosines + at the waterside of the glass; + min. and max. depth give window in object space, which can be + transformed into _2 image */ + double a, b, c; + double X1,Y1,Z1,X,Y,Z; - double a, b, c, xa,ya,xb,yb; - double X1,Y1,Z1,X,Y,Z; - - double Zmin, Zmax; + double Zmin, Zmax; - ray_tracing_v2 (x1,y1, Ex1, I1, G1, mmp, &X1, &Y1, &Z1, &a, &b, &c); + ray_tracing_v2 (x1,y1, Ex1, I1, G1, mmp, &X1, &Y1, &Z1, &a, &b, &c); - /* calculate min and max depth for position (valid only for one setup) */ - Zmin = Zmin_lay[0] - + (X1-X_lay[0]) * (Zmin_lay[1]-Zmin_lay[0]) / (X_lay[1]-X_lay[0]); - Zmax = Zmax_lay[0] - + (X1-X_lay[0]) * (Zmax_lay[1]-Zmax_lay[0]) / (X_lay[1]-X_lay[0]); + /* calculate min and max depth for position (valid only for one setup) */ + Zmin = Zmin_lay[0] + + (X1-X_lay[0]) * (Zmin_lay[1]-Zmin_lay[0]) / (X_lay[1]-X_lay[0]); + Zmax = Zmax_lay[0] + + (X1-X_lay[0]) * (Zmax_lay[1]-Zmax_lay[0]) / (X_lay[1]-X_lay[0]); + Z = 0.5*(Zmin+Zmax); + X = X1 + (Z-Z1) * a/c; + Y = Y1 + (Z-Z1) * b/c; - Z = 0.5*(Zmin+Zmax); - X = X1 + (Z-Z1) * a/c; - Y = Y1 + (Z-Z1) * b/c; - - *xp=X; *yp=Y; *zp=Z; + *xp=X; *yp=Y; *zp=Z; - return (0); + return (0); } -void find_candidate (crd, pix, num, xa,ya,xb,yb,eps, n, nx, ny, sumg, - cand, count, nr) - -/* binarized search in a x-sorted coord-set, exploits shape information */ -/* gives messages (in examination) */ - -coord_2d crd[]; /* metric coordinates */ -target pix[]; /* pixel data for correlation */ -int num, *count, nr; -double xa, ya, xb, yb, eps; -int n, nx, ny, sumg; -candidate cand[]; - +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +void find_candidate (coord_2d crd[], target pix[], int num, + double xa, double ya, double xb, double yb, + double eps, int n, int nx, int ny, int sumg, + candidate cand[], int *count, int nr) + // binarized search in a x-sorted coord-set, exploits shape information + // gives messages (in examination) + // coord_2d crd[] metric coordinates + // target pix[] pixel data for correlation { - register int j; - int j0, dj, p2; - double m, b, d, temp, qn, qnx, qny, qsumg, corr; - double xmin = -4.40, xmax = 4.40, ymin = -2.94, ymax = 2.94; - /* max. sensor format (HR 480 / Maxscan) */ - char str[64], buf[32]; - - - - /* define sensor format for search interrupt */ - xmin = (-1) * pix_x * imx/2; xmax = pix_x * imx/2; - ymin = (-1) * pix_y * imy/2; ymax = pix_y * imy/2; - xmin -= I[nr].xh; ymin -= I[nr].yh; - xmax -= I[nr].xh; ymax -= I[nr].yh; - correct_brown_affin (xmin,ymin, ap[nr], &xmin,&ymin); - correct_brown_affin (xmax,ymax, ap[nr], &xmax,&ymax); + register int j; + int j0, dj, p2; + double m, b, d, temp, qn, qnx, qny, qsumg, corr; + double xmin = -4.40, xmax = 4.40, ymin = -2.94, ymax = 2.94; + /* max. sensor format (HR 480 / Maxscan) */ + char str[64], buf[32]; + + /* define sensor format for search interrupt */ + xmin = -pix_x * imx/2; xmax = pix_x * imx/2; + ymin = -pix_y * imy/2; ymax = pix_y * imy/2; + xmin -= I[nr].xh; ymin -= I[nr].yh; + xmax -= I[nr].xh; ymax -= I[nr].yh; + correct_brown_affin (xmin,ymin, ap[nr], &xmin,&ymin); + correct_brown_affin (xmax,ymax, ap[nr], &xmax,&ymax); + + if (nr != 0) { + strcpy (str, ""); + strcpy (buf, ""); + puts (str); + } + for (j=0; j<8; j++) { /* initialize */ + cand[j].pnr = -999; + cand[j].tol = -999; + cand[j].corr = -999; + } + m = (yb-ya)/(xb-xa); b = ya - m*xa; /* line equation: y = m*x + b */ - if (nr != 0) - { - strcpy (str, ""); strcpy (buf, ""); puts (str); - } + if (xa > xb) { temp = xa; xa = xb; xb = temp; } /* sort search window */ + if (ya > yb) { temp = ya; ya = yb; yb = temp; } - for (j=0; j<8; j++) /* initialize */ - { - cand[j].pnr = -999; cand[j].tol = -999; cand[j].corr = -999; - } + if ((xb>xmin) && (xaymin) && (ya1; dj/=2) { + if (crd[j0].x < (xa - eps)) j0 += dj; + else j0 -= dj; + } + j0 -= 12; if (j0 < 0) j0 = 0; /* due to truncation */ + + for (j=j0, *count=0; j xb+eps) return; /* stop search */ + + if ((crd[j].y > ya-eps) && (crd[j].y < yb+eps)) { + if ((crd[j].x > xa-eps) && (crd[j].x < xb+eps)) { + d = fabs ((crd[j].y - m*crd[j].x - b) / sqrt(m*m+1)); + if (d < eps) { + p2 = crd[j].pnr; + if (n < pix[p2].n) qn = (double) n/pix[p2].n; + else qn = (double) pix[p2].n/n; + if (nx < pix[p2].nx) qnx = (double) nx/pix[p2].nx; + else qnx = (double) pix[p2].nx/nx; + if (ny < pix[p2].ny) qny = (double) ny/pix[p2].ny; + else qny = (double) pix[p2].ny/ny; + if (sumg < pix[p2].sumg) qsumg = (double) sumg/pix[p2].sumg; + else qsumg = (double) pix[p2].sumg/sumg; + + /* empirical correlation coefficient from shape and + brightness parameters */ + corr = (4*qsumg + 2*qn + qnx + qny); + /* create a tendency to prefer those matches + with brighter targets */ + corr *= ((double) (sumg + pix[p2].sumg)); + + if (qn>=cn && qnx>=cnx && qny>=cny && qsumg>csumg) { + cand[*count].pnr = p2; + cand[*count].tol = d; + cand[*count].corr = corr; + (*count)++; + if (nr > 0) { + sprintf (buf, "%3.0f/%3.1f + ", corr, d*1000); + strcat (str, buf); puts (str); + } + } + } + } + } + } + if (*count == 0 && nr > 0) puts ("- - -"); + } + else *count = -1; /* out of sensor area */ +} +#endif +void find_candidate_plus (coord_2d crd[], target pix[], int num, + double xa, double ya, double xb, double yb, + double eps, int n, int nx, int ny, int sumg, + candidate cand[], int *count, int nr, const char** argv) - m = (yb-ya)/(xb-xa); b = ya - m*xa; /* line equation: y = m*x + b */ + // binarized search in a x-sorted coord-set, exploits shape information + // int nr image number for ap etc. +{ + register int j; + int dummy, j0, dj, p2; + double m, b, d, temp, qn, qnx, qny, qsumg, corr; + double xmin, xmax, ymin, ymax,particle_size; + int dumbbell=0; + double tol_band_width; + + //Beat Mai 2010 for dumbbell + if (atoi(argv[1])==3) + dumbbell=1; + + if (dumbbell==0) { + ///// here is new Beat version of April 2010 + if (nx>ny) particle_size = nx; + else particle_size = ny; + tol_band_width = eps*0.5*(pix_x+pix_y)*particle_size; + } + else + tol_band_width=eps; + + if (tol_band_width < 0.06) + tol_band_width = 0.06; + + /* define sensor format for search interrupt */ + xmin = -pix_x * imx/2; xmax = pix_x * imx/2; + ymin = -pix_y * imy/2; ymax = pix_y * imy/2; + xmin -= I[nr].xh; ymin -= I[nr].yh; + xmax -= I[nr].xh; ymax -= I[nr].yh; + correct_brown_affin (xmin,ymin, ap[nr], &xmin,&ymin); + correct_brown_affin (xmax,ymax, ap[nr], &xmax,&ymax); + + for (j=0; j<4; j++) { /* initialize, why 8 in the prev version, adh ????*/ + cand[j].pnr = -999; + cand[j].tol = -999; + cand[j].corr = -999; + } + /* line equation: y = m*x + b */ + if (xa == xb) xa += 1e-10; + m = (yb-ya)/(xb-xa); b = ya - m*xa; - if (xa > xb) /* sort search window */ - { - temp = xa; xa = xb; xb = temp; - } - if (ya > yb) - { - temp = ya; ya = yb; yb = temp; - } + if (xa > xb) { temp = xa; xa = xb; xb = temp; } + if (ya > yb) { temp = ya; ya = yb; yb = temp; } + if ((xb>xmin) && (xaymin) && (ya1; dj/=2) { + if (crd[j0].x < (xa - tol_band_width)) j0 += dj; + else j0 -= dj; + } + j0 -= 12; if (j0 < 0) j0 = 0; /* due to trunc */ - if ( (xb>xmin) && (xaymin) && (ya xb+tol_band_width) return; /* finish search */ - /* binarized search for start point of candidate search */ - for (j0=num/2, dj=num/4; dj>1; dj/=2) - { - if (crd[j0].x < (xa - eps)) j0 += dj; - else j0 -= dj; - } - j0 -= 12; if (j0 < 0) j0 = 0; /* due to truncation */ - - - for (j=j0, *count=0; j xb+eps) return; /* stop search */ - - if ((crd[j].y > ya-eps) && (crd[j].y < yb+eps)) - { - if ((crd[j].x > xa-eps) && (crd[j].x < xb+eps)) - { - d = fabs ((crd[j].y - m*crd[j].x - b) / sqrt(m*m+1)); - if (d < eps) - { - p2 = crd[j].pnr; - if (n < pix[p2].n) qn = (double) n/pix[p2].n; - else qn = (double) pix[p2].n/n; - if (nx < pix[p2].nx) qnx = (double) nx/pix[p2].nx; - else qnx = (double) pix[p2].nx/nx; - if (ny < pix[p2].ny) qny = (double) ny/pix[p2].ny; - else qny = (double) pix[p2].ny/ny; - if (sumg < pix[p2].sumg) - qsumg = (double) sumg/pix[p2].sumg; - else qsumg = (double) pix[p2].sumg/sumg; - - /* empirical correlation coefficient - from shape and brightness parameters */ - corr = (4*qsumg + 2*qn + qnx + qny); - /* create a tendency to prefer those matches - with brighter targets */ - corr *= ((double) (sumg + pix[p2].sumg)); - - if (qn>=cn && qnx>=cnx && qny>=cny && qsumg>csumg) + // ad holten, 12-2012 : merged the two if's + // if ((crd[j].y > ya-tol_band_width) && (crd[j].y < yb+tol_band_width)) { + // if ((crd[j].x > xa-tol_band_width) && (crd[j].x < xb+tol_band_width)) { + if (crd[j].y > ya-tol_band_width && crd[j].y < yb+tol_band_width && + crd[j].x > xa-tol_band_width && crd[j].x < xb+tol_band_width) { - cand[*count].pnr = p2; - cand[*count].tol = d; - cand[*count].corr = corr; - (*count)++; - if (nr > 0) - { - sprintf (buf, "%3.0f/%3.1f + ", corr, d*1000); - strcat (str, buf); puts (str); - } + d = fabs ((crd[j].y - m*crd[j].x - b) / sqrt(m*m+1)); + + // Beat: modified in April 2010 to allow for better treatment of + // different sized traced particles, in particular colloids and tracers + // old : if (d < eps) { + // new : // if (nx>ny) particle_size=nx; + // // else particle_size=ny; + // if (d < tol_band_width) { + + if (d < tol_band_width) { + p2 = crd[j].pnr; + if (n < pix[p2].n) qn = (double) n/pix[p2].n; + else qn = (double) pix[p2].n/n; + if (nx < pix[p2].nx) qnx = (double) nx/pix[p2].nx; + else qnx = (double) pix[p2].nx/nx; + if (ny < pix[p2].ny) qny = (double) ny/pix[p2].ny; + else qny = (double) pix[p2].ny/ny; + if (sumg < pix[p2].sumg) qsumg = (double) sumg/pix[p2].sumg; + else qsumg = (double) pix[p2].sumg/sumg; + + // empirical correlation coefficient from shape and + // brightness parameters + corr = (4*qsumg + 2*qn + qnx + qny); + // create a tendency to prefer those matches + // with brighter targets + corr *= ((double) (sumg + pix[p2].sumg)); + + if (qn>=cn && qnx>=cnx && qny>=cny && qsumg>csumg) { + if (*count < maxcand) { + cand[*count].pnr = j; + cand[*count].tol = d; + cand[*count].corr = corr; + (*count)++; + } else { + dummy = (int)maxcand; + printf("in find_candidate_plus: count > maxcand\n"); + } + } + } } - } } - } } - if (*count == 0 && nr > 0) puts ("- - -"); - } - else *count = -1; /* out of sensor area */ + + else *count = -1; /* out of sensor area */ } - - - -void find_candidate_plus (crd, pix, num, xa,ya,xb,yb, eps, n, nx, ny, sumg, - cand, count, nr,argv) - -/* binarized search in a x-sorted coord-set, exploits shape information */ - -coord_2d crd[]; -target pix[]; -int num, *count; -double xa, ya, xb, yb, eps; -int n, nx, ny, sumg; -candidate cand[]; -int nr; /* image number for ap etc. */ -const char** argv; - - +void find_candidate_plus_msg (coord_2d crd[], target pix[], int num, + double xa, double ya, double xb, double yb, + double eps, int n, int nx, int ny, int sumg, + candidate cand[], int *count, int i12) + // binarized search in a x-sorted coord-set, exploits shape information */ + // gives messages (in examination) */ { - register int j; - int dummy; - int j0, dj, p2; - double m, b, d, temp, qn, qnx, qny, qsumg, corr; - double xmin, xmax, ymin, ymax,particle_size; - int dumbbell=0; - double tol_band_width; - - //Beat Mai 2010 for dumbbell - if (atoi(argv[1])==3){ - dumbbell=1; - } - if (dumbbell==0){ - /////here is new Beat version of April 2010 - if (nx>ny) particle_size=nx; - else particle_size=ny; - tol_band_width=eps*0.5*(pix_x+pix_y)*particle_size; - } - else{ - tol_band_width=eps; - } - if(tol_band_width<0.01){ //if(tol_band_width<0.06){ - tol_band_width=0.01; //tol_band_width=0.06; - } - - - /* define sensor format for search interrupt */ - xmin = (-1) * pix_x * imx/2; xmax = pix_x * imx/2; - ymin = (-1) * pix_y * imy/2; ymax = pix_y * imy/2; - xmin -= I[nr].xh; ymin -= I[nr].yh; - xmax -= I[nr].xh; ymax -= I[nr].yh; - correct_brown_affin (xmin,ymin, ap[nr], &xmin,&ymin); - correct_brown_affin (xmax,ymax, ap[nr], &xmax,&ymax); - - - for (j=0; j<4; j++) - { - cand[j].pnr = -999; cand[j].tol = -999; cand[j].corr = -999; - } - - - /* line equation: y = m*x + b */ - if (xa == xb) xa += 1e-10; - m = (yb-ya)/(xb-xa); b = ya - m*xa; - - if (xa > xb) - { - temp = xa; xa = xb; xb = temp; - } - if (ya > yb) - { - temp = ya; ya = yb; yb = temp; - } - - if ( (xb>xmin) && (xaymin) && (ya1; dj/=2) - { - if (crd[j0].x < (xa - tol_band_width)) j0 += dj; - else j0 -= dj; - } - j0 -= 12; if (j0 < 0) j0 = 0; /* due to trunc */ - - for (j=j0, *count=0; j xb+tol_band_width) return; /* finish search */ - - if ((crd[j].y > ya-tol_band_width) && (crd[j].y < yb+tol_band_width)) - { - if ((crd[j].x > xa-tol_band_width) && (crd[j].x < xb+tol_band_width)) - { - d = fabs ((crd[j].y - m*crd[j].x - b) / sqrt(m*m+1)); - - /* Beat: modified in April 2010 to allow for better treatment of - //different sized traced particles, in particular colloids and tracers - if ( d < eps ){ - */ - /////here is new Beat version of April 2010 - //if (nx>ny) particle_size=nx; - //else particle_size=ny; - if ( d < tol_band_width ){ - ///////end of new Beat version - - p2 = crd[j].pnr; - if (n < pix[p2].n) qn = (double) n/pix[p2].n; - else qn = (double) pix[p2].n/n; - if (nx < pix[p2].nx) qnx = (double) nx/pix[p2].nx; - else qnx = (double) pix[p2].nx/nx; - if (ny < pix[p2].ny) qny = (double) ny/pix[p2].ny; - else qny = (double) pix[p2].ny/ny; - if (sumg < pix[p2].sumg) - qsumg = (double) sumg/pix[p2].sumg; - else qsumg = (double) pix[p2].sumg/sumg; - - // empirical correlation coefficient - // from shape and brightness parameters - corr = (4*qsumg + 2*qn + qnx + qny); - // create a tendency to prefer those matches - // with brighter targets - corr *= ((double) (sumg + pix[p2].sumg)); - - if (qn>=cn && qnx>=cnx && qny>=cny && qsumg>csumg){ - if ( *count < maxcand) { - cand[*count].pnr = j; - cand[*count].tol = d; - cand[*count].corr = corr; - (*count)++; - } else { - dummy=(int)maxcand; - printf("in find_candidate_plus: count > maxcand\n");} - } - } - } - - - } + register int j; + int j0, dj, p2; + double m, b, d, temp, qn, qnx, qny, qsumg, corr; + double xmin, xmax, ymin, ymax, tol_band_width, particle_size; + + /* define sensor format for search interrupt */ + xmin = -pix_x * imx/2; xmax = pix_x * imx/2; + ymin = -pix_y * imy/2; ymax = pix_y * imy/2; + xmin -= I[i12].xh; ymin -= I[i12].yh; + xmax -= I[i12].xh; ymax -= I[i12].yh; + correct_brown_affin (xmin,ymin, ap[i12], &xmin,&ymin); + correct_brown_affin (xmax,ymax, ap[i12], &xmax,&ymax); + + if (nx>ny) particle_size=nx; + else particle_size=ny; + tol_band_width = eps*0.5*(pix_x+pix_y)*particle_size; + + for (j=0; j<4; j++) { + cand[j].pnr = -999; + cand[j].tol = 999; } - } - - else *count = -1; /* out of sensor area */ -} - - + m = (yb-ya)/(xb-xa); b = ya - m*xa; /* line equation: y = m*x + b */ + if (xa > xb) { temp = xa; xa = xb; xb = temp; } + if (ya > yb) { temp = ya; ya = yb; yb = temp; } -void find_candidate_plus_msg (crd, pix, num, xa,ya,xb,yb,eps, n, nx, ny, sumg, - cand, count, i12) - -/* binarized search in a x-sorted coord-set, exploits shape information */ -/* gives messages (in examination) */ + if ( (xb>xmin) && (xaymin) && (ya1; dj/=2) { + if (crd[j0].x < (xa - tol_band_width)) j0 += dj; + else j0 -= dj; + } + j0 -= 12; if (j0 < 0) j0 = 0; /* due to trunc */ -coord_2d crd[]; -target pix[]; -int num, *count, i12; -double xa, ya, xb, yb, eps; -int n, nx, ny, sumg; -/* -candidate cand[3]; -*/ -candidate cand[]; + for (j=j0, *count=0; j xb+tol_band_width) return; /* finish search */ -{ - register int j; - int j0, dj, p2; - double m, b, d, temp, qn, qnx, qny, qsumg, corr; - double xmin, xmax, ymin, ymax; - double tol_band_width,particle_size; - - /* define sensor format for search interrupt */ - xmin = (-1) * pix_x * imx/2; xmax = pix_x * imx/2; - ymin = (-1) * pix_y * imy/2; ymax = pix_y * imy/2; - xmin -= I[i12].xh; ymin -= I[i12].yh; - xmax -= I[i12].xh; ymax -= I[i12].yh; - correct_brown_affin (xmin,ymin, ap[i12], &xmin,&ymin); - correct_brown_affin (xmax,ymax, ap[i12], &xmax,&ymax); - - if (nx>ny) particle_size=nx; - else particle_size=ny; - tol_band_width=eps*0.5*(pix_x+pix_y)*particle_size; - - for (j=0; j<4; j++) - { - cand[j].pnr = -999; cand[j].tol = 999; - } - m = (yb-ya)/(xb-xa); b = ya - m*xa; /* line equation: y = m*x + b */ - - if (xa > xb) - { - temp = xa; xa = xb; xb = temp; - } - if (ya > yb) - { - temp = ya; ya = yb; yb = temp; - } - - if ( (xb>xmin) && (xaymin) && (ya1; dj/=2) - { - if (crd[j0].x < (xa - tol_band_width)) j0 += dj; - else j0 -= dj; - } - j0 -= 12; if (j0 < 0) j0 = 0; /* due to trunc */ - - for (j=j0, *count=0; j xb+tol_band_width) return; /* finish search */ - - if ((crd[j].y > ya-tol_band_width) && (crd[j].y < yb+tol_band_width)) - { - if ((crd[j].x > xa-tol_band_width) && (crd[j].x < xb+tol_band_width)) - { - d = fabs ((crd[j].y - m*crd[j].x - b) / sqrt(m*m+1)); - if ( d < tol_band_width ){ - p2 = crd[j].pnr; - if (n < pix[p2].n) qn = (double) n/pix[p2].n; - else qn = (double) pix[p2].n/n; - if (nx < pix[p2].nx) qnx = (double) nx/pix[p2].nx; - else qnx = (double) pix[p2].nx/nx; - if (ny < pix[p2].ny) qny = (double) ny/pix[p2].ny; - else qny = (double) pix[p2].ny/ny; - if (sumg < pix[p2].sumg) - qsumg = (double) sumg/pix[p2].sumg; - else qsumg = (double) pix[p2].sumg/sumg; - - - /* empirical correlation coefficient - from shape and brightness parameters */ - corr = (4*qsumg + 2*qn + qnx + qny); - /* create a tendency to prefer those matches - with brighter targets */ - corr *= ((double) (sumg + pix[p2].sumg)); - - if (qn>=cn && qnx>=cnx && qny>=cny && qsumg>csumg) + // ad holten, 12-2012 : merged two if's to a single one + if (crd[j].y > ya-tol_band_width && crd[j].y < yb+tol_band_width && + crd[j].x > xa-tol_band_width && crd[j].x < xb+tol_band_width) { - if (*count>=maxcand) - { printf("More candidates than (maxcand): %d\n",*count); return; } - cand[*count].pnr = p2; - cand[*count].tol = d; - cand[*count].corr = corr; - (*count)++; - printf ("%d %3.0f/%3.1f \n", p2, corr, d*1000); + d = fabs ((crd[j].y - m*crd[j].x - b) / sqrt(m*m+1)); + if ( d < tol_band_width ){ + p2 = crd[j].pnr; + if (n < pix[p2].n) qn = (double) n/pix[p2].n; + else qn = (double) pix[p2].n/n; + if (nx < pix[p2].nx) qnx = (double) nx/pix[p2].nx; + else qnx = (double) pix[p2].nx/nx; + if (ny < pix[p2].ny) qny = (double) ny/pix[p2].ny; + else qny = (double) pix[p2].ny/ny; + if (sumg < pix[p2].sumg) qsumg = (double) sumg/pix[p2].sumg; + else qsumg = (double) pix[p2].sumg/sumg; + + // empirical correlation coefficient from shape and + // brightness parameters + corr = (4*qsumg + 2*qn + qnx + qny); + // create a tendency to prefer those matches + // with brighter targets + corr *= ((double) (sumg + pix[p2].sumg)); + + if (qn>=cn && qnx>=cnx && qny>=cny && qsumg>csumg) { + if (*count >= maxcand) { + printf("More candidates than (maxcand): %d\n",*count); + return; + } + cand[*count].pnr = p2; + cand[*count].tol = d; + cand[*count].corr = corr; + (*count)++; + printf ("%d %3.0f/%3.1f \n", p2, corr, d*1000); + } + } } - } } - } + if (*count == 0) puts ("- - -"); } - if (*count == 0) puts ("- - -"); - } - else *count = -1; /* out of sensor area */ - + else *count = -1; /* out of sensor area */ } - - - - -void crossprod (a,b,c) - -double a[3], b[3], c[3]; - +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +void crossprod (double a[3], double b[3], double c[3]) { - c[0] = a[1] * b[2] - a[2] * b[1]; - c[1] = a[2] * b[0] - a[0] * b[2]; - c[2] = a[0] * b[1] - a[1] * b[0]; + c[0] = a[1] * b[2] - a[2] * b[1]; + c[1] = a[2] * b[0] - a[0] * b[2]; + c[2] = a[0] * b[1] - a[1] * b[0]; } +#endif \ No newline at end of file diff --git a/PTV/src_c/globals.h b/PTV/src_c/globals.h index c6ce080..4023aea 100644 Binary files a/PTV/src_c/globals.h and b/PTV/src_c/globals.h differ diff --git a/PTV/src_c/globals.h original b/PTV/src_c/globals.h original new file mode 100644 index 0000000..c6ce080 --- /dev/null +++ b/PTV/src_c/globals.h original @@ -0,0 +1,256 @@ +/* global declarations for ptv */ + +#define nmax 20240 + +extern int n_img; /* no of images */ +extern int hp_flag; /* flag for highpass */ +extern int allCam_flag; /* flag for using all cams for points */ +extern int tiff_flag; /* flag for tiff header */ +extern int chfield; /* flag for field mode */ +extern int nfix; /* no. of control points */ +extern int num[]; /* no. of particles per image */ +extern int nump[4]; /* no. of particles in previous image */ +extern int numc[4]; /* no. of particles in current image */ +extern int numn[4]; /* no. of particles in next image */ +extern int n_trac[]; /* no. of tracks */ +extern int match; /* no. of matches */ +extern int match2; /* no. of matches in 2nd pass */ +extern int corp, corc, corn; /* no. of correspondences in p,c,n */ + +extern int nr[4][4]; /* point numbers for man. ori */ +extern int imx, imy, imgsize; /* image size */ +extern int zoom_x[], zoom_y[], zoom_f[]; /* zoom parameters */ +extern int pp1, pp2, pp3, pp4,pp5; /* for man. orientation */ +extern int seq_first, seq_last; /* 1. and last img of seq */ +extern int demo_nr; /* for demo purposes */ +extern int examine; /* extra output */ +extern int dump_for_rdb; /* # of dumpfiles for rdb */ +extern int cr_sz; /* size of crosses */ +extern int display; /* display flag */ +extern int m[4]; +extern int trackallocflag; /* checkflag if mega, c4, t4 already allocated */ + +extern double pix_x, pix_y; /* pixel size */ +extern double pi, ro; /* pi, ro */ +extern double cn, cnx, cny, csumg, eps0, corrmin; /* correspond. par */ +extern double rmsX, rmsY, rmsZ, mean_sigma0; /* a priori rms */ +extern double X_lay[], Zmin_lay[], Zmax_lay[]; /* illu. layer current slice */ +extern double db_scale; /*dumbbell length, Beat Mai 2010*/ + +extern FILE *fp1, *fp2, *fp3, *fp4, *fpp; /* file pointers */ + +extern char img_name[4][256]; /* original image names */ +extern char img_lp_name[4][256]; /* lowpass image names */ +extern char img_hp_name[4][256]; /* highpass image names */ +extern char img_cal[4][128]; /* calibration image names */ +extern char img_ori[4][128]; /* image orientation data */ +extern char img_ori0[4][128]; /* orientation approx. values */ +extern char img_addpar0[4][128]; /* approx. image additional parameters */ +extern char img_addpar[4][128]; /* image additional parameters */ +extern char fixp_name[128]; /* testfield fixpoints */ +extern char seq_name[4][128]; /* sequence names */ +extern char track_dir[128]; /* directory with dap track data */ +extern char res_name[128]; /* result destination */ +extern char buf[], val[]; /* buffer */ +extern char filename[128]; + +extern unsigned char *img[]; /* image data */ +extern unsigned char *zoomimg; /* zomm image data */ + +extern Exterior Ex[]; /* exterior orientation */ +extern Interior I[]; /* interior orientation */ +extern Glass G[]; /* glass orientation */ +extern ap_52 ap[]; /* add. parameters */ +extern mm_np mmp; /* 3-media parameters */ +extern target pix[4][nmax]; /* target pixel data */ +extern target pix0[4][12]; /* pixel data for man_ori points */ +extern coord_2d crd[4][nmax]; /* (distorted) metric coordinates */ +extern coord_2d geo[4][nmax]; /* corrected metric coordinates */ +extern coord_3d fix[]; /* testfield points coordinates */ +extern n_tupel con[]; /* list of correspondences */ +extern mm_LUT mmLUT[4]; /* LUT for mm radial displacement */ +extern coord_3d *p_c3d; +extern target *p[4]; +extern target *c[4]; +extern target *n[4]; +extern target *t4[4][4]; +extern int nt4[4][4]; +extern corres *corrp; +extern corres *corrc; +extern corres *corrn; +extern corres *c4[4]; +extern trackparameters tpar; + +extern P *mega[4]; + +extern FILE *fopen_r (); +extern double multimed_r (); + + +int jw_Init(); +int init_proc_c(); +int start_proc_c(); +int pre_processing_c (); +int detection_proc_c(); +int correspondences_proc_c (); +int determination_proc_c (); +int sequence_proc_c(); +void checkpoint_proc (); +int quit_proc_c (); +int parameter_panel_init(); +int done_proc_c(); + +void read_ori(); +void write_ori (); +void compose_name_plus_nr (); +int mouse_proc_c (); +void read_image(); +int read_tiff (); +int write_tiff (); +void copy_images(); +void subtract_mask(); + +int peak_fit_new(); +void highpass(); +void zoom (); +void lowpass_n (); +void lowpass_3 (); +void filter_3 (); +void unsharp_mask (); +void split (); +void multimed_nlay (); +void multimed_nlay_v2 (); +void trans_Cam_Point (); +void back_trans_Point (); +void getG(); + +void quicksort_con (); +int flow_demo_c(); +void simple_connectivity(); +void targ_rec(); +void quicksort_target_y(); +void transp(); +void quicksort_coord2d_x(); +void rotation_matrix(); +void img_xy (); +void img_xy_mm (); +void img_xy_mm_geo (); +void pixel_to_metric(); +void metric_to_pixel (); +void correct_brown_affin(); +void distort_brown_affin (); +int pix_in_next(); +int kill_in_list (); +void compose_name_plus_nr (); +void compose_name_plus_nr_str (); +int nearest_neighbour_geo (); +int nearest_neighbour_pix (); +void bubble_y (); +void init_mmLUT (); + + +void correspondences_4(); +void det_lsq(); +void pos_from_ray(); +void dist_to_ray(); +void ray_tracing(); +void point_line_line(); +void norm_cross(); +void dot(); +void modu(); +void ray_tracing_v2(); +void mid_point(); +void cross(); +void dotP(); +void intersect_rt_3m(); +void intersect_rt(); +int nearest_neighbour_pix(); +void img_coord(); +void img_coord_old(); +void raw_orient(); +void raw_orient_v3(); +void raw_orient_v4(); +void raw_orient_v5(); +int mod(); +void getabcFromRot(); +void sortgrid_man(); +void sortgrid_file(); +void just_plot(); +void det_lsq_3d (); +void det_lsq_2 (); +void det_lsq_3 (); +void det_lsq_4 (); +void orient(); +void orient_v3(); +void orient_v6(); +void orient_v4(); +void orient_v5(); +void prepare_eval(); +void eval_ori(); +void eval_ori_v2(); +void volumedimension(); + +double epi_line (); +int epi_mm (); +int epi_mm_2D (); +void find_candidate_plus_msg (); +void find_candidate_plus (); + +int drawcross(); +int drawvector (); +int draw_pnr (); +int draw_value (); +void mark_detections (); +void mark_correspondences (); +void mark_corr (); + +void ata (); +void ata_v2 (); +void matinv (); +void atl (); +void matmul (); +void mat_transpose (); + +void tracking_proc (); +int find_track (); + +int candsearch_in_pix (); +int candsearch_in_pixrest (); +int corrtest(); +void predict(); +void readseqtrackcrit (); +void searchquader(); +void angle_acc(); +void sortwhatfound(); +void bubble_foundpix1 (); +void bubble_foundpix2 (); +void tclimg2cimg(); + +void Pix_crd (); +void Back_trafo (); +void crd_pix(); +int vrmltracks_c(); +int trajectories_c(); +int vrmldetections_c(); +int vrmldettracks_c(); + + +int tracking(); +void read_parameter(); +void level1(); +void level2(); +void level3(); +void read_ascii_data(); +void rotate_dataset(); +void write_ascii_data(); +void readcoord_parameter(); +void change_coordinates(); +void sort(); +void neighbours(); +int seq_track_proc_c(); + +void read_ascii_datanew(); +void write_ascii_datanew(); +void read_targets(); + + diff --git a/PTV/src_c/image_processing.c b/PTV/src_c/image_processing.c index 36a2c3b..c2beff2 100644 --- a/PTV/src_c/image_processing.c +++ b/PTV/src_c/image_processing.c @@ -1,145 +1,116 @@ /**************************************************************************** +Routine: image_processing.c -Routine: image_processing.c +Author/Copyright: Hans-Gerd Maas -Author/Copyright: Hans-Gerd Maas +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich - -Creation Date: 1988 +Creation Date: 1988 -Description: different image processing routines ... +Description: different image processing routines ... -Routines contained: filter_3: 3*3 filter, reads matrix from filter.par - lowpass_3: 3*3 local average with 9 pointers, fast - lowpass_n: n*n local average,, fast - computation time independent from n - histogram: computes histogram - enhance: enhances gray value spectrum to 0..255, - some extreme gray values are cut off - mark_img: reads image and pixel coordinate set, - marks the image in a certain (monocomp) - color and writes image - +Routines contained: filter_3: 3*3 filter, reads matrix from filter.par + lowpass_3: 3*3 local average with 9 pointers, fast + lowpass_n: n*n local average, fast computation time + independent from n + histogram: computes histogram + enhance: enhances gray value spectrum to 0..255, + some extreme gray values are cut off + mark_img: reads image and pixel coordinate set, + marks the image in a certain (monocomp) + color and writes image ****************************************************************************/ - #include "ptv.h" -void filter_3 (img, img_lp) - -unsigned char *img, *img_lp; - +void filter_3(unsigned char *img, unsigned char *img_lp) { - register unsigned char *ptr, *ptr1, *ptr2, *ptr3, - *ptr4, *ptr5, *ptr6, - *ptr7, *ptr8, *ptr9; - int end; - float m[9], sum; - short buf; - register int i; - FILE *fp; - + register unsigned char *ptr, *ptr1, *ptr2, *ptr3, *ptr4, *ptr5, + *ptr6, *ptr7, *ptr8, *ptr9; + int end; + float m[9], sum, val; + short buf; + FILE *fp; + register int i; /* read filter elements from parameter file */ - fp = fopen_r ("filter.par"); - for (i=0, sum=0; i<9; i++) - { - fscanf (fp, "%f", &m[i]); - sum += m[i]; + fp = fopen_rp("filter.par"); // replaced fopen_r, ad holten 12-2012 + sum = 0; + if (fp) { + for (i=0, sum=0; i<9; i++) { + fscanf (fp, "%f", &m[i]); + sum += m[i]; + } + fclose (fp); } - fclose (fp); if (sum == 0) exit(1); + if (sum == 0) exit(1); end = imgsize - 513; ptr = img_lp + 513; - ptr1 = img; ptr2 = img + 1; ptr3 = img + 2; - ptr4 = img + imx; ptr5 = img + imx + 1; ptr6 = img + imx + 2; - ptr7 = img + 2*imx; ptr8 = img + 2*imx + 1; ptr9 = img + 2*imx + 2; - - for (i=513; i 255) buf = 255; if (buf < 8) buf = 8; - *ptr++ = buf; + ptr1 = img; ptr2 = img + 1; ptr3 = img + 2; + ptr4 = img + imx; ptr5 = img + imx + 1; ptr6 = img + imx + 2; + ptr7 = img + 2*imx; ptr8 = img + 2*imx + 1; ptr9 = img + 2*imx + 2; + + for (i=513; i 255) buf = 255; + if (buf < 8) buf = 8; + *ptr++ = (unsigned char) buf; } } - - - - -void enhance (img) - -unsigned char *img; - +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +void enhance(unsigned char *img) { - register unsigned char *ptr; - unsigned char *end, gmin = 255, gmax = 0, offs; - float diff, gain; - int i, sum, histo[256]; + register unsigned char *ptr; + unsigned char *end, gmin = 255, gmax = 0, offs; + float diff, gain; + int i, sum, histo[256]; - void histogram (); + void histogram(unsigned char *img, int *hist); end = img + imgsize; - histogram (img, histo); + histogram(img, histo); for (i=0, sum=0; (i<255)&&(sum0)&&(sum<512); sum+=histo[i], i--); gmax = i; - offs = gmin; diff = gmax - gmin; gain = 255 / diff; + offs = gmin; diff = (float)(gmax - gmin); gain = (float)255 / diff; for (ptr=img; ptr gmax) *ptr = gmax; - *ptr = (*ptr - offs) * gain; - if (*ptr < 8) *ptr = 8; /* due monocomp colors */ + *ptr = (unsigned char) ((*ptr - offs) * gain); + if (*ptr < 8) *ptr = 8; /* due monocomp colors */ } } +#endif - - - - - - -void histogram (img, hist) - -unsigned char *img; -int *hist; - +void histogram(unsigned char *img, int *hist) { - int i; - unsigned char *end; - register unsigned char *ptr; - + int i; + unsigned char *end; + register unsigned char *ptr; - for (i=0; i<256; i++) hist[i] = 0; + for (i=0; i<256; i++) hist[i] = 0; end = img + imgsize; for (ptr=img; ptr error"); exit (1); } buf2 = (short *) calloc (imx, sizeof(short)); - /* -------------- average over lines --------------- */ end = buf1 + imgsize; buf = 0; - for (ptrr = img; ptrr < img + n2; ptrr ++) buf += *ptrr; + for (ptrr = img; ptrr < img + n2; ptrr ++) buf += *ptrr; *(buf1 + n) = buf; - for (ptrl=img, ptr = buf1+n+1; ptr error"); exit (1); } buf1 = (int *) calloc (imgsize, sizeof(int)); - if ( ! buf1) - { + if ( ! buf1) { puts ("calloc for buf1 --> error"); exit (1); } buf2 = (int *) calloc (imx, sizeof(int)); - - - /* set imgum = img0 (so there cannot be written to the original image) */ - for (ptrl=imgum, ptrr=img0; ptrl<(imgum+imgsize); ptrl++, ptrr++) - { - *ptrl = *ptrr; - - - } + // for (ptrl=imgum, ptrr=img0; ptrl<(imgum+imgsize); ptrl++, ptrr++) + // *ptrl = *ptrr; + copy_images(img0, imgum); // ad holten, 12-2012 /* cut off high gray values (not in general use !) for (ptrz=imgum; ptrz<(imgum+imgsize); ptrz++) if (*ptrz > 160) *ptrz = 160; */ - - - /* -------------- average over lines --------------- */ - for (i=0; i0; i--) - { + for (i=n; i>0; i--) { ptr1 = buf1 + (imy-2*i-1)*imx; ptr2 = ptr1 + imx; ptrz = img_lp + (imy-i)*imx; - for (ptr3=buf2; ptr3 intersection -Routines contained: - +Routines contained: - ****************************************************************************/ #include "ptv.h" - /*-------------------------------------------------------------------------*/ /* 2 cameras */ - - -void intersect_rt (X1,Y1,Z1,a1,b1,c1,X2,Y2,Z2,a2,b2,c2,X,Y,Z) - -double X1, Y1, Z1, a1, b1, c1, X2, Y2, Z2, a2, b2, c2, *X, *Y, *Z; - +void intersect_rt (double X1, double Y1, double Z1, double a1, double b1, double c1, + double X2, double Y2, double Z2, double a2, double b2, double c2, + double *X, double *Y, double *Z) /* intersection, given two points with direction cosines */ /* only valid, if Z1 = Z2 = 0 , which is the case after ray tracing */ - { - if (fabs(b1-b2) > fabs(a1-a2)) *Z = (Y2-Y1) / ((b1/c1) - (b2/c2)); - else *Z = (X2-X1) / ((a1/c1) - (a2/c2)); + // ad holten, test first if lines are parallel + if (a1/c1 == a2/c2 && b1/c1 == b2/c2) { // if parallel, return high values + *X = *Y = *Z = 1e6; // ie, out of the measuring volume + return; + } + + if (fabs(b1-b2) > fabs(a1-a2)) *Z = (Y2-Y1) / ((b1/c1) - (b2/c2)); + else *Z = (X2-X1) / ((a1/c1) - (a2/c2)); - *X = ( X1 + X2 + *Z * (a1/c1 + a2/c2)) /2; - *Y = ( Y1 + Y2 + *Z * (b1/c1 + b2/c2)) /2; + *X = (X1 + X2 + *Z * (a1/c1 + a2/c2)) /2; + *Y = (Y1 + Y2 + *Z * (b1/c1 + b2/c2)) /2; } - - - -void intersect (X1,Y1,Z1,a1,b1,c1,X2,Y2,Z2,a2,b2,c2,X,Y,Z) - -double X1, Y1, Z1, a1, b1, c1, X2, Y2, Z2, a2, b2, c2, *X, *Y, *Z; - +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +void intersect (double X1, double Y1, double Z1, double a1, double b1, double c1, + double X2, double Y2, double Z2, double a2, double b2, double c2, + double *X, double *Y, double *Z) /* intersection, given two points with direction cosines */ - { if (fabs(b1-b2) > fabs(a1-a2)) *Z = (Y2 - Y1 + Z1*(b1/c1) - Z2*(b2/c2)) / ((b1/c1) - (b2/c2)); else *Z = (X2 - X1 + Z1*(a1/c1) - Z2*(a2/c2)) / ((a1/c1) - (a2/c2)); - *X = ( X1 + X2 + *Z * (a1/c1 + a2/c2)) / 2; - *Y = ( Y1 + Y2 + *Z * (b1/c1 + b2/c2)) / 2; + *X = ( X1 + X2 + *Z * (a1/c1 + a2/c2)) / 2; + *Y = ( Y1 + Y2 + *Z * (b1/c1 + b2/c2)) / 2; } - - - - +#endif /*-------------------------------------------------------------------------*/ /* 3 cameras */ - - - - -void -intersect_rt_3 (X1,Y1,Z1,a1,b1,c1,X2,Y2,Z2,a2,b2,c2,X3,Y3,Z3,a3,b3,c3, X,Y,Z) - -double X1,Y1,Z1,a1,b1,c1, X2,Y2,Z2,a2,b2,c2, X3,Y3,Z3,a3,b3,c3, *X,*Y,*Z; - +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +void intersect_rt_3 (double X1, double Y1, double Z1, double a1, double b1, double c1, + double X2, double Y2, double Z2, double a2, double b2, double c2, + double X3, double Y3, double Z3, double a3, double b3, double c3, + double *X, double *Y, double *Z) /* intersection, given three points with direction cosines */ - { - int i, n_max; - double base[6], max_base=0; + int i, n_max; + double base[6], max_base=0; /* find the maximum base component to decide, wether to take X or Y for determination of Z */ - /* should be taken out as constant */ + /* should be taken out as constant */ base[0] = fabs (X1 + X2 - 2*X3); base[1] = fabs (X1 + X3 - 2*X2); base[2] = fabs (X2 + X3 - 2*X1); @@ -92,50 +81,46 @@ double X1,Y1,Z1,a1,b1,c1, X2,Y2,Z2,a2,b2,c2, X3,Y3,Z3,a3,b3,c3, *X,*Y,*Z; base[4] = fabs (Y1 + Y3 - 2*Y2); base[5] = fabs (Y2 + Y3 - 2*Y1); - for (i=0; i<6; i++) if (base[i] > max_base) + for (i=0; i<6; i++) + if (base[i] > max_base) { max_base = base[i]; n_max = i; } - switch (n_max) - { - case 0: *Z = (2*X3 - X1 - X2) / ((a1/c1) + (a2/c2) - (2*a3/c3)); + switch (n_max) { + case 0: *Z = (2*X3 - X1 - X2) / ((a1/c1) + (a2/c2) - (2*a3/c3)); break; - case 1: *Z = (2*X2 - X1 - X3) / ((a1/c1) + (a3/c3) - (2*a2/c2)); + case 1: *Z = (2*X2 - X1 - X3) / ((a1/c1) + (a3/c3) - (2*a2/c2)); break; - case 2: *Z = (2*X1 - X2 - X3) / ((a2/c2) + (a3/c3) - (2*a1/c1)); + case 2: *Z = (2*X1 - X2 - X3) / ((a2/c2) + (a3/c3) - (2*a1/c1)); break; - case 3: *Z = (2*Y3 - Y1 - Y2) / ((b1/c1) + (b2/c2) - (2*b3/c3)); + case 3: *Z = (2*Y3 - Y1 - Y2) / ((b1/c1) + (b2/c2) - (2*b3/c3)); break; - case 4: *Z = (2*Y2 - Y1 - Y3) / ((b1/c1) + (b3/c3) - (2*b2/c2)); + case 4: *Z = (2*Y2 - Y1 - Y3) / ((b1/c1) + (b3/c3) - (2*b2/c2)); break; - case 5: *Z = (2*Y1 - Y2 - Y3) / ((b2/c2) + (b3/c3) - (2*b1/c1)); + case 5: *Z = (2*Y1 - Y2 - Y3) / ((b2/c2) + (b3/c3) - (2*b1/c1)); break; } *X = (X1+(*Z)*(a1/c1) + X2+(*Z)*(a2/c2) + X3+(*Z)*(a3/c3)) / 3; *Y = (Y1+(*Z)*(b1/c1) + Y2+(*Z)*(b2/c2) + Y3+(*Z)*(b3/c3)) / 3; } +#endif - - - - -void -intersect_rt_3m (X1,Y1,Z1,a1,b1,c1,X2,Y2,Z2,a2,b2,c2,X3,Y3,Z3,a3,b3,c3, X,Y,Z) - -double X1,Y1,Z1,a1,b1,c1, X2,Y2,Z2,a2,b2,c2, X3,Y3,Z3,a3,b3,c3, *X,*Y,*Z; - +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +void intersect_rt_3m(double X1, double Y1, double Z1, double a1, double b1, double c1, + double X2, double Y2, double Z2, double a2, double b2, double c2, + double X3, double Y3, double Z3, double a3, double b3, double c3, + double *X, double *Y, double *Z) /* intersection, given three points with direction cosines */ - { - int i, n_max1=0, n_max2; - double base[6], max_base1=0, max_base2=0, Za, Zb; + int i, n_max1=0, n_max2; + double base[6], max_base1=0, max_base2=0, Za, Zb; /* find the maximum base component to decide, wether to take X or Y for determination of Z */ - /* should be taken out as constant */ + /* should be taken out as constant */ base[0] = fabs (X1 + X2 - 2*X3); base[1] = fabs (X1 + X3 - 2*X2); base[2] = fabs (X2 + X3 - 2*X1); @@ -143,95 +128,91 @@ double X1,Y1,Z1,a1,b1,c1, X2,Y2,Z2,a2,b2,c2, X3,Y3,Z3,a3,b3,c3, *X,*Y,*Z; base[4] = fabs (Y1 + Y3 - 2*Y2); base[5] = fabs (Y2 + Y3 - 2*Y1); - for (i=0; i<6; i++) - { - if (base[i] > max_base1) - { - max_base2 = max_base1; max_base1 = base[i]; - n_max2 = n_max1; n_max1 = i; + for (i=0; i<6; i++) { + if (base[i] > max_base1) { + max_base2 = max_base1; + max_base1 = base[i]; + n_max2 = n_max1; + n_max1 = i; } - else if (base[i] > max_base2) - { - max_base2 = base[i]; n_max2 = i; + else if (base[i] > max_base2) { + max_base2 = base[i]; + n_max2 = i; } } /* compute *Z from the 2 longest base components, what was empirically found to be the most accurate value */ - switch (n_max1) - { - case 0: Za = (2*X3 - X1 - X2) / ((a1/c1) + (a2/c2) - (2*a3/c3)); + switch (n_max1) { + case 0: Za = (2*X3 - X1 - X2) / ((a1/c1) + (a2/c2) - (2*a3/c3)); break; - case 1: Za = (2*X2 - X1 - X3) / ((a1/c1) + (a3/c3) - (2*a2/c2)); + case 1: Za = (2*X2 - X1 - X3) / ((a1/c1) + (a3/c3) - (2*a2/c2)); break; - case 2: Za = (2*X1 - X2 - X3) / ((a2/c2) + (a3/c3) - (2*a1/c1)); + case 2: Za = (2*X1 - X2 - X3) / ((a2/c2) + (a3/c3) - (2*a1/c1)); break; - case 3: Za = (2*Y3 - Y1 - Y2) / ((b1/c1) + (b2/c2) - (2*b3/c3)); + case 3: Za = (2*Y3 - Y1 - Y2) / ((b1/c1) + (b2/c2) - (2*b3/c3)); break; - case 4: Za = (2*Y2 - Y1 - Y3) / ((b1/c1) + (b3/c3) - (2*b2/c2)); + case 4: Za = (2*Y2 - Y1 - Y3) / ((b1/c1) + (b3/c3) - (2*b2/c2)); break; - case 5: Za = (2*Y1 - Y2 - Y3) / ((b2/c2) + (b3/c3) - (2*b1/c1)); + case 5: Za = (2*Y1 - Y2 - Y3) / ((b2/c2) + (b3/c3) - (2*b1/c1)); break; } - switch (n_max2) - { - case 0: Zb = (2*X3 - X1 - X2) / ((a1/c1) + (a2/c2) - (2*a3/c3)); + switch (n_max2) { + case 0: Zb = (2*X3 - X1 - X2) / ((a1/c1) + (a2/c2) - (2*a3/c3)); break; - case 1: Zb = (2*X2 - X1 - X3) / ((a1/c1) + (a3/c3) - (2*a2/c2)); + case 1: Zb = (2*X2 - X1 - X3) / ((a1/c1) + (a3/c3) - (2*a2/c2)); break; - case 2: Zb = (2*X1 - X2 - X3) / ((a2/c2) + (a3/c3) - (2*a1/c1)); + case 2: Zb = (2*X1 - X2 - X3) / ((a2/c2) + (a3/c3) - (2*a1/c1)); break; - case 3: Zb = (2*Y3 - Y1 - Y2) / ((b1/c1) + (b2/c2) - (2*b3/c3)); + case 3: Zb = (2*Y3 - Y1 - Y2) / ((b1/c1) + (b2/c2) - (2*b3/c3)); break; - case 4: Zb = (2*Y2 - Y1 - Y3) / ((b1/c1) + (b3/c3) - (2*b2/c2)); + case 4: Zb = (2*Y2 - Y1 - Y3) / ((b1/c1) + (b3/c3) - (2*b2/c2)); break; - case 5: Zb = (2*Y1 - Y2 - Y3) / ((b2/c2) + (b3/c3) - (2*b1/c1)); + case 5: Zb = (2*Y1 - Y2 - Y3) / ((b2/c2) + (b3/c3) - (2*b1/c1)); break; } *Z = (Za + Zb) / 2; - *X = (X1+(*Z)*(a1/c1) + X2+(*Z)*(a2/c2) + X3+(*Z)*(a3/c3)) / 3; *Y = (Y1+(*Z)*(b1/c1) + Y2+(*Z)*(b2/c2) + Y3+(*Z)*(b3/c3)) / 3; } +#endif - - - -void intersect_3 (X1,Y1,Z1,a1,b1,c1,X2,Y2,Z2,a2,b2,c2,X3,Y3,Z3,a3,b3,c3, X,Y,Z) - -double X1,Y1,Z1,a1,b1,c1, X2,Y2,Z2,a2,b2,c2, X3,Y3,Z3,a3,b3,c3, *X,*Y,*Z; - +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +void intersect_3 (double X1, double Y1, double Z1, double a1, double b1, double c1, + double X2, double Y2, double Z2, double a2, double b2, double c2, + double X3, double Y3, double Z3, double a3, double b3, double c3, + double *X, double *Y, double *Z) /* intersection, given three points with direction cosines */ - { - int i, n_min; - double base[6], min_base=1e20; + int i, n_min; + double base[6], min_base=1e20; /* find the minimum base component to decide, wether to take X or Y for determination of Z */ base[0] = fabs (a1-a2); base[1] = fabs (a1-a3); base[2] = fabs (a2-a3); base[3] = fabs (b1-b2); base[4] = fabs (b1-b3); base[5] = fabs (b2-b3); - for (i=0; i<6; i++) if (base[i] < min_base) + for (i=0; i<6; i++) + if (base[i] < min_base) { min_base = base[i]; n_min = i; } - if (n_min > 2) /* min base in y-direction => take x */ + if (n_min > 2) /* min base in y-direction => take x */ *Z = (X2 + X3 - 2*X1 + 2*Z1*(a1/c1) - Z2*(a2/c2) - Z3*(a3/c3)) - / ((2*a1/c1) - (a2/c2) - (a3/c3)); + / ((2*a1/c1) - (a2/c2) - (a3/c3)); else /* min base in y-direction => take x */ *Z = (Y2 + Y3 - 2*Y1 + 2*Z1*(b1/c1) - Z2*(b2/c2) - Z3*(b3/c3)) - / ((2*b1/c1) - (b2/c2) - (b3/c3)); + / ((2*b1/c1) - (b2/c2) - (b3/c3)); *X = (X1+(*Z-Z1)*(a1/c1) + X2+(*Z-Z2)*(a2/c2) + X3+(*Z-Z3)*(a3/c3)) / 3; *Y = (Y1+(*Z-Z1)*(b1/c1) + Y2+(*Z-Z2)*(b2/c2) + Y3+(*Z-Z3)*(b3/c3)) / 3; } - +#endif diff --git a/PTV/src_c/jw_ImgFmtTIF.c b/PTV/src_c/jw_ImgFmtTIF.c index f45b57f..fd960f4 100644 --- a/PTV/src_c/jw_ImgFmtTIF.c +++ b/PTV/src_c/jw_ImgFmtTIF.c @@ -250,14 +250,14 @@ int FileReadTIF(Tcl_Interp *interp, return TCL_ERROR; } - if ((srcX + width) > fileImgWidth) { + if ((srcX + width) > (int)fileImgWidth) { width = fileImgWidth - srcX; } - if ((srcY + height) > fileImgHeight) { + if ((srcY + height) > (int)fileImgHeight) { height = fileImgHeight - srcY; } if ((width <= 0) || (height <= 0) - || (srcX >= fileImgWidth) || (srcY >= fileImgHeight)) { + || (srcX >= (int)fileImgWidth) || (srcY >= (int)fileImgHeight)) { return TCL_OK; } diff --git a/PTV/src_c/jw_main.c b/PTV/src_c/jw_main.c index 77db22d..981a561 100644 --- a/PTV/src_c/jw_main.c +++ b/PTV/src_c/jw_main.c @@ -1,9 +1,8 @@ /* * tkAppInit.c -- * - * Provides a default version of the Tcl_AppInit procedure for - * use in wish and similar Tk-based applications. - + * Provides a default version of the Tcl_AppInit procedure for + * use in wish and similar Tk-based applications. */ #include "ptv.h" @@ -17,35 +16,30 @@ extern int matherr(); int *tclDummyMathPtr = (int *) matherr; #ifdef TK_TEST -EXTERN int Tktest_Init _ANSI_ARGS_((Tcl_Interp *interp)); -#endif /* TK_TEST */ - - +EXTERN int Tktest_Init _ANSI_ARGS_((Tcl_Interp *interp)); +#endif /* *---------------------------------------------------------------------- * * main -- * - * This is the main program for the application. + * This is the main program for the application. * * Results: - * None: Tk_Main never returns here, so this procedure never - * returns either. + * None: Tk_Main never returns here, so this procedure never + * returns either. * * Side effects: - * Whatever the application does. + * Whatever the application does. * *---------------------------------------------------------------------- */ -int -main(argc, argv) - int argc; /* Number of command-line arguments. */ - char **argv; /* Values of command-line arguments. */ +int main(int argc, char **argv) { - Tk_Main(argc, argv, Tcl_AppInit); - return 0; /* Needed only to prevent compiler warning. */ + Tk_Main(argc, argv, Tcl_AppInit); + return 0; /* Needed only to prevent compiler warning. */ } /* @@ -53,137 +47,136 @@ main(argc, argv) * * Tcl_AppInit -- * - * This procedure performs application-specific initialization. - * Most applications, especially those that incorporate additional - * packages, will have their own version of this procedure. + * This procedure performs application-specific initialization. + * Most applications, especially those that incorporate additional + * packages, will have their own version of this procedure. * * Results: - * Returns a standard Tcl completion code, and leaves an error - * message in interp->result if an error occurs. + * Returns a standard Tcl completion code, and leaves an error + * message in interp->result if an error occurs. * * Side effects: - * Depends on the startup script. + * Depends on the startup script. * *---------------------------------------------------------------------- */ -int -Tcl_AppInit(interp) - Tcl_Interp *interp; /* Interpreter for application. */ +int Tcl_AppInit(Tcl_Interp *interp) +// Tcl_Interp *interp Interpreter for application. { - if (Tcl_Init(interp) == TCL_ERROR) { - return TCL_ERROR; - } - if (Tk_Init(interp) == TCL_ERROR) { - return TCL_ERROR; - } - Tcl_StaticPackage(interp, "Tk", Tk_Init, Tk_SafeInit); + if (Tcl_Init(interp) == TCL_ERROR) { + return TCL_ERROR; + } + if (Tk_Init(interp) == TCL_ERROR) { + return TCL_ERROR; + } + Tcl_StaticPackage(interp, "Tk", Tk_Init, Tk_SafeInit); + #ifdef TK_TEST - if (Tktest_Init(interp) == TCL_ERROR) { + if (Tktest_Init(interp) == TCL_ERROR) { return TCL_ERROR; - } - Tcl_StaticPackage(interp, "Tktest", Tktest_Init, - (Tcl_PackageInitProc *) NULL); + } + Tcl_StaticPackage(interp, "Tktest", Tktest_Init, + (Tcl_PackageInitProc *) NULL); #endif /* TK_TEST */ - /* - * Call the init procedures for included packages. Each call should - * look like this: - * - * if (Mod_Init(interp) == TCL_ERROR) { - * return TCL_ERROR; - * } - * - * where "Mod" is the name of the module. - */ - - if (jw_Init(interp) == TCL_ERROR) { - return TCL_ERROR; - } - - /* - * Call Tcl_CreateCommand for application-specific commands, if - * they weren't already created by the init procedures called above. - */ - Tk_CreatePhotoImageFormat(&tkImgFmtTIF); - - /* - * Specify a user-specific startup file to invoke if the application - * is run interactively. Typically the startup file is "~/.apprc" - * where "app" is the name of the application. If this line is deleted - * then no user-specific startup file will be run under any conditions. - */ - - Tcl_SetVar(interp, "tcl_rcFileName", "~/.jwshrc", TCL_GLOBAL_ONLY); - return TCL_OK; + /* + * Call the init procedures for included packages. Each call should + * look like this: + * + * if (Mod_Init(interp) == TCL_ERROR) { + * return TCL_ERROR; + * } + * + * where "Mod" is the name of the module. + */ + + if (jw_Init(interp) == TCL_ERROR) { + return TCL_ERROR; + } + + /* + * Call Tcl_CreateCommand for application-specific commands, if + * they weren't already created by the init procedures called above. + */ + Tk_CreatePhotoImageFormat(&tkImgFmtTIF); + + /* + * Specify a user-specific startup file to invoke if the application + * is run interactively. Typically the startup file is "~/.apprc" + * where "app" is the name of the application. If this line is deleted + * then no user-specific startup file will be run under any conditions. + */ + + Tcl_SetVar(interp, "tcl_rcFileName", "~/.jwshrc", TCL_GLOBAL_ONLY); + return TCL_OK; } -int jw_Init(interp) - Tcl_Interp *interp; +int jw_Init(Tcl_Interp *interp) { Tcl_CreateCommand(interp, "init_proc_cmd", init_proc_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "start_proc_cmd", start_proc_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "done_proc_cmd", done_proc_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "detection_proc_cmd", detection_proc_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "pre_processing_cmd", pre_processing_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "correspondences_cmd", correspondences_proc_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "determination_cmd", determination_proc_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "sequence_cmd", sequence_proc_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "quit_cmd", quit_proc_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "calib_cmd", calibration_proc_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "restore_cmd", restore_proc_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "mouse_cmd", mouse_proc_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "flow_cmd", flow_demo_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "marktrack_cmd", mark_track_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "trackcorr_cmd", trackcorr_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "trackback_cmd", trackback_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "trajectories_cmd", trajectories_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "VRMLtracks", vrmltracks_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "VRMLdetections", vrmldetections_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); Tcl_CreateCommand(interp, "VRMLdettracks", vrmldettracks_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); - Tcl_CreateCommand(interp, "ptv_cmd", seq_track_proc_c, - (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); + Tcl_CreateCommand(interp, "ptv_cmd", seq_track_proc_c, + (ClientData)NULL, (Tcl_CmdDeleteProc *)NULL); return TCL_OK; } diff --git a/PTV/src_c/jw_ptv.c b/PTV/src_c/jw_ptv.c index 6549a76..3d05242 100644 --- a/PTV/src_c/jw_ptv.c +++ b/PTV/src_c/jw_ptv.c @@ -1,19 +1,18 @@ /**************************************************************************** -***************************************************************************** -Author/Copyright: Hans-Gerd Maas / Jochen Willneff +Author/Copyright: Hans-Gerd Maas / Jochen Willneff -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: took a longer time ... +Creation Date: took a longer time ... -Description: target detection, correspondences and - positioning with tclTk display - -> 4 camera version +Description: target detection, correspondences and + positioning with tclTk display + -> 4 camera version -Routines contained: many ... +Routines contained: many ... ****************************************************************************/ @@ -28,1189 +27,1137 @@ See the file license.txt for copying permission. #define nmax 20240 -/* global declarations for ptv */ +/* global declarations for ptv */ /*-------------------------------------------------------------------------*/ -int n_img; /* no of images */ -int hp_flag=0; /* flag for highpass */ -int allCam_flag=0; /* flag for using all cams for points */ -int tiff_flag=0; /* flag for tiff header */ -int pair_flag=0; /*flag for accept pair */ -int chfield; /* flag for field mode */ -int nfix; /* no. of control points */ -int num[4]; /* no. of targets per image */ -int numc[4]; /* no. of targets in current image */ -int nump[4]; /* no. of targets in previous image */ -int numn[4]; /* no. of targets in next image */ -int n_trac[4]; /* no. of tracks */ -int match=0; /* no. of matches */ -int match2=0; /* no. of matches in 2nd pass */ -int nr[4][4]; /* point numbers for man. ori */ -int imx, imy, imgsize; /* image size */ -int zoom_x[4],zoom_y[4],zoom_f[4]; /* zoom parameters */ -int pp1=0, pp2=0, pp3=0, pp4=0,pp5=0; /* for man. orientation */ -int seq_first, seq_last; /* 1. and last img of seq */ -int max_shake_points, max_shake_frames, step_shake; -int demo_nr; /* for demo purposes */ -int examine = 0; /* for more detailed output */ -int dump_for_rdb; /* # of dumpfiles for rdb */ -int cr_sz; /* size of crosses */ -int display; /* display flag */ -int corp, corc, corn; -int m[4]; -int trackallocflag = 0; /* checkflag if mega, c4, t4 already allocated */ -int mask; /*checkmark for subtract mask*/ - -double pix_x, pix_y; /* pixel size */ -double ro; /* 200/pi */ -double cn, cnx, cny, csumg, eps0, corrmin; /* correspondences par */ -double rmsX, rmsY, rmsZ, mean_sigma0; /* a priori rms */ -double X_lay[2], Zmin_lay[2], Zmax_lay[2]; /* illu. layer data */ -double db_scale; /*dumbbell length, Beat Mai 2010*/ +int n_img; /* no of images */ +int hp_flag=0; /* flag for highpass */ +int allCam_flag=0; /* flag for using all cams for points */ +int tiff_flag=0; /* flag for tiff header */ +int pair_flag=0; /*flag for accept pair */ +int chfield; /* flag for field mode */ +int nfix; /* no. of control points */ +int num[4]; /* no. of targets per image */ +int numc[4]; /* no. of targets in current image */ +int nump[4]; /* no. of targets in previous image */ +int numn[4]; /* no. of targets in next image */ +int n_trac[4]; /* no. of tracks */ +int match=0; /* no. of matches */ +int match2=0; /* no. of matches in 2nd pass */ +int nr[4][4]; /* point numbers for man. ori */ +int imx, imy, imgsize; /* image size */ +int zoom_x[4],zoom_y[4],zoom_f[4]; /* zoom parameters */ +int pp1=0, pp2=0, pp3=0, pp4=0,pp5=0; /* for man. orientation */ +int seq_first, seq_last; /* 1. and last img of seq */ +int max_shake_points, max_shake_frames, step_shake; +int demo_nr; /* for demo purposes */ +int examine = 0; /* for more detailed output */ +int dump_for_rdb; /* # of dumpfiles for rdb */ +int cr_sz; /* size of crosses */ +int display; /* display flag */ +int corp, corc, corn; +int m[4]; +int trackallocflag = 0; /* checkflag if mega, c4, t4 already allocated */ +int mask; /*checkmark for subtract mask*/ + +double pix_x, pix_y; /* pixel size */ +double ro; /* 200/pi */ +double cn, cnx, cny, csumg, eps0, corrmin; /* correspondences par */ +double rmsX, rmsY, rmsZ, mean_sigma0; /* a priori rms */ +double X_lay[2], Zmin_lay[2], Zmax_lay[2]; /* illu. layer data */ +double db_scale; /*dumbbell length, Beat Mai 2010*/ FILE *fp1, *fp2, *fp3, *fp4, *fpp; -char img_name[4][256]; /* original image names */ -char img_lp_name[4][256]; /* lowpass image names */ -char img_hp_name[4][256]; /* highpass image names */ -char img_cal[4][128]; /* calibrayion image names */ -char img_ori[4][128]; /* image orientation data */ -char img_ori0[4][128]; /* orientation approx. values */ -char img_addpar[4][128]; /* image additional parameters */ -char safety[4][128]; -char safety_addpar[4][128]; -char img_addpar0[4][128]; /* ap approx. values */ -char seq_name[4][128]; /* sequence names */ -char img_mask_name[4][256]; /* mask image names*/ +char img_name[4][256]; /* original image names */ +char img_lp_name[4][256]; /* lowpass image names */ +char img_hp_name[4][256]; /* highpass image names */ +char img_cal[4][128]; /* calibrayion image names */ +char img_ori[4][128]; /* image orientation data */ +char img_ori0[4][128]; /* orientation approx. values */ +char img_addpar[4][128]; /* image additional parameters */ +char safety[4][128]; +char safety_addpar[4][128]; +char img_addpar0[4][128]; /* ap approx. values */ +char seq_name[4][128]; /* sequence names */ +char img_mask_name[4][256]; /* mask image names*/ char img_mask_path[256]; -char track_dir[128]; /* directory with dap track data */ -char fixp_name[128]; -char res_name[128]; /* result destination */ -char filename[128]; /* for general use */ -char buf[256], val[256]; /* buffer */ -char name[128]; //Beat Dez 08 -double xp, yp; //Beat Dez 08 - -unsigned char *img[4]; /* image data */ -unsigned char *img_mask[4]; /* mask data */ -unsigned char *img_new[4]; /* image data for reducing mask */ -unsigned char *img0[4]; /* image data for filtering etc */ -unsigned char *zoomimg; /* zoom image data */ - -Exterior Ex[4],sEx[4]; /* exterior orientation */ -Interior I[4],sI[4]; /* interior orientation */ -Glass G[4],sG[4]; /* glass orientation */ -ap_52 ap[4],sap[4]; /* add. parameters k1,k2,k3,p1,p2,scx,she */ -mm_np mmp; /* n-media parameters */ -target pix[4][nmax]; /* target pixel data */ -target pix0[4][12]; /* pixel data for man_ori points */ - -target *t4[4][4]; -int nt4[4][4]; - -coord_2d crd[4][nmax]; /* (distorted) metric coordinates */ -coord_2d geo[4][nmax]; /* corrected metric coordinates */ -coord_3d fix[20096]; /* testfield points coordinates */ //Beat changed it on 090325 -n_tupel con[nmax]; /* list of correspondences */ - -corres *c4[4]; -trackparameters tpar; /* tracking parameters */ - -mm_LUT mmLUT[4]; /* LUT for multimedia radial displacement */ -coord_3d *p_c3d; -P *mega[4]; +char track_dir[128]; /* directory with dap track data */ +char fixp_name[128]; +char res_name[128]; /* result destination */ +char filename[128]; /* for general use */ +char buf[256], val[256]; /* buffer */ +char name[128]; //Beat Dez 08 +double xp, yp; //Beat Dez 08 + +unsigned char *img[4]; /* image data */ +unsigned char *img_mask[4]; /* mask data */ +unsigned char *img_new[4]; /* image data for reducing mask */ +unsigned char *img0[4]; /* image data for filtering etc */ +unsigned char *zoomimg; /* zoom image data */ + +Exterior Ex[4],sEx[4]; /* exterior orientation */ +Interior I[4],sI[4]; /* interior orientation */ +Glass G[4],sG[4]; /* glass orientation */ +ap_52 ap[4],sap[4]; /* add. parameters k1,k2,k3,p1,p2,scx,she */ +mm_np mmp; /* n-media parameters */ +target pix[4][nmax]; /* target pixel data */ +target pix0[4][12]; /* pixel data for man_ori points */ + +target *t4[4][4]; +int nt4[4][4]; + +coord_2d crd[4][nmax]; /* (distorted) metric coordinates */ +coord_2d geo[4][nmax]; /* corrected metric coordinates */ +coord_3d fix[20096]; /* testfield points coordinates */ //Beat changed it on 090325 +n_tupel con[nmax]; /* list of correspondences */ + +corres *c4[4]; +trackparameters tpar; /* tracking parameters */ + +mm_LUT mmLUT[4]; /* LUT for multimedia radial displacement */ +coord_3d *p_c3d; +P *mega[4]; /***************************************************************************/ int init_proc_c(ClientData clientData, Tcl_Interp* interp, int argc, const char** argv) { - int i; - const char *valp; - - puts ("\n Multimedia Particle Positioning and Tracking Software \n"); - - valp = Tcl_GetVar(interp, "examine", TCL_GLOBAL_ONLY); - examine = atoi (valp); - - ro = 200/M_PI; - - fpp = fopen ("parameters/pft_version.par", "r"); - if (fpp){ - } - else{ - fpp = fopen ("parameters/pft_version.par", "w"); - fprintf(fpp,"%d\n", 0); - fclose(fpp); - } - - /* read from main parameter file */ - fpp = fopen_r ("parameters/ptv.par"); - - fscanf (fpp, "%d\n", &n_img); - - for (i=0; i<4; i++) - { - fscanf (fpp, "%s\n", img_name[i]); - fscanf (fpp, "%s\n", img_cal[i]); - } - fscanf (fpp, "%d\n", &hp_flag); - fscanf (fpp, "%d\n", &allCam_flag); - fscanf (fpp, "%d\n", &tiff_flag); - fscanf (fpp, "%d\n", &imx); - fscanf (fpp, "%d\n", &imy); - fscanf (fpp, "%lf\n", &pix_x); - fscanf (fpp, "%lf\n", &pix_y); - fscanf (fpp, "%d\n", &chfield); - fscanf (fpp, "%lf\n", &mmp.n1); - fscanf (fpp, "%lf\n", &mmp.n2[0]); - fscanf (fpp, "%lf\n", &mmp.n3); - fscanf (fpp, "%lf\n", &mmp.d[0]); - fclose (fpp); - - /* read illuminated layer data */ - fpp = fopen_r ("parameters/criteria.par"); - fscanf (fpp, "%lf\n", &X_lay[0]); - fscanf (fpp, "%lf\n", &Zmin_lay[0]); - fscanf (fpp, "%lf\n", &Zmax_lay[0]); - fscanf (fpp, "%lf\n", &X_lay[1]); - fscanf (fpp, "%lf\n", &Zmin_lay[1]); - fscanf (fpp, "%lf\n", &Zmax_lay[1]); - fscanf (fpp, "%lf", &cnx); - fscanf (fpp, "%lf", &cny); - fscanf (fpp, "%lf", &cn); - fscanf (fpp, "%lf", &csumg); - fscanf (fpp, "%lf", &corrmin); - fscanf (fpp, "%lf", &eps0); - fclose (fpp); - - mmp.nlay = 1; - - /* read sequence parameters (needed for some demos) */ - - fpp = fopen_r ("parameters/sequence.par"); - - for (i=0; i<4; i++) fscanf (fpp, "%s\n", seq_name[i]); - fscanf (fpp,"%d\n", &seq_first); - fscanf (fpp,"%d\n", &seq_last); - fclose (fpp); - - /* initialize zoom parameters and image positions */ - for (i=0; i error\n", i); - exit (1); + int i; + const char *valp; + + puts ("\n Multimedia Particle Positioning and Tracking Software \n"); + + valp = Tcl_GetVar(interp, "examine", TCL_GLOBAL_ONLY); + examine = atoi (valp); + + ro = 200/M_PI; + + fpp = fopen ("parameters/pft_version.par", "r"); + if (! fpp) { + fpp = fopen ("parameters/pft_version.par", "w"); + fprintf(fpp,"%d\n", 0); + fclose(fpp); } - } - for (i=0; i error\n", i); - exit (1); + /* read from main parameter file */ + fpp = fopen_rp ("parameters/ptv.par"); // replaced fopen_r, ad holten 12-2012 + if (!fpp) return TCL_OK; + + fscanf (fpp, "%d\n", &n_img); + + for (i=0; i<4; i++) { + fscanf (fpp, "%s\n", img_name[i]); + fscanf (fpp, "%s\n", img_cal[i]); + } + fscanf (fpp, "%d\n", &hp_flag); + fscanf (fpp, "%d\n", &allCam_flag); + fscanf (fpp, "%d\n", &tiff_flag); + fscanf (fpp, "%d\n", &imx); + fscanf (fpp, "%d\n", &imy); + fscanf (fpp, "%lf\n", &pix_x); + fscanf (fpp, "%lf\n", &pix_y); + fscanf (fpp, "%d\n", &chfield); + fscanf (fpp, "%lf\n", &mmp.n1); + fscanf (fpp, "%lf\n", &mmp.n2[0]); + fscanf (fpp, "%lf\n", &mmp.n3); + fscanf (fpp, "%lf\n", &mmp.d[0]); + fclose (fpp); + + /* read illuminated layer data */ + fpp = fopen_rp ("parameters/criteria.par"); // replaced fopen_r, ad holten 12-2012 + if (!fpp) return TCL_OK; + + fscanf (fpp, "%lf\n", &X_lay[0]); + fscanf (fpp, "%lf\n", &Zmin_lay[0]); + fscanf (fpp, "%lf\n", &Zmax_lay[0]); + fscanf (fpp, "%lf\n", &X_lay[1]); + fscanf (fpp, "%lf\n", &Zmin_lay[1]); + fscanf (fpp, "%lf\n", &Zmax_lay[1]); + fscanf (fpp, "%lf", &cnx); + fscanf (fpp, "%lf", &cny); + fscanf (fpp, "%lf", &cn); + fscanf (fpp, "%lf", &csumg); + fscanf (fpp, "%lf", &corrmin); + fscanf (fpp, "%lf", &eps0); + fclose (fpp); + + mmp.nlay = 1; + + /* read sequence parameters (needed for some demos) */ + + fpp = fopen_rp ("parameters/sequence.par"); // replaced fopen_r, ad holten 12-2012 + if (!fpp) return TCL_OK; + + for (i=0; i<4; i++) + fscanf (fpp, "%s\n", seq_name[i]); + fscanf (fpp,"%d\n", &seq_first); + fscanf (fpp,"%d\n", &seq_last); + fclose (fpp); + + /* initialize zoom parameters and image positions */ + for (i=0; i error\n", i); + exit (1); + } } - } - for (i=0; i error\n", i); - exit (1); + for (i=0; i error\n", i); + exit (1); + } } - } - for (i=0; i error\n", i); - exit (1); + for (i=0; i error\n", i); + exit (1); + } } - } - zoomimg = (unsigned char *) calloc (imgsize, 1); - if ( ! zoomimg) - { - printf ("calloc for zoomimg --> error\n"); - return TCL_ERROR; - } + for (i=0; i error\n", i); + exit (1); + } + } - parameter_panel_init(interp); - cr_sz = atoi(Tcl_GetVar2(interp, "mp", "pcrossize", TCL_GLOBAL_ONLY)); + zoomimg = (unsigned char*) calloc(imgsize, 1); + if (! zoomimg) { + printf ("calloc for zoomimg --> error\n"); + return TCL_ERROR; + } - display = 1; - return TCL_OK; + parameter_panel_init(interp); + cr_sz = atoi(Tcl_GetVar2(interp, "mp", "pcrossize", TCL_GLOBAL_ONLY)); + display = 1; + return TCL_OK; } int start_proc_c(ClientData clientData, Tcl_Interp* interp, int argc, const char** argv) { - int i, k; - - /* read from main parameter file */ - fpp = fopen_r ("parameters/ptv.par"); - - fscanf (fpp, "%d\n", &n_img); - - for (i=0; i<4; i++) - { - fscanf (fpp, "%s\n", img_name[i]); - fscanf (fpp, "%s\n", img_cal[i]); - } - fscanf (fpp, "%d\n", &hp_flag); - fscanf (fpp, "%d\n", &allCam_flag); - fscanf (fpp, "%d\n", &tiff_flag); - fscanf (fpp, "%d\n", &imx); - fscanf (fpp, "%d\n", &imy); - fscanf (fpp, "%lf\n", &pix_x); - fscanf (fpp, "%lf\n", &pix_y); - fscanf (fpp, "%d\n", &chfield); - fscanf (fpp, "%lf\n", &mmp.n1); - fscanf (fpp, "%lf\n", &mmp.n2[0]); - fscanf (fpp, "%lf\n", &mmp.n3); - fscanf (fpp, "%lf\n", &mmp.d[0]); - fclose (fpp); - - /* read illuminated layer data */ - fpp = fopen_r ("parameters/criteria.par"); - fscanf (fpp, "%lf\n", &X_lay[0]); - fscanf (fpp, "%lf\n", &Zmin_lay[0]); - fscanf (fpp, "%lf\n", &Zmax_lay[0]); - fscanf (fpp, "%lf\n", &X_lay[1]); - fscanf (fpp, "%lf\n", &Zmin_lay[1]); - fscanf (fpp, "%lf\n", &Zmax_lay[1]); - fscanf (fpp, "%lf", &cnx); - fscanf (fpp, "%lf", &cny); - fscanf (fpp, "%lf", &cn); - fscanf (fpp, "%lf", &csumg); - fscanf (fpp, "%lf", &corrmin); - fscanf (fpp, "%lf", &eps0); - fclose (fpp); - - mmp.nlay = 1; - - /* read sequence parameters (needed for some demos) */ - - fpp = fopen_r ("parameters/sequence.par"); - - for (i=0; i<4; i++) fscanf (fpp, "%s\n", seq_name[i]); - fscanf (fpp,"%d\n", &seq_first); - fscanf (fpp,"%d\n", &seq_last); - fclose (fpp); - - /* create file names */ - for (i=0; i metric coordinates */ + /* transformations metric coordinates -> corrected metric coordinates */ + for (i_img=0; i_img metric coordinates */ - /* transformations metric coordinates -> corrected metric coordinates */ - for (i_img=0; i_img= 0) p[j] = geo[j][con[i].p[j]].pnr; + else p[j] = -1; + } + for (j=0, n=0; j<4; j++) { + if (p[j] > -1) { + x[j] = crd[j][p[j]].x; y[j] = crd[j][p[j]].y; + n++; + } + else { + x[j] = -1e10; y[j] = -1e10; + if (p[j] == -2) n = -100; + } + } - return TCL_OK; -} + // take only points which are matched in all images + // or triplets/quadruplets which result from object model + // e.g.: quad -> n=4; model triplet -> n=3; model pair -> n=2; + // unrestricted triplet -> n<0; unrestricted pair -> n<0 + /* if (n_img > 2 && n < 3) continue; */ + if ((n_img > 2 && num[0]>64 && num[1]>64 && num[2]>64 && num[3]>64) + && n < 3) continue; -int determination_proc_c (ClientData clientData, Tcl_Interp* interp, int argc, const char** argv) -{ - int i, j, n,dummy; - int p[4]; - double x[4], y[4], X,Y,Z; - double Zlo = 1e20, Zhi = -1e20; - int dumbbell=0,i1,i2; - double x1,y1,z1,x2,y2,z2,dist,mx,my,mz,nx,ny,nz; - int a1[4],a2[4],checksum_1,checksum_2; - - - puts ("Determinate"); - - sprintf (buf, "Point positioning (mid_point in 3d)"); - Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); - Tcl_Eval(interp, ".text delete 2"); - Tcl_Eval(interp, ".text insert 2 $tbuf"); - - - /* Beat Mai 2007 to set the variable examine for mulit-plane calibration*/ - fp1 = fopen_r ("parameters/examine.par"); - fscanf (fp1,"%d\n", &dummy); - fclose (fp1); - if (dummy==1){ - examine=4; - } - else{ - examine=0; - } - ////////////////////////////////// - - - fp1 = fopen (res_name, "w"); - - if ( ! fp1) - { - sprintf(res_name,"res/dt_lsq"); - fp1 = fopen (res_name, "w"); - } - if ( ! fp1) - { - printf ("cannot find dir: res, data written to dt_lsq in same dir\n"); - sprintf (res_name, "dt_lsq"); - fp1 = fopen (res_name, "w"); - } - /* create dump file for rdb */ - if (examine == 4) - { - /* create filename for dumped dataset */ - sprintf (res_name, "dump_for_rdb"); - printf ("dataset dumped into %s\n", res_name); - fp2 = fopen (res_name, "w"); - - /* write # of points to file */ - fprintf (fp2, "%d\n", match); - } - /* first line to be updated in res_name file */ - fprintf (fp1, "%4d\n", match); - /* least squares determination for triplets */ - - rmsX = 0; rmsY = 0; rmsZ = 0; mean_sigma0 = 0; - - for (i=0; i= 0) p[j] = geo[j][con[i].p[j]].pnr; - else p[j] = -1; - - for (j=0, n=0; j<4; j++) - { - if (p[j] > -1) - { - x[j] = crd[j][p[j]].x; y[j] = crd[j][p[j]].y; - n++; - } - else - { - x[j] = -1e10; y[j] = -1e10; - if (p[j] == -2) n = -100; - } + /* hack due to problems with approx in det_lsq: */ + X = 0.0; Y = 0.0; Z = (Zmin_lay[0]+Zmax_lay[0])/2.0; + for (j=0; j -1) fprintf (fp1, " %4d", pix[0][p[0]].sumg); + // else fprintf (fp1, " %4d", -1); + // if (p[1] > -1) fprintf (fp1, " %4d", pix[1][p[1]].sumg); + // else fprintf (fp1, " %4d", -1); + // if (p[2] > -1) fprintf (fp1, " %4d", pix[2][p[2]].sumg); + // else fprintf (fp1, " %4d", -1); + // if (p[3] > -1) fprintf (fp1, " %4d", pix[3][p[3]].sumg); + // else fprintf (fp1, " %4d", -1); + // */ + + // fprintf (fp1, " %9.3f %9.3f %9.3f", X, Y, Z); + // if (p[0] > -1) fprintf (fp1, " %4d", pix[0][p[0]].pnr); + // else fprintf (fp1, " %4d", -1); + // if (p[1] > -1) fprintf (fp1, " %4d", pix[1][p[1]].pnr); + // else fprintf (fp1, " %4d", -1); + // if (p[2] > -1) fprintf (fp1, " %4d", pix[2][p[2]].pnr); + // else fprintf (fp1, " %4d", -1); + // if (p[3] > -1) fprintf (fp1, " %4d\n", pix[3][p[3]].pnr); + // else fprintf (fp1, " %4d\n", -1); + + fprintf (fp1, "%4d %9.3f %9.3f %9.3f", i+1, X, Y, Z); + for (j=0; j<4; j++) { + if (p[j] > -1) fprintf (fp1, " %4d", pix[j][p[j]].pnr); + else fprintf (fp1, " %4d", -1); + } + fprintf (fp1, "\n"); + + /* write data as new points to dump for rdb */ + if (examine == 4) { + fprintf (fp2, "%d %10.3f %10.3f %10.3f %d ", i, X, Y, Z, 3); + for (j=0; j Zhi) Zhi = Z; } - - /* take only points which are matched in all images */ - /* or triplets/quadruplets which result from object model */ - /* e.g.: quad -> n=4; model triplet -> n=3; model pair -> n=2; - unrestricted triplet -> n<0; unrestricted pair -> n<0 */ - /* if (n_img > 2 && n < 3) continue; */ - - /* ################################# */ - /* take only points which are matched in all images */ - /* or triplets/quadruplets which result from object model */ - /* e.g.: quad -> n=4; model triplet -> n=3; model pair -> n=2; - unrestricted triplet -> n<0; unrestricted pair -> n<0 */ - if ((n_img > 2 && num[0]>64 && num[1]>64 && num[2]>64 && num[3]>64) - && n < 3) continue; - -/* hack due to problems with approx in det_lsq: */ -X = 0.0; Y = 0.0; Z = (Zmin_lay[0]+Zmax_lay[0])/2.0; -for (j=0; j -1) fprintf (fp1, " %4d", pix[0][p[0]].sumg); - else fprintf (fp1, " %4d", -1); - if (p[1] > -1) fprintf (fp1, " %4d", pix[1][p[1]].sumg); - else fprintf (fp1, " %4d", -1); - if (p[2] > -1) fprintf (fp1, " %4d", pix[2][p[2]].sumg); - else fprintf (fp1, " %4d", -1); - if (p[3] > -1) fprintf (fp1, " %4d", pix[3][p[3]].sumg); - else fprintf (fp1, " %4d", -1); - */ - - fprintf (fp1, " %9.3f %9.3f %9.3f", X, Y, Z); - if (p[0] > -1) fprintf (fp1, " %4d", pix[0][p[0]].pnr); - else fprintf (fp1, " %4d", -1); - if (p[1] > -1) fprintf (fp1, " %4d", pix[1][p[1]].pnr); - else fprintf (fp1, " %4d", -1); - if (p[2] > -1) fprintf (fp1, " %4d", pix[2][p[2]].pnr); - else fprintf (fp1, " %4d", -1); - if (p[3] > -1) fprintf (fp1, " %4d\n", pix[3][p[3]].pnr); - else fprintf (fp1, " %4d\n", -1); - - /* write data as new points to dump for rdb */ - if (examine == 4) - { - fprintf (fp2, "%d %10.3f %10.3f %10.3f %d ", i, X, Y, Z, 3); - for (j=0; j 0) { + if (atoi(argv[1])==3){ + dumbbell=1; + display=0; + } } - - if (Z < Zlo) Zlo = Z; if (Z > Zhi) Zhi = Z; - } - - fclose (fp1); - - //Beat Mai 2010: now we should open the file db_is.* again, check - // if it has exactly two points, rescale them, write them again and close the file. - if ((argv[1])>0){ - if (atoi(argv[1])==3){ - dumbbell=1; - display=0; - } - } - if (dumbbell==1){ - fpp = fopen ("parameters/dumbbell.par", "r"); - if (fpp){ - fscanf (fpp, "%lf", &eps0); - fscanf (fpp, "%lf", &db_scale); - fclose (fpp); - } - fpp = fopen (res_name, "r"); - fscanf (fpp, "%d\n", &match); - if(match==2){ - fscanf(fpp, "%d %lf %lf %lf %d %d %d %d\n", - &i1, &x1, &y1, &z1, - &a1[0], &a1[1], &a1[2], &a1[3]); - fscanf(fpp, "%d %lf %lf %lf %d %d %d %d\n", - &i2, &x2, &y2, &z2, - &a2[0], &a2[1], &a2[2], &a2[3]); - //now adapt x,y,z - /*dist=pow(pow(x2-x1,2.)+pow(y2-y1,2.)+pow(z2-z1,2.),0.5); - mx=0.5*(x1+x2); - my=0.5*(y1+y2); - mz=0.5*(z1+z2); - nx=(x2-x1)/dist; - ny=(y2-y1)/dist; - nz=(z2-z1)/dist; - x1=mx-0.5*db_scale*nx; - x2=mx+0.5*db_scale*nx; - y1=my-0.5*db_scale*ny; - y2=my+0.5*db_scale*ny; - z1=mz-0.5*db_scale*nz; - z2=mz+0.5*db_scale*nz;*/ - - //check if reasonable - /*dist=pow(pow(x2-x1,2.)+pow(y2-y1,2.)+pow(z2-z1,2.),0.5); - if (fabs(dist-38)>1){ - match=0; - }*/ + if (dumbbell==1) { + fpp = fopen_rp ("parameters/dumbbell.par"); // replaced fopen, ad holten, 12-2012 + if (!fpp) return TCL_OK; + fscanf (fpp, "%lf", &eps0); + fscanf (fpp, "%lf", &db_scale); + fclose (fpp); + + fpp = fopen_rp (res_name); // replaced fopen, ad holten, 12-2012 + if (!fpp) return TCL_OK; + fscanf (fpp, "%d\n", &match); + if (match==2) { + fscanf(fpp, "%d %lf %lf %lf %d %d %d %d\n", + &i1, &x1, &y1, &z1, &a1[0], &a1[1], &a1[2], &a1[3]); + fscanf(fpp, "%d %lf %lf %lf %d %d %d %d\n", + &i2, &x2, &y2, &z2, &a2[0], &a2[1], &a2[2], &a2[3]); + // // now adapt x,y,z + // /* dist=pow(pow(x2-x1,2.)+pow(y2-y1,2.)+pow(z2-z1,2.),0.5); + // mx=0.5*(x1+x2); + // my=0.5*(y1+y2); + // mz=0.5*(z1+z2); + // nx=(x2-x1)/dist; + // ny=(y2-y1)/dist; + // nz=(z2-z1)/dist; + // x1=mx-0.5*db_scale*nx; + // x2=mx+0.5*db_scale*nx; + // y1=my-0.5*db_scale*ny; + // y2=my+0.5*db_scale*ny; + // z1=mz-0.5*db_scale*nz; + // z2=mz+0.5*db_scale*nz; + // */ + + // // check if reasonable + // /* dist=pow(pow(x2-x1,2.)+pow(y2-y1,2.)+pow(z2-z1,2.),0.5); + // if (fabs(dist-38)>1){ + // match=0; + // }*/ - //check if all quadruplets or triplets - checksum_1=0; - checksum_2=0; - for(j=0;j<4;j++){ - if(a1[1]<0){ - checksum_1++; - } - if(a2[1]<0){ - checksum_2++; + //check if all quadruplets or triplets + checksum_1=0; + checksum_2=0; + for(j=0; j<4; j++) { + if (a1[1] < 0) + checksum_1++; + if (a2[1] < 0) + checksum_2++; } - } - if(checksum_1>1 || checksum_2>1){ - match=0; - } - //end of check if all quadruplets or triplets - } - else{ - match=0; - } - fclose (fpp); - fpp = fopen (res_name, "w"); - if(match==2){ - fprintf (fpp, "%4d\n", match); - fprintf (fpp, " %4d %9.3f %9.3f %9.3f %4d %4d %4d %4d\n", i1,x1,y1,z1,a1[0],a1[1],a1[2],a1[3]); - fprintf (fpp, " %4d %9.3f %9.3f %9.3f %4d %4d %4d %4d\n", i2,x2,y2,z2,a2[0],a2[1],a2[2],a2[3]); - } - else{ - fprintf (fpp, "%4d\n", 0); - } - fclose (fpp); - - } - //end of dumbbell treatment - - rmsX = sqrt(rmsX/match); rmsY = sqrt(rmsY/match); rmsZ = sqrt(rmsZ/match); - mean_sigma0 = sqrt (mean_sigma0/match); - - sprintf (buf, "Match: %d, => rms = %4.2f micron, rms_x,y,z = %5.3f/%5.3f/%5.3f mm", match, mean_sigma0*1000, rmsX, rmsY, rmsZ); - puts (buf); - Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); - Tcl_Eval(interp, ".text delete 3"); - Tcl_Eval(interp, ".text insert 3 $tbuf"); - - /* sort coordinates for binary search in epi line segment drawing */ - for (i=0; i1 || checksum_2>1) + match=0; + // end of check if all quadruplets or triplets + } + else + match=0; + + fclose (fpp); + fpp = fopen (res_name, "w"); + if (match==2) { + fprintf (fpp, "%4d\n", match); + fprintf (fpp, " %4d %9.3f %9.3f %9.3f %4d %4d %4d %4d\n", i1,x1,y1,z1,a1[0],a1[1],a1[2],a1[3]); + fprintf (fpp, " %4d %9.3f %9.3f %9.3f %4d %4d %4d %4d\n", i2,x2,y2,z2,a2[0],a2[1],a2[2],a2[3]); + } + else + fprintf (fpp, "%4d\n", 0); + fclose (fpp); + } + //end of dumbbell treatment + rmsX = sqrt(rmsX/match); rmsY = sqrt(rmsY/match); rmsZ = sqrt(rmsZ/match); + mean_sigma0 = sqrt (mean_sigma0/match); + + sprintf (buf, "Match: %d, => rms = %4.2f micron, rms_x,y,z = %5.3f/%5.3f/%5.3f mm", match, mean_sigma0*1000, rmsX, rmsY, rmsZ); + puts (buf); + Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); + Tcl_Eval(interp, ".text delete 3"); + Tcl_Eval(interp, ".text insert 3 $tbuf"); + + /* sort coordinates for binary search in epi line segment drawing */ + for (i=0; i1){ - determination_proc_c (clientData, interp, argc, argv);} + //slicepos++; if (slicepos==nslices) {slicepos=0;} + // if (i < 10) sprintf (seq_ch, "%1d", i); + // else if (i < 100) sprintf (seq_ch, "%2d", i); + // else sprintf (seq_ch, "%3d", i); + // changed, ad holten 12-2012 + sprintf (seq_ch, "%d", i); - /* delete unneeded files */ + for (j=0; j1) + determination_proc_c (clientData, interp, argc, argv); + + /* delete unneeded files */ + for (j=0; j nmax) { + printf("Aborted, too many targets detected!\n"); // ad holten, 12-2012 + exit(1); + } + } - /* target recognition */ - for (i=0; i nmax) exit (1); - } + 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; - /* save pixel coord as approx. for template matching */ - if (examine) for (i=0; i= 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); - } + /* adapt # of detected points */ + num[i] = nfix; + + for (j=0; j= 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 16: puts ("Sort grid points using files"); //Beat Jan 2011 - for (i=0; i= 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); - } + /* adapt # of detected points */ + num[i] = nfix; + + for (j=0; j= 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, ""); + // replaced next lines by for loop, ad holten 12-2012 + // strcpy (safety[0], "safety_0"); + // strcat (safety[0], ".ori"); + // strcpy (safety[1], "safety_1"); + // strcat (safety[1], ".ori"); + // strcpy (safety[2], "safety_2"); + // strcat (safety[2], ".ori"); + // strcpy (safety[3], "safety_3"); + // strcat (safety[3], ".ori"); + // strcpy (safety_addpar[0], "safety_0"); + // strcat (safety_addpar[0], ".addpar"); + // strcpy (safety_addpar[1], "safety_1"); + // strcat (safety_addpar[1], ".addpar"); + // strcpy (safety_addpar[2], "safety_2"); + // strcat (safety_addpar[2], ".addpar"); + // strcpy (safety_addpar[3], "safety_3"); + // strcat (safety_addpar[3], ".addpar"); + for (i=0; i<4; i++) { + sprintf(safety[i], "safety_%d.ori", i); + sprintf(safety_addpar[i], "safety_%d.addpar", i); + } - case 6: puts ("Orientation"); strcpy(buf, ""); + for (i_img=0; i_imgpix */ + metric_to_pixel (crd[i_img][i].x, crd[i_img][i].y, imx,imy, pix_x,pix_y, &pix[i_img][i].x, &pix[i_img][i].y, chfield); + /*then draw crosses*/ + //intx1 = (int) pix[i_img][i].x; + //inty1 = (int) pix[i_img][i].y; + //drawcross (interp, intx1, inty1, 3, i_img, "orange"); + } + orient_v3 (interp, Ex[i_img], I[i_img], G[i_img], ap[i_img], mmp, + nfix, fix, crd[i_img], &Ex[i_img], &I[i_img], &G[i_img], &ap[i_img], i_img); + /////////////////////////////////////////// + } - /* markus 14.05.2007 show coordinates combined */ - + /* save orientation and additional parameters */ + write_ori (Ex[i_img], I[i_img], G[i_img], img_ori[i_img]); + fp1 = fopen( img_ori[i_img], "r" ); + if(fp1 != NULL) { + fclose(fp1); + read_ori (&sEx[i_img], &sI[i_img], &sG[i_img], img_ori[i_img]); + fp1 = fopen (img_addpar0[i_img], "r"); + if (! fp1) fp1 = fopen ("addpar.raw", "r"); + + if (fp1) { + fscanf (fp1, "%lf %lf %lf %lf %lf %lf %lf", + &sap[i_img].k1, &sap[i_img].k2,&sap[i_img].k3, + &sap[i_img].p1, &sap[i_img].p2, + &sap[i_img].scx, &sap[i_img].she); + fclose (fp1); + } + else { + printf("no addpar.raw\n"); + sap[i_img].k1=sap[i_img].k2=sap[i_img].k3=sap[i_img].p1=sap[i_img].p2=sap[i_img].she=0.0; + sap[i_img].scx=1.0; + } - - for (i=0; ipix */ - metric_to_pixel (crd[i_img][i].x, crd[i_img][i].y, imx,imy, pix_x,pix_y, &pix[i_img][i].x, &pix[i_img][i].y, chfield); - /*then draw crosses*/ - //intx1 = (int) pix[i_img][i].x; - //inty1 = (int) pix[i_img][i].y; + write_ori (sEx[i_img], sI[i_img], sG[i_img], safety[i_img]); + fp1 = fopen (safety_addpar[i_img], "w"); + fprintf (fp1, "%f %f %f %f %f %f %f", + sap[i_img].k1, sap[i_img].k2, sap[i_img].k3, + sap[i_img].p1, sap[i_img].p2, + sap[i_img].scx, sap[i_img].she); + fclose (fp1); + } + else{ + write_ori (Ex[i_img], I[i_img], G[i_img], safety[i_img]); + fp1 = fopen (safety_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); + } + write_ori (Ex[i_img], I[i_img], G[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"); + if (examine != 4) + Tcl_Eval(interp, ".text insert 2 \"...done, sigma0 for each image -> \""); + if (examine == 4 && multi==0) + Tcl_Eval(interp, ".text insert 2 \"resection data written to disk \""); + Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); + Tcl_Eval(interp, ".text insert 3 $tbuf"); + break; - //drawcross (interp, intx1, inty1, 3, i_img, "orange"); - - - } + case 7: checkpoint_proc (interp); + 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"); + break; - orient_v3 (interp, Ex[i_img], I[i_img], G[i_img], ap[i_img], mmp, - nfix, fix, crd[i_img], - &Ex[i_img], &I[i_img], &G[i_img], &ap[i_img], i_img); - - - + case 8: /* draw additional parameter figures */ + Tcl_Eval(interp, "clearcam"); + /* read orientation and additional parameters */ + for (i=0; i \""); - if (examine == 4 && multi==0) - Tcl_Eval(interp, ".text insert 2 \"resection data written to disk \""); - Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); - Tcl_Eval(interp, ".text insert 3 $tbuf"); - - break; - - case 7: checkpoint_proc (interp); - 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"); - break; - - - case 8: /* draw additional parameter figures */ - - Tcl_Eval(interp, "clearcam"); - - /* read orientation and additional parameters */ - for (i=0; i-25 && - //fix[i].x>-25 && - if( (a[i_img]>-1 && next>-1 && prev>-1 && i-1 && a[1]>-1 && a[2]>-1 && a[3]>-1 && next>-1 && prev>-1 && filenumber==seq_first+2 ) ){// OR ALLE QUADRUPLETS - pix[i_img][i].x=t4[3][i_img][a[i_img]].x; - pix[i_img][i].y=t4[3][i_img][a[i_img]].y; - pix[i_img][i].pnr=i; - fix[i].pnr=i; - - i++; - nfix =i; - if(currentFrame-1 && a[1]>-1 && a[2]>-1 && a[3]>-1 && next>-1 && prev>-1){ - pix[i_img][i].x=t4[3][i_img][a[i_img]].x; - pix[i_img][i].y=t4[3][i_img][a[i_img]].y; - pix[i_img][i].pnr=i; - fix[i].pnr=i; - - i++; - nfix =i; - } - } - if (n_img==3){ - if(a[0]>-1 && a[1]>-1 && a[2]>-1 && next>-1 && prev>-1){ - pix[i_img][i].x=t4[3][i_img][a[i_img]].x; - pix[i_img][i].y=t4[3][i_img][a[i_img]].y; - pix[i_img][i].pnr=i; - fix[i].pnr=i; - - i++; - nfix =i; - } - }*/ - - }while(!feof(FILEIN)); - fclose(FILEIN); + for (i_img=0; i_img=n_img; j--) a[j] = -1; + + fscanf(FILEIN_ptv, "%d %d %*lf %*lf %*lf\n", &prev, &next); + + // read pixel data according a0,a1,a2,a3 + if (( a[i_img]>-1 && next>-1 && prev>-1 && i-1 && a[1]>-1 && a[2]>-1 && a[3]>-1 && next>-1 && prev>-1 && filenumber==seq_first+2 ) ) + { + pix[i_img][i].x = t4[3][i_img][a[i_img]].x; + pix[i_img][i].y = t4[3][i_img][a[i_img]].y; + pix[i_img][i].pnr = i; + fix[i].pnr=i; + + nfix = ++i; + if (currentFrame < filenumber) { + currentFrame = filenumber; + frameCount++; + } + } + } while (!feof(FILEIN)); + fclose(FILEIN); + + } // end of loop through seq, but loop i_img still open + if (frameCount==1) + printf("Using %d linked points of %d frame for camera %d\n", nfix,frameCount, i_img+1); + else + printf("Using %d linked points of %d frames for camera %d\n", nfix,frameCount, i_img+1); + + for (i=0; i \""); + if (examine == 4 && multi==0) + Tcl_Eval(interp, ".text insert 2 \"resection data written to disk \""); + Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); + Tcl_Eval(interp, ".text insert 3 $tbuf"); - - /* ================= */ - - orient_v3 (interp, Ex[i_img], I[i_img], G[i_img], ap[i_img], mmp, - nfix, fix, crd[i_img], - &Ex[i_img], &I[i_img], &G[i_img], &ap[i_img], i_img); - - /* ================= */ - - - /* save orientation and additional parameters */ - //make safety copy of ori files - - fp1 = fopen( img_ori[i_img], "r" ); - if(fp1 != NULL) { - fclose(fp1); - read_ori (&sEx[i_img], &sI[i_img], &sG[i_img], img_ori[i_img]); - fp1 = fopen (img_addpar0[i_img], "r"); - if (! fp1) fp1 = fopen ("addpar.raw", "r"); - - if (fp1) { - fscanf (fp1, "%lf %lf %lf %lf %lf %lf %lf", - &sap[i_img].k1,&sap[i_img].k2,&sap[i_img].k3, - &sap[i_img].p1,&sap[i_img].p2, - &sap[i_img].scx,&sap[i_img].she); - fclose (fp1);} - else { - printf("no addpar.raw\n"); - sap[i_img].k1=sap[i_img].k2=sap[i_img].k3=sap[i_img].p1=sap[i_img].p2=sap[i_img].she=0.0; - sap[i_img].scx=1.0; - } - - write_ori (sEx[i_img], sI[i_img], sG[i_img], safety[i_img]); - fp1 = fopen (safety_addpar[i_img], "w"); - fprintf (fp1, "%f %f %f %f %f %f %f", - sap[i_img].k1, sap[i_img].k2, sap[i_img].k3, - sap[i_img].p1, sap[i_img].p2, - sap[i_img].scx, sap[i_img].she); - fclose (fp1); - } - else{ - write_ori (Ex[i_img], I[i_img], G[i_img], safety[i_img]); - fp1 = fopen (safety_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); - - } - write_ori (Ex[i_img], I[i_img], G[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); - }//end of loop through images - - Tcl_Eval(interp, ".text delete 3"); - Tcl_Eval(interp, ".text delete 1"); - Tcl_Eval(interp, ".text insert 1 \"Orientation from particles \""); - Tcl_Eval(interp, ".text delete 2"); - if (examine != 4) - Tcl_Eval(interp, ".text insert 2 \"...done, sigma0 for each image -> \""); - if (examine == 4 && multi==0) - Tcl_Eval(interp, ".text insert 2 \"resection data written to disk \""); - Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); - Tcl_Eval(interp, ".text insert 3 $tbuf"); - - break; - - case 20: puts ("Orientation from particles, discarding bad 3d-points"); strcpy(buf, ""); - - strcpy (safety[0], "safety_0"); - strcat (safety[0], ".ori"); - strcpy (safety[1], "safety_1"); - strcat (safety[1], ".ori"); - strcpy (safety[2], "safety_2"); - strcat (safety[2], ".ori"); - strcpy (safety[3], "safety_3"); - strcat (safety[3], ".ori"); - strcpy (safety_addpar[0], "safety_0"); - strcat (safety_addpar[0], ".addpar"); - strcpy (safety_addpar[1], "safety_1"); - strcat (safety_addpar[1], ".addpar"); - strcpy (safety_addpar[2], "safety_2"); - strcat (safety_addpar[2], ".addpar"); - strcpy (safety_addpar[3], "safety_3"); - strcat (safety_addpar[3], ".addpar"); - - 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); - - fpp = fopen_r ("parameters/shaking.par"); - fscanf (fpp,"%d\n", &seq_first); - fscanf (fpp,"%d\n", &seq_last); - fscanf (fpp,"%d\n", &max_shake_points); - fscanf (fpp,"%d\n", &max_shake_frames); - fclose (fpp); - - /* read from main parameter file */ - fpp = fopen_r ("parameters/ptv.par"); - fscanf (fpp, "%d\n", &n_img); - fclose (fpp); - //here I will insert a check for each fix -point. Beat Nov 2011 - //The idea is to mark those n of nfix, which are bad, so not to use them in thenext loop - //here I will insert a check for each fix -point. Beat Nov 2011 - //The idea is to mark those n of nfix, which are bad, so not to use them in thenext loop - //here I will insert a check for each fix -point. Beat Nov 2011 - //The idea is to mark those n of nfix, which are bad, so not to use them in thenext loop - - i=0; - frameCount=0; - currentFrame=0; - step_shake=(int)((double)(seq_last-seq_first+1)/(double)max_shake_frames+0.5); - printf("\nframe step size for each camera is %d\n", step_shake); - for (filenumber=seq_first+2; filenumber-1 && a[1]>-1 && a[2]>-1 && a[3]>-1 && next>-1 && prev>-1 && filenumber==seq_first+2 ) ){// OR ALLE QUADRUPLETS - for (i_img=0; i_img=n_img; j--) a[j] = -1; + + fscanf(FILEIN_ptv, "%d %d %*lf %*lf %*lf\n", &prev, &next); + + // read pixel data according a0,a1,a2,a3 + if ( (a[0]>-1 && a[1]>-1 && a[2]>-1 && a[3]>-1 + && next>-1 && prev>-1 && filenumber==seq_first+2 ) ) // OR ALLE QUADRUPLETS + { + for (i_img=0; i_img-999){ - x = crd[0][i].x - I[0].xh; - y = crd[0][i].y - I[0].yh; - //correct_brown_affin (x, y, ap[0], &x, &y); - ray_tracing_v2 (x,y, Ex[0], I[0], G[0], mmp, &X[0], &Y[0], &Z[0], &aa[0], &b[0], &c[0]); - } - if(crd[1][i].x>-999){ - x = crd[1][i].x - I[1].xh; - y = crd[1][i].y - I[1].yh; - //correct_brown_affin (x, y, ap[1], &x, &y); - ray_tracing_v2 (x,y, Ex[1], I[1], G[1], mmp, &X[1], &Y[1], &Z[1], &aa[1], &b[1], &c[1]); - } - if(crd[2][i].x>-999){ - x = crd[2][i].x - I[2].xh; - y = crd[2][i].y - I[2].yh; - //correct_brown_affin (x, y, ap[2], &x, &y); - ray_tracing_v2 (x,y, Ex[2], I[2], G[2], mmp, &X[2], &Y[2], &Z[2], &aa[2], &b[2], &c[2]); - } - if(crd[3][i].x>-999){ - x = crd[3][i].x - I[3].xh; - y = crd[3][i].y - I[3].yh; - //correct_brown_affin (x, y, ap[3], &x, &y); - ray_tracing_v2 (x,y, Ex[3], I[3], G[3], mmp, &X[3], &Y[3], &Z[3], &aa[3], &b[3], &c[3]); - } - - count_inner=0; - X_pos=0.;Y_pos=0.;Z_pos=0.; - for (n=0;n-999 && crd[m][i].x>-999){ - mid_point(X[n],Y[n],Z[n],aa[n],b[n],c[n],X[m],Y[m],Z[m],aa[m],b[m],c[m],&dist,&XX,&YY,&ZZ); - pX[count_inner]=XX; - pY[count_inner]=YY; - pZ[count_inner]=ZZ; + for (i=0; i-999){ + // x = crd[0][i].x - I[0].xh; + // y = crd[0][i].y - I[0].yh; + // //correct_brown_affin (x, y, ap[0], &x, &y); + // ray_tracing_v2 (x,y, Ex[0], I[0], G[0], mmp, &X[0], &Y[0], &Z[0], &aa[0], &b[0], &c[0]); + // } + // if(crd[1][i].x>-999) { + // x = crd[1][i].x - I[1].xh; + // y = crd[1][i].y - I[1].yh; + // //correct_brown_affin (x, y, ap[1], &x, &y); + // ray_tracing_v2 (x,y, Ex[1], I[1], G[1], mmp, &X[1], &Y[1], &Z[1], &aa[1], &b[1], &c[1]); + // } + // if(crd[2][i].x>-999) { + // x = crd[2][i].x - I[2].xh; + // y = crd[2][i].y - I[2].yh; + // //correct_brown_affin (x, y, ap[2], &x, &y); + // ray_tracing_v2 (x,y, Ex[2], I[2], G[2], mmp, &X[2], &Y[2], &Z[2], &aa[2], &b[2], &c[2]); + // } + // if(crd[3][i].x>-999){ + // x = crd[3][i].x - I[3].xh; + // y = crd[3][i].y - I[3].yh; + // //correct_brown_affin (x, y, ap[3], &x, &y); + // ray_tracing_v2 (x,y, Ex[3], I[3], G[3], mmp, &X[3], &Y[3], &Z[3], &aa[3], &b[3], &c[3]); + // } + // by + + // here comes the actual check!!!!! + for (j=0; j<4; j++) { + if (crd[j][i].x>-999) { + x = crd[j][i].x - I[j].xh; + y = crd[j][i].y - I[j].yh; + // correct_brown_affin (x, y, ap[0], &x, &y); + ray_tracing_v2 (x,y, Ex[j], I[j], G[j], mmp, &X[j], &Y[j], &Z[j], &aa[j], &b[j], &c[j]); + } + } + + count_inner=0; + X_pos=0.; Y_pos=0.; Z_pos=0.; + for (n=0; n-999 && crd[m][i].x>-999){ + mid_point(X[n],Y[n],Z[n],aa[n],b[n],c[n],X[m],Y[m],Z[m],aa[m],b[m],c[m],&dist,&XX,&YY,&ZZ); + pX[count_inner] = XX; + pY[count_inner] = YY; + pZ[count_inner] = ZZ; count_inner++; - d_inner += dist; - X_pos+=XX;Y_pos+=YY;Z_pos+=ZZ; - } - } - } - d_inner/=(double)count_inner; - X_pos/=(double)count_inner;Y_pos/=(double)count_inner;Z_pos/=(double)count_inner; - stdX=0;stdY=0;stdZ=0; - for (n=0;n \""); - if (examine == 4 && multi==0) - Tcl_Eval(interp, ".text insert 2 \"resection data written to disk \""); - Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); - Tcl_Eval(interp, ".text insert 3 $tbuf"); - - break; - - - case 11: puts ("Orientation from dumbbells"); strcpy(buf, ""); - - strcpy (safety[0], "safety_0"); - strcat (safety[0], ".ori"); - strcpy (safety[1], "safety_1"); - strcat (safety[1], ".ori"); - strcpy (safety[2], "safety_2"); - strcat (safety[2], ".ori"); - strcpy (safety[3], "safety_3"); - strcat (safety[3], ".ori"); - strcpy (safety_addpar[0], "safety_0"); - strcat (safety_addpar[0], ".addpar"); - strcpy (safety_addpar[1], "safety_1"); - strcat (safety_addpar[1], ".addpar"); - strcpy (safety_addpar[2], "safety_2"); - strcat (safety_addpar[2], ".addpar"); - strcpy (safety_addpar[3], "safety_3"); - strcat (safety_addpar[3], ".addpar"); - -for (i_img=0; i_img2){ - for (i=0; i2 - else{ - success=0; - } -}//loop through images - - -if(success==1){ - Tcl_Eval(interp, ".text delete 3"); - Tcl_Eval(interp, ".text delete 1"); - Tcl_Eval(interp, ".text insert 1 \"Orientation from dumbbells \""); - Tcl_Eval(interp, ".text delete 2"); - Tcl_Eval(interp, ".text insert 2 \"...done, sigma0 for each view -> \""); - Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); - Tcl_Eval(interp, ".text insert 3 $tbuf"); -} -else{ - Tcl_Eval(interp, ".text delete 3"); - Tcl_Eval(interp, ".text delete 1"); - Tcl_Eval(interp, ".text insert 1 \"Orientation from dummbbells \""); - Tcl_Eval(interp, ".text delete 2"); - Tcl_Eval(interp, ".text insert 2 \"...failed \""); - Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); - Tcl_Eval(interp, ".text insert 3 $tbuf"); -} - - break; + write_ori (Ex[i_img], I[i_img], G[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); + } //end of loop through images + + Tcl_Eval(interp, ".text delete 3"); + Tcl_Eval(interp, ".text delete 1"); + Tcl_Eval(interp, ".text insert 1 \"Orientation from particles \""); + Tcl_Eval(interp, ".text delete 2"); + if (examine != 4) + Tcl_Eval(interp, ".text insert 2 \"...done, sigma0 for each image -> \""); + if (examine == 4 && multi==0) + Tcl_Eval(interp, ".text insert 2 \"resection data written to disk \""); + Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); + Tcl_Eval(interp, ".text insert 3 $tbuf"); + break; - case 12: puts ("Orientation from dumbbells"); strcpy(buf, ""); - - prepare_eval(n_img,&nfix); //goes and looks up what sequence is defined and takes all cord. from rt_is - orient_v5 (n_img, nfix,&Ex, &I, &G, &ap); - - for(i_img=0;i_img=n_img; j--) + a1[j] = a2[j] = -1; + + // read pixel data according a0,a1,a2,a3 + pix[i_img][i].x = t4[3][i_img][a1[i_img]].x; + pix[i_img][i].y = t4[3][i_img][a1[i_img]].y; + pix[i_img][i].pnr = i; + fix[i].pnr = i; + pix[i_img][i+1].x = t4[3][i_img][a2[i_img]].x; + pix[i_img][i+1].y = t4[3][i_img][a2[i_img]].y; + pix[i_img][i+1].pnr = i+1; + fix[i+1].pnr = i+1; + + i += 2; + nfix = i; + } + fclose(FILEIN); + } //end of loop through seq, but loop i_img still open + printf("Using %d points for camera %d\n", nfix, i_img+1); + if (nfix>2) { + for (i=0; i2 + else { + success=0; + } + } //loop through images + + if (success==1) { + Tcl_Eval(interp, ".text delete 3"); + Tcl_Eval(interp, ".text delete 1"); + Tcl_Eval(interp, ".text insert 1 \"Orientation from dumbbells \""); + Tcl_Eval(interp, ".text delete 2"); + Tcl_Eval(interp, ".text insert 2 \"...done, sigma0 for each view -> \""); + Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); + Tcl_Eval(interp, ".text insert 3 $tbuf"); + } + else { + Tcl_Eval(interp, ".text delete 3"); + Tcl_Eval(interp, ".text delete 1"); + Tcl_Eval(interp, ".text insert 1 \"Orientation from dummbbells \""); + Tcl_Eval(interp, ".text delete 2"); + Tcl_Eval(interp, ".text insert 2 \"...failed \""); + Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); + Tcl_Eval(interp, ".text insert 3 $tbuf"); + } + break; - } + case 12: puts ("Orientation from dumbbells"); strcpy(buf, ""); + + prepare_eval(n_img,&nfix); //goes and looks up what sequence is defined and takes all cord. from rt_is + // orient_v5 (n_img, nfix, &Ex, &I, &G, &ap); corrected pointer errors, ad holten + orient_v5 (n_img, nfix, Ex, I, G, ap); + + for(i_img=0;i_img imx-imx/(2*zoom_f[n])) zoom_x[n] = imx-imx/(2*zoom_f[n]); - if (zoom_y[n] < imy/(2*zoom_f[n])) zoom_y[n] = imy/(2*zoom_f[n]); - if (zoom_y[n] > imy-imy/(2*zoom_f[n])) zoom_y[n] = imy-imy/(2*zoom_f[n]); - - zoom (img[n], zoomimg, zoom_x[n], zoom_y[n], zoom_f[n]); - - img_handle = Tk_FindPhoto( interp, "temp"); - Tk_PhotoGetImage (img_handle, &img_block); - tclimg2cimg (interp, zoomimg, &img_block); - - sprintf(val, "newimage %d", n+1); - Tcl_Eval(interp, val); - - break; - - - case 2: /* zoom images at corresponding position by factor zf */ - - if (n == 0) + switch (kind) { - /* zoom and show img1 */ - zoom_x[0] = (click_x-imx/2)/zoom_f[0] + zoom_x[0]; - zoom_y[0] = (click_y-imy/2)/zoom_f[0] + zoom_y[0]; - zoom_f[0] = zf; - - if (zoom_x[0] < imx/(2*zoom_f[0])) zoom_x[0] = imx/(2*zoom_f[0]); - if (zoom_x[0] > imx-imx/(2*zoom_f[0])) zoom_x[0] = imx-imx/(2*zoom_f[0]); - if (zoom_y[0] < imy/(2*zoom_f[0])) zoom_y[0] = imy/(2*zoom_f[0]); - if (zoom_y[0] > imy-imy/(2*zoom_f[0])) zoom_y[0] = imy-imy/(2*zoom_f[0]); - - zoom (img[0], zoomimg, zoom_x[0], zoom_y[0], zoom_f[0]); - - img_handle = Tk_FindPhoto( interp, "temp"); - Tk_PhotoGetImage (img_handle, &img_block); - tclimg2cimg (interp, zoomimg, &img_block); - - sprintf(val, "newimage %d", 1); - Tcl_Eval(interp, val); +/* -------------------------- MIDDLE MOUSE BUTTON ---------------------------------- */ + case 1: + zoom_x[n] = (click_x - imx/2)/zoom_f[n] + zoom_x[n]; + zoom_y[n] = (click_y - imy/2)/zoom_f[n] + zoom_y[n]; + zoom_f[n] *= 2; + + if (zoom_x[n] < imx/(2*zoom_f[n])) zoom_x[n] = imx/(2*zoom_f[n]); + if (zoom_x[n] > imx-imx/(2*zoom_f[n])) zoom_x[n] = imx-imx/(2*zoom_f[n]); + if (zoom_y[n] < imy/(2*zoom_f[n])) zoom_y[n] = imy/(2*zoom_f[n]); + if (zoom_y[n] > imy-imy/(2*zoom_f[n])) zoom_y[n] = imy-imy/(2*zoom_f[n]); + + zoom (img[n], zoomimg, zoom_x[n], zoom_y[n], zoom_f[n]); + + img_handle = Tk_FindPhoto( interp, "temp"); + Tk_PhotoGetImage (img_handle, &img_block); + tclimg2cimg (interp, zoomimg, &img_block); + + sprintf(val, "newimage %d", n+1); + Tcl_Eval(interp, val); + + break; + - if (argc != 6 ) { - mark_detections (interp, 0); - mark_correspondences (interp, 0); - mark_corr (interp, 0); - } - - /* transform center of zoomed img1 */ - x = (double) zoom_x[0]; y = (double) zoom_y[0]; - pixel_to_metric (x,y, imx,imy, pix_x,pix_y, &x,&y, chfield); - x -= I[n].xh; y -= I[n].yh; - correct_brown_affin (x, y, ap[0], &x, &y); + case 2: /* zoom images at corresponding position by factor zf */ - for (i=1; i imx - imx/(2*zoom_f[i])) - zoom_x[i] = imx - imx/(2*zoom_f[i]); - if (zoom_y[i] < imy/(2*zoom_f[i])) - zoom_y[i] = imy/(2*zoom_f[i]); - if (zoom_y[i] > imy - imy/(2*zoom_f[i])) - zoom_y[i] = imy - imy/(2*zoom_f[i]); - zoom (img[i], zoomimg, zoom_x[i],zoom_y[i],zoom_f[i]); - - img_handle = Tk_FindPhoto( interp, "temp"); - Tk_PhotoGetImage (img_handle, &img_block); - tclimg2cimg (interp, zoomimg, &img_block); - - sprintf(val, "newimage %d", i+1); - Tcl_Eval(interp, val); - - if (argc != 6 ) { - mark_detections (interp, i); - mark_correspondences (interp, i); - mark_corr (interp, i); - } - - } - } else { - sprintf(val,"Zooming of corresp. areas is only possible by clicking in image 1"); - Tcl_SetVar(interp, "tbuf", val, TCL_GLOBAL_ONLY); - Tcl_Eval(interp, ".text delete 2"); - Tcl_Eval(interp, ".text insert 2 $tbuf"); - } - break; - - - case 6: - - /* Zoom with high zf for tracking */ - zoom_x[n] = (click_x - imx/2)/zoom_f[n] + zoom_x[n]; - zoom_y[n] = (click_y - imy/2)/zoom_f[n] + zoom_y[n]; - zoom_f[n] = atoi(argv[5]); - - if (zoom_x[n] < imx/(2*zoom_f[n])) zoom_x[n] = imx/(2*zoom_f[n]); - if (zoom_x[n] > imx-imx/(2*zoom_f[n])) zoom_x[n] = imx-imx/(2*zoom_f[n]); - if (zoom_y[n] < imy/(2*zoom_f[n])) zoom_y[n] = imy/(2*zoom_f[n]); - if (zoom_y[n] > imy-imy/(2*zoom_f[n])) zoom_y[n] = imy-imy/(2*zoom_f[n]); - - zoom (img[n], zoomimg, zoom_x[n], zoom_y[n], zoom_f[n]); - - img_handle = Tk_FindPhoto( interp, "temp"); - Tk_PhotoGetImage (img_handle, &img_block); - tclimg2cimg (interp, zoomimg, &img_block); - - sprintf(val, "newimage %d", n+1); - Tcl_Eval(interp, val); - - break; -/*----------------------- RIGHT MOUSE BUTTON ------------------------------*/ - - case 3: /* generate epipolar line segments */ - - /* get geometric coordinates of nearest point in img[n] */ - x = (float) (click_x - imx/2)/zoom_f[n] + zoom_x[n]; - y = (float) (click_y - imy/2)/zoom_f[n] + zoom_y[n]; - - pixel_to_metric (x,y, imx,imy, pix_x,pix_y, &x,&y, chfield); - 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) - { - sprintf (buf, "no point near click coord ! Click again!"); puts (buf); - Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); - Tcl_Eval(interp, ".text delete 2"); - Tcl_Eval(interp, ".text insert 2 $tbuf"); - return TCL_OK; - } - pt1 = geo[n][k].pnr; - - intx1 = (int) ( imx/2 + zoom_f[n] * (pix[n][pt1].x-zoom_x[n])); - inty1 = (int) ( imy/2 + zoom_f[n] * (pix[n][pt1].y-zoom_y[n])); - drawcross (interp, intx1, inty1, cr_sz+2, n, "BlueViolet"); - - sprintf (buf, "%d %d %d %d %d\n", pt1, pix[n][pt1].nx, pix[n][pt1].ny, - pix[n][pt1].n, pix[n][pt1].sumg); puts (buf); - - for (i=0; i imx-imx/(2*zoom_f[0])) zoom_x[0] = imx-imx/(2*zoom_f[0]); + if (zoom_y[0] < imy/(2*zoom_f[0])) zoom_y[0] = imy/(2*zoom_f[0]); + if (zoom_y[0] > imy-imy/(2*zoom_f[0])) zoom_y[0] = imy-imy/(2*zoom_f[0]); + + zoom (img[0], zoomimg, zoom_x[0], zoom_y[0], zoom_f[0]); + + img_handle = Tk_FindPhoto( interp, "temp"); + Tk_PhotoGetImage (img_handle, &img_block); + tclimg2cimg (interp, zoomimg, &img_block); + + sprintf(val, "newimage %d", 1); + Tcl_Eval(interp, val); + + if (argc != 6 ) { + mark_detections (interp, 0); + mark_correspondences (interp, 0); + mark_corr (interp, 0); + } + + /* transform center of zoomed img1 */ + x = (double) zoom_x[0]; y = (double) zoom_y[0]; + pixel_to_metric (x,y, imx,imy, pix_x,pix_y, &x,&y, chfield); + x -= I[n].xh; y -= I[n].yh; + correct_brown_affin (x, y, ap[0], &x, &y); + + for (i=1; i imx - imx/(2*zoom_f[i])) + zoom_x[i] = imx - imx/(2*zoom_f[i]); + if (zoom_y[i] < imy/(2*zoom_f[i])) + zoom_y[i] = imy/(2*zoom_f[i]); + if (zoom_y[i] > imy - imy/(2*zoom_f[i])) + zoom_y[i] = imy - imy/(2*zoom_f[i]); + zoom (img[i], zoomimg, zoom_x[i],zoom_y[i],zoom_f[i]); + + img_handle = Tk_FindPhoto( interp, "temp"); + Tk_PhotoGetImage (img_handle, &img_block); + tclimg2cimg (interp, zoomimg, &img_block); + + sprintf(val, "newimage %d", i+1); + Tcl_Eval(interp, val); + + if (argc != 6 ) { + mark_detections (interp, i); + mark_correspondences (interp, i); + mark_corr (interp, i); + } + } + } + else { + sprintf(val,"Zooming of corresp. areas is only possible by clicking in image 1"); + Tcl_SetVar(interp, "tbuf", val, TCL_GLOBAL_ONLY); + Tcl_Eval(interp, ".text delete 2"); + Tcl_Eval(interp, ".text insert 2 $tbuf"); + } + break; + + + case 6: + /* Zoom with high zf for tracking */ + zoom_x[n] = (click_x - imx/2)/zoom_f[n] + zoom_x[n]; + zoom_y[n] = (click_y - imy/2)/zoom_f[n] + zoom_y[n]; + zoom_f[n] = atoi(argv[5]); + + if (zoom_x[n] < imx/(2*zoom_f[n])) zoom_x[n] = imx/(2*zoom_f[n]); + if (zoom_x[n] > imx-imx/(2*zoom_f[n])) zoom_x[n] = imx-imx/(2*zoom_f[n]); + if (zoom_y[n] < imy/(2*zoom_f[n])) zoom_y[n] = imy/(2*zoom_f[n]); + if (zoom_y[n] > imy-imy/(2*zoom_f[n])) zoom_y[n] = imy-imy/(2*zoom_f[n]); + + zoom (img[n], zoomimg, zoom_x[n], zoom_y[n], zoom_f[n]); + + img_handle = Tk_FindPhoto( interp, "temp"); + Tk_PhotoGetImage (img_handle, &img_block); + tclimg2cimg (interp, zoomimg, &img_block); + + sprintf(val, "newimage %d", n+1); + Tcl_Eval(interp, val); + + break; +/*----------------------- RIGHT MOUSE BUTTON ------------------------------*/ + case 3: /* generate epipolar line segments */ + /* get geometric coordinates of nearest point in img[n] */ + x = (float) (click_x - imx/2)/zoom_f[n] + zoom_x[n]; + y = (float) (click_y - imy/2)/zoom_f[n] + zoom_y[n]; + + pixel_to_metric (x,y, imx,imy, pix_x,pix_y, &x,&y, chfield); + 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) { + sprintf (buf, "no point near click coord ! Click again!"); puts (buf); + Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); + Tcl_Eval(interp, ".text delete 2"); + Tcl_Eval(interp, ".text insert 2 $tbuf"); + return TCL_OK; + } + pt1 = geo[n][k].pnr; + + intx1 = (int) ( imx/2 + zoom_f[n] * (pix[n][pt1].x-zoom_x[n])); + inty1 = (int) ( imy/2 + zoom_f[n] * (pix[n][pt1].y-zoom_y[n])); + drawcross (interp, intx1, inty1, cr_sz+2, n, "BlueViolet"); + + sprintf (buf, "%d %d %d %d %d\n", pt1, pix[n][pt1].nx, pix[n][pt1].ny, + pix[n][pt1].n, pix[n][pt1].sumg); puts (buf); + + for (i=0; i imx-imx/(2*zoom_f[n])) zoom_x[n] = imx-imx/(2*zoom_f[n]); - if (zoom_y[n] < imy/(2*zoom_f[n])) zoom_y[n] = imy/(2*zoom_f[n]); - if (zoom_y[n] > imy-imy/(2*zoom_f[n])) zoom_y[n] = imy-imy/(2*zoom_f[n]); - - zoom (img[n], zoomimg, zoom_x[n], zoom_y[n], zoom_f[n]); - - img_handle = Tk_FindPhoto( interp, "temp"); - Tk_PhotoGetImage (img_handle, &img_block); - tclimg2cimg (interp, zoomimg, &img_block); + case 1: + + zoom_x[n] = (click_x - imx/2)/zoom_f[n] + zoom_x[n]; + zoom_y[n] = (click_y - imy/2)/zoom_f[n] + zoom_y[n]; + zoom_f[n] *= 2; + + if (zoom_x[n] < imx/(2*zoom_f[n])) zoom_x[n] = imx/(2*zoom_f[n]); + if (zoom_x[n] > imx-imx/(2*zoom_f[n])) zoom_x[n] = imx-imx/(2*zoom_f[n]); + if (zoom_y[n] < imy/(2*zoom_f[n])) zoom_y[n] = imy/(2*zoom_f[n]); + if (zoom_y[n] > imy-imy/(2*zoom_f[n])) zoom_y[n] = imy-imy/(2*zoom_f[n]); + + zoom (img[n], zoomimg, zoom_x[n], zoom_y[n], zoom_f[n]); + + img_handle = Tk_FindPhoto( interp, "temp"); + Tk_PhotoGetImage (img_handle, &img_block); + tclimg2cimg (interp, zoomimg, &img_block); - sprintf(val, "newimage %d", n+1); - Tcl_Eval(interp, val); + sprintf(val, "newimage %d", n+1); + Tcl_Eval(interp, val); - break; - - - case 2: /* zoom images at corresponding position by factor zf */ - - if (n == 0) + break; + + + case 2: /* zoom images at corresponding position by factor zf */ + + if (n == 0) { /* zoom and show img1 */ zoom_x[0] = (click_x-imx/2)/zoom_f[0] + zoom_x[0]; @@ -68,104 +68,104 @@ int mouse_proc_c (ClientData clientData, Tcl_Interp* interp, int argc, const cha img_handle = Tk_FindPhoto( interp, "temp"); Tk_PhotoGetImage (img_handle, &img_block); tclimg2cimg (interp, zoomimg, &img_block); - + sprintf(val, "newimage %d", 1); Tcl_Eval(interp, val); if (argc != 6 ) { - mark_detections (interp, 0); - mark_correspondences (interp, 0); - mark_corr (interp, 0); + mark_detections (interp, 0); + mark_correspondences (interp, 0); + mark_corr (interp, 0); } /* transform center of zoomed img1 */ - x = (double) zoom_x[0]; y = (double) zoom_y[0]; + x = (double) zoom_x[0]; y = (double) zoom_y[0]; pixel_to_metric (x,y, imx,imy, pix_x,pix_y, &x,&y, chfield); x -= I[n].xh; y -= I[n].yh; correct_brown_affin (x, y, ap[0], &x, &y); for (i=1; i imx - imx/(2*zoom_f[i])) + if (zoom_x[i] > imx - imx/(2*zoom_f[i])) zoom_x[i] = imx - imx/(2*zoom_f[i]); - if (zoom_y[i] < imy/(2*zoom_f[i])) + if (zoom_y[i] < imy/(2*zoom_f[i])) zoom_y[i] = imy/(2*zoom_f[i]); - if (zoom_y[i] > imy - imy/(2*zoom_f[i])) + if (zoom_y[i] > imy - imy/(2*zoom_f[i])) zoom_y[i] = imy - imy/(2*zoom_f[i]); - zoom (img[i], zoomimg, zoom_x[i],zoom_y[i],zoom_f[i]); + zoom (img[i], zoomimg, zoom_x[i],zoom_y[i],zoom_f[i]); - img_handle = Tk_FindPhoto( interp, "temp"); - Tk_PhotoGetImage (img_handle, &img_block); - tclimg2cimg (interp, zoomimg, &img_block); - - sprintf(val, "newimage %d", i+1); - Tcl_Eval(interp, val); - - if (argc != 6 ) { - mark_detections (interp, i); + img_handle = Tk_FindPhoto( interp, "temp"); + Tk_PhotoGetImage (img_handle, &img_block); + tclimg2cimg (interp, zoomimg, &img_block); + + sprintf(val, "newimage %d", i+1); + Tcl_Eval(interp, val); + + if (argc != 6 ) { + mark_detections (interp, i); mark_correspondences (interp, i); mark_corr (interp, i); - } - - } + } + + } } else { sprintf(val,"Zooming of corresp. areas is only possible by clicking in image 1"); Tcl_SetVar(interp, "tbuf", val, TCL_GLOBAL_ONLY); Tcl_Eval(interp, ".text delete 2"); Tcl_Eval(interp, ".text insert 2 $tbuf"); } - break; + break; - case 6: - - /* Zoom with high zf for tracking */ - zoom_x[n] = (click_x - imx/2)/zoom_f[n] + zoom_x[n]; - zoom_y[n] = (click_y - imy/2)/zoom_f[n] + zoom_y[n]; - zoom_f[n] = atoi(argv[5]); + case 6: + + /* Zoom with high zf for tracking */ + zoom_x[n] = (click_x - imx/2)/zoom_f[n] + zoom_x[n]; + zoom_y[n] = (click_y - imy/2)/zoom_f[n] + zoom_y[n]; + zoom_f[n] = atoi(argv[5]); - if (zoom_x[n] < imx/(2*zoom_f[n])) zoom_x[n] = imx/(2*zoom_f[n]); - if (zoom_x[n] > imx-imx/(2*zoom_f[n])) zoom_x[n] = imx-imx/(2*zoom_f[n]); - if (zoom_y[n] < imy/(2*zoom_f[n])) zoom_y[n] = imy/(2*zoom_f[n]); - if (zoom_y[n] > imy-imy/(2*zoom_f[n])) zoom_y[n] = imy-imy/(2*zoom_f[n]); - - zoom (img[n], zoomimg, zoom_x[n], zoom_y[n], zoom_f[n]); - - img_handle = Tk_FindPhoto( interp, "temp"); - Tk_PhotoGetImage (img_handle, &img_block); - tclimg2cimg (interp, zoomimg, &img_block); - - sprintf(val, "newimage %d", n+1); - Tcl_Eval(interp, val); + if (zoom_x[n] < imx/(2*zoom_f[n])) zoom_x[n] = imx/(2*zoom_f[n]); + if (zoom_x[n] > imx-imx/(2*zoom_f[n])) zoom_x[n] = imx-imx/(2*zoom_f[n]); + if (zoom_y[n] < imy/(2*zoom_f[n])) zoom_y[n] = imy/(2*zoom_f[n]); + if (zoom_y[n] > imy-imy/(2*zoom_f[n])) zoom_y[n] = imy-imy/(2*zoom_f[n]); + + zoom (img[n], zoomimg, zoom_x[n], zoom_y[n], zoom_f[n]); + + img_handle = Tk_FindPhoto( interp, "temp"); + Tk_PhotoGetImage (img_handle, &img_block); + tclimg2cimg (interp, zoomimg, &img_block); + + sprintf(val, "newimage %d", n+1); + Tcl_Eval(interp, val); - break; + break; /*----------------------- RIGHT MOUSE BUTTON ------------------------------*/ - case 3: /* generate epipolar line segments */ - - /* get geometric coordinates of nearest point in img[n] */ - x = (float) (click_x - imx/2)/zoom_f[n] + zoom_x[n]; - y = (float) (click_y - imy/2)/zoom_f[n] + zoom_y[n]; + case 3: /* generate epipolar line segments */ + + /* get geometric coordinates of nearest point in img[n] */ + x = (float) (click_x - imx/2)/zoom_f[n] + zoom_x[n]; + y = (float) (click_y - imy/2)/zoom_f[n] + zoom_y[n]; - pixel_to_metric (x,y, imx,imy, pix_x,pix_y, &x,&y, chfield); - 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) + pixel_to_metric (x,y, imx,imy, pix_x,pix_y, &x,&y, chfield); + 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) { sprintf (buf, "no point near click coord ! Click again!"); puts (buf); Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); @@ -173,68 +173,68 @@ int mouse_proc_c (ClientData clientData, Tcl_Interp* interp, int argc, const cha Tcl_Eval(interp, ".text insert 2 $tbuf"); return TCL_OK; } - pt1 = geo[n][k].pnr; + pt1 = geo[n][k].pnr; - intx1 = (int) ( imx/2 + zoom_f[n] * (pix[n][pt1].x-zoom_x[n])); - inty1 = (int) ( imy/2 + zoom_f[n] * (pix[n][pt1].y-zoom_y[n])); - drawcross (interp, intx1, inty1, cr_sz+2, n, "BlueViolet"); + intx1 = (int) ( imx/2 + zoom_f[n] * (pix[n][pt1].x-zoom_x[n])); + inty1 = (int) ( imy/2 + zoom_f[n] * (pix[n][pt1].y-zoom_y[n])); + drawcross (interp, intx1, inty1, cr_sz+2, n, "BlueViolet"); - sprintf (buf, "%d %d %d %d %d\n", pt1, pix[n][pt1].nx, pix[n][pt1].ny, - pix[n][pt1].n, pix[n][pt1].sumg); puts (buf); - - for (i=0; i X,Y=shifts +1 point, exterior orientation, multimedia parameters ==> X,Y=shifts (special case 3 media, medium 2 = plane parallel) -Routines contained: - +Routines contained: - ****************************************************************************/ + +// Repaired 'pointer to array' errors, ad holten, 12-2012 +// In all the calls to trans_Cam_Point() the & is stripped of from the arguments +// &cross_p and &cross_c. (lines: 699 711 727 737 753 763 780 790 808 831) + #include "ptv.h" double get_mmf_from_mmLUT (); - -void multimed (ex,mm,X,Y,Z,Xq,Yq) -Exterior ex; -mm_3p mm; -double X, Y, Z, *Xq,*Yq; +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +void multimed (Exterior ex, mm_3p mm, double X, double Y, double Z, + double *Xq, double *Yq) { - int i, it=0; - double beta1, beta2, beta3, r, rbeta, rdiff, rq, mmf; - double ocf=1.0; /* over compensation factor for faster convergence */ + int i, it=0; + double beta1, beta2, beta3, r, rbeta, rdiff, rq, mmf; + double ocf=1.0; /* over compensation factor for faster convergence */ - if (mm.n1==1 && mm.n2==1 && mm.n3==1) /* 1-medium case */ - { - *Xq = X; *Yq = Y; return; - } - - /* interpolation in mmLUT, if selected (requires some global variables) */ - if (mm.lut) - { - /* check, which is the correct image */ - for (i=0; i 0) - { - *Xq = ex.x0 + (X-ex.x0) * mmf; - *Yq = ex.y0 + (Y-ex.y0) * mmf; - return; + if (mm.n1==1 && mm.n2==1 && mm.n3==1) { /* 1-medium case */ + *Xq = X; + *Yq = Y; + return; + } + + /* interpolation in mmLUT, if selected (requires some global variables) */ + if (mm.lut) { + /* check, which is the correct image */ + for (i=0; i 0) { + *Xq = ex.x0 + (X-ex.x0) * mmf; + *Yq = ex.y0 + (Y-ex.y0) * mmf; + return; + } } - } - - /* iterative procedure */ - - r = sqrt ((X-ex.x0)*(X-ex.x0)+(Y-ex.y0)*(Y-ex.y0)); - rq = r; - - do - { - beta1 = atan (rq/(ex.z0-Z)); - beta2 = asin (sin(beta1) * mm.n1/mm.n2); - beta3 = asin (sin(beta1) * mm.n1/mm.n3); - - rbeta = (ex.z0-mm.d) * tan(beta1) + mm.d * tan(beta2) - Z * tan(beta3); - rdiff = r - rbeta; - rdiff *= ocf; - rq += rdiff; - it++; - } - while (((rdiff > 0.001) || (rdiff < -0.001)) && it < 40); - if (it >= 40) - { - puts ("Multimed stopped after 40. Iteration"); - if (fabs(rdiff) < 0.1) - { - *Xq = ex.x0 + (X-ex.x0) * rq/r; - *Yq = ex.y0 + (Y-ex.y0) * rq/r; + /* iterative procedure */ + r = sqrt ((X-ex.x0)*(X-ex.x0)+(Y-ex.y0)*(Y-ex.y0)); + rq = r; + do { + beta1 = atan (rq/(ex.z0-Z)); + beta2 = asin (sin(beta1) * mm.n1/mm.n2); + beta3 = asin (sin(beta1) * mm.n1/mm.n3); + + rbeta = (ex.z0-mm.d) * tan(beta1) + mm.d * tan(beta2) - Z * tan(beta3); + rdiff = r - rbeta; + rdiff *= ocf; + rq += rdiff; + it++; + } while ((rdiff > 0.001 || rdiff < -0.001) && it < 40); + + if (it >= 40) { + puts ("Multimed stopped after 40. Iteration"); + if (fabs(rdiff) < 0.1) { + *Xq = ex.x0 + (X-ex.x0) * rq/r; + *Yq = ex.y0 + (Y-ex.y0) * rq/r; + } + else { + *Xq = X; + *Yq = Y; + } + return; } - else - { - *Xq = X; *Yq = Y; + if (r != 0) { + *Xq = ex.x0 + (X-ex.x0) * rq/r; + *Yq = ex.y0 + (Y-ex.y0) * rq/r; + } + else { + *Xq = X; + *Yq = Y; } - return; - } - - if (r != 0) - { - *Xq = ex.x0 + (X-ex.x0) * rq/r; - *Yq = ex.y0 + (Y-ex.y0) * rq/r; - } - else - { - *Xq = X; - *Yq = Y; - } } +#endif - - -double multimed_r (ex,mm,X,Y,Z) +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +double multimed_r (Exterior ex, mm_3p mm, double X, double Y, double Z) /* calculates and returns the radial shift */ -Exterior ex; -mm_3p mm; -double X, Y, Z; { - int i, it=0; - double beta1, beta2, beta3, r, rbeta, rdiff, rq, mmf; - double ocf=1.0; /* over compensation factor for faster convergence */ - - if (mm.n1==1 && mm.n2==1 && mm.n3==1) return (1.0); /* 1-medium case */ - - /* interpolation in mmLUT, if selected (requires some global variables) */ - if (mm.lut) - { - /* check, which is the correct image */ - for (i=0; i 0) return (mmf); - } - - /* iterative procedure */ - r = sqrt ((X-ex.x0)*(X-ex.x0)+(Y-ex.y0)*(Y-ex.y0)); - rq = r; - - do - { - beta1 = atan (rq/(ex.z0-Z)); - beta2 = asin (sin(beta1) * mm.n1/mm.n2); - beta3 = asin (sin(beta1) * mm.n1/mm.n3); - - rbeta = (ex.z0-mm.d) * tan(beta1) + mm.d * tan(beta2) - Z * tan(beta3); - rdiff = r - rbeta; - rdiff *= ocf; - rq += rdiff; - it++; - } - while (((rdiff > 0.001) || (rdiff < -0.001)) && it < 40); - - if (it >= 40) - { - puts ("Multimed_r stopped after 40. Iteration"); return (1.0); - } - - if (r != 0) return (rq/r); else return (1.0); -} + int i, it=0; + double beta1, beta2, beta3, r, rbeta, rdiff, rq, mmf; + double ocf=1.0; /* over compensation factor for faster convergence */ + + if (mm.n1==1 && mm.n2==1 && mm.n3==1) + return (1.0); /* 1-medium case */ + + /* interpolation in mmLUT, if selected (requires some global variables) */ + if (mm.lut) { + /* check, which is the correct image */ + for (i=0; i 0) + return (mmf); + } + /* iterative procedure */ + r = sqrt ((X-ex.x0)*(X-ex.x0)+(Y-ex.y0)*(Y-ex.y0)); + rq = r; + + do { + beta1 = atan (rq/(ex.z0-Z)); + beta2 = asin (sin(beta1) * mm.n1/mm.n2); + beta3 = asin (sin(beta1) * mm.n1/mm.n3); + + rbeta = (ex.z0-mm.d) * tan(beta1) + mm.d * tan(beta2) - Z * tan(beta3); + rdiff = r - rbeta; + rdiff *= ocf; + rq += rdiff; + it++; + } while (((rdiff > 0.001) || (rdiff < -0.001)) && it < 40); + + if (it >= 40) { + puts ("Multimed_r stopped after 40. Iteration"); + return (1.0); + } + if (r != 0) + return (rq/r); + else + return (1.0); +} +#endif -void multimed_nlay_true (ex,gl,mm,X,Y,Z,Xq,Yq) -Exterior ex; -Glass gl; -mm_np mm; -double X, Y, Z, *Xq,*Yq; +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +void multimed_nlay_true (Exterior ex, Glass gl,mm_np mm, + double X, double Y, double Z, double *Xq, double *Yq) { - int i, it=0; - double beta1, beta2[32], beta3, r, rbeta, rdiff, rq, mmf; - double ocf=1.00; /* over compensation factor for faster convergence */ - - /* interpolation in mmLUT, if selected (requires some global variables) */ - if (mm.lut) - { - /* check, which is the correct image */ - for (i=0; i 0) - { - *Xq = ex.x0 + (X-ex.x0) * mmf; - *Yq = ex.y0 + (Y-ex.y0) * mmf; - return; + int i, it=0; + double beta1, beta2[32], beta3, r, rbeta, rdiff, rq, mmf; + double ocf=1.00; /* over compensation factor for faster convergence */ + + /* interpolation in mmLUT, if selected (requires some global variables) */ + if (mm.lut) { + /* check, which is the correct image */ + for (i=0; i 0) { + *Xq = ex.x0 + (X-ex.x0) * mmf; + *Yq = ex.y0 + (Y-ex.y0) * mmf; + return; + } } - } - - /* iterative procedure */ - r = sqrt ((X-ex.x0)*(X-ex.x0)+(Y-ex.y0)*(Y-ex.y0)); - rq = r; - - do - { - beta1 = atan (rq/(ex.z0-Z)); - for (i=0; i 0.001) || (rdiff < -0.001)) && it < 40); - - if (it >= 40) - { - *Xq = X; *Yq = Y; - puts ("Multimed_nlay_true stopped after 40. Iteration"); return; - } - - if (r != 0) - { - *Xq = ex.x0 + (X-ex.x0) * rq/r; - *Yq = ex.y0 + (Y-ex.y0) * rq/r; - } - else - { - *Xq = X; - *Yq = Y; - } -} + /* iterative procedure */ + r = sqrt ((X-ex.x0)*(X-ex.x0)+(Y-ex.y0)*(Y-ex.y0)); + rq = r; + + do { + beta1 = atan (rq/(ex.z0-Z)); + for (i=0; i 0.001) || (rdiff < -0.001)) && it < 40); + + if (it >= 40) { + *Xq = X; *Yq = Y; + puts ("Multimed_nlay_true stopped after 40. Iteration"); + return; + } + if (r != 0) { + *Xq = ex.x0 + (X-ex.x0) * rq/r; + *Yq = ex.y0 + (Y-ex.y0) * rq/r; + } + else { + *Xq = X; + *Yq = Y; + } +} +#endif -void multimed_nlay (ex,mm,X,Y,Z,Xq,Yq) -Exterior ex; -mm_np mm; -double X, Y, Z, *Xq,*Yq; +void multimed_nlay (Exterior ex, mm_np mm, + double X, double Y, double Z, double *Xq, double *Yq) { - int i, it=0; - double beta1, beta2[32], beta3, r, rbeta, rdiff, rq, mmf; - - /* interpolation in mmLUT, if selected (requires some global variables) */ - if (mm.lut) - { - /* check, which is the correct image */ - for (i=0; i 0) - { - *Xq = ex.x0 + (X-ex.x0) * mmf; - *Yq = ex.y0 + (Y-ex.y0) * mmf; - return; + int i, it=0; + double beta1, beta2[32], beta3, r, rbeta, rdiff, rq, mmf; + + /* interpolation in mmLUT, if selected (requires some global variables) */ + if (mm.lut) { + /* check, which is the correct image */ + for (i=0; i 0) { + *Xq = ex.x0 + (X-ex.x0) * mmf; + *Yq = ex.y0 + (Y-ex.y0) * mmf; + return; + } + } + + /* iterative procedure (if mmLUT does not exist or has no entry) */ + r = sqrt ((X-ex.x0)*(X-ex.x0)+(Y-ex.y0)*(Y-ex.y0)); + rq = r; + + do { + beta1 = atan (rq/(ex.z0-Z)); + for (i=0; i 0.001 || rdiff < -0.001) && it < 40); + + if (it >= 40) { + *Xq = X; *Yq = Y; + puts ("Multimed_nlay stopped after 40. Iteration"); + return; + } + + if (r != 0) { + *Xq = ex.x0 + (X-ex.x0) * rq/r; + *Yq = ex.y0 + (Y-ex.y0) * rq/r; + } + else { + *Xq = X; + *Yq = Y; } - } - - /* iterative procedure (if mmLUT does not exist or has no entry) */ - r = sqrt ((X-ex.x0)*(X-ex.x0)+(Y-ex.y0)*(Y-ex.y0)); - rq = r; - - do - { - beta1 = atan (rq/(ex.z0-Z)); - for (i=0; i 0.001) || (rdiff < -0.001)) && it < 40); - - if (it >= 40) - { - *Xq = X; *Yq = Y; - puts ("Multimed_nlay stopped after 40. Iteration"); return; - } - - if (r != 0) - { - *Xq = ex.x0 + (X-ex.x0) * rq/r; - *Yq = ex.y0 + (Y-ex.y0) * rq/r; - } - else - { - *Xq = X; - *Yq = Y; - } } -void multimed_nlay_v2 (ex,ex_o,mm,X,Y,Z,Xq,Yq) -Exterior ex; -Exterior ex_o; -mm_np mm; -double X, Y, Z, *Xq,*Yq; +void multimed_nlay_v2 (Exterior ex, Exterior ex_o, mm_np mm, + double X, double Y, double Z, double *Xq, double *Yq) { - - //Beat Lüthi, Nov 2007 comment actually only Xq is affected since all Y and Yq are always zero - int i, it=0; - double beta1, beta2[32], beta3, r, rbeta, rdiff, rq, mmf; - - // interpolation in mmLUT, if selected (requires some global variables) - if (mm.lut) - { - // check, which is the correct image - for (i=0; i 0) - { - *Xq = ex.x0 + (X-ex.x0) * mmf; - *Yq = ex.y0 + (Y-ex.y0) * mmf; - return; + // Beat L¸thi, Nov 2007 comment actually only Xq is affected since all Y and Yq are always zero + int i, it=0; + double beta1, beta2[32], beta3, r, rbeta, rdiff, rq, mmf; + + // interpolation in mmLUT, if selected (requires some global variables) + if (mm.lut) { + // check, which is the correct image + for (i=0; i 0) { + *Xq = ex.x0 + (X-ex.x0) * mmf; + *Yq = ex.y0 + (Y-ex.y0) * mmf; + return; + } } - } - - // iterative procedure (if mmLUT does not exist or has no entry) - r = sqrt ((X-ex.x0)*(X-ex.x0)+(Y-ex.y0)*(Y-ex.y0)); - rq = r; - - do - { - beta1 = atan (rq/(ex.z0-Z)); - for (i=0; i 0.001) || (rdiff < -0.001)) && it < 40); - - if (it >= 40) - { - *Xq = X; *Yq = Y; - puts ("Multimed_nlay stopped after 40. Iteration"); return; - } - - if (r != 0) - { - *Xq = ex.x0 + (X-ex.x0) * rq/r; - *Yq = ex.y0 + (Y-ex.y0) * rq/r; - } - else - { - *Xq = X; - *Yq = Y; - } - - // + // iterative procedure (if mmLUT does not exist or has no entry) + r = sqrt ((X-ex.x0)*(X-ex.x0)+(Y-ex.y0)*(Y-ex.y0)); + rq = r; + + do { + beta1 = atan (rq/(ex.z0-Z)); + for (i=0; i 0.001 || rdiff < -0.001) && it < 40); + + if (it >= 40) { + *Xq = X; *Yq = Y; + puts ("Multimed_nlay stopped after 40. Iteration"); + return; + } + + if (r != 0) { + *Xq = ex.x0 + (X-ex.x0) * rq/r; + *Yq = ex.y0 + (Y-ex.y0) * rq/r; + } + else { + *Xq = X; + *Yq = Y; + } -//----------------------------------------------------------------------------------------- -//----------------------------------------------------------------------------------------- -//----------------------------------------------------------------------------------------- + //----------------------------------------------------------------------------------------- + //----------------------------------------------------------------------------------------- + //----------------------------------------------------------------------------------------- - //Beat Lüthi, Nov 2007, I will implement the Risö method to avoid all this sin, atan etc - + //Beat L¸thi, Nov 2007, I will implement the Risˆ method to avoid all this sin, atan etc - /*double absError=100; - int counter=0; - double dir_water_x=-X; - double dir_water_z=ex.z0-Z; - double dist=pow(dir_water_x*dir_water_x+dir_water_z*dir_water_z,0.5); - double xInInterFace,comp_parallel,comp_perpendicular,dir_air_x,dir_air_z,error_x,error_z; - dir_water_x=dir_water_x/dist; - dir_water_z=dir_water_z/dist; - - while(absError>0.00001 && counter<40){ - //sending the water_ray to air + /*double absError=100; + int counter=0; + double dir_water_x=-X; + double dir_water_z=ex.z0-Z; + double dist=pow(dir_water_x*dir_water_x+dir_water_z*dir_water_z,0.5); + double xInInterFace,comp_parallel,comp_perpendicular,dir_air_x,dir_air_z,error_x,error_z; + dir_water_x=dir_water_x/dist; + dir_water_z=dir_water_z/dist; + + while(absError>0.00001 && counter<40){ + //sending the water_ray to air xInInterFace=X-Z/dir_water_z*dir_water_x; // //snellNormal_y=cross(dir_water,interF.base[2]); //snellNormal=snellNormal/norm(snellNormal); @@ -386,101 +346,94 @@ double X, Y, Z, *Xq,*Yq; //measuring how much cam_origo is missed and correcting water_ray //error=pCamOrigo-xInInterFace-dot(pCamOrigo-xInInterFace,dir_air)*dir_air/norm2(dir_air); error_x=ex.x0-xInInterFace-((ex.x0-xInInterFace)*dir_air_x+ex.z0*dir_air_z)*dir_air_x; - error_z=ex.z0- ((ex.x0-xInInterFace)*dir_air_x+ex.z0*dir_air_z)*dir_air_z; + error_z=ex.z0- ((ex.x0-xInInterFace)*dir_air_x+ex.z0*dir_air_z)*dir_air_z; absError=pow(error_x*error_x+error_z*error_z,0.5); - //dir_water=xInInterFace-particlePoint+0.1*error; + //dir_water=xInInterFace-particlePoint+0.1*error; dir_water_x=xInInterFace-X+0.1*error_x; dir_water_z=-Z+0.1*error_z; //dir_water = dir_water/norm(dir_water); dist=pow(dir_water_x*dir_water_x+dir_water_z*dir_water_z,0.5); dir_water_x=dir_water_x/dist; - dir_water_z=dir_water_z/dist; + dir_water_z=dir_water_z/dist; counter++; - } - *Xq = (Z-ex.z0)/dir_air_z*dir_air_x; - *Yq = 0; - */ - + } + *Xq = (Z-ex.z0)/dir_air_z*dir_air_x; + *Yq = 0; + */ } - -double multimed_r_nlay (ex,mm,X,Y,Z) - - +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +double multimed_r_nlay (Exterior ex, mm_np mm, double X, double Y, double Z) // calculates and returns the radial shift -Exterior ex; -mm_np mm; -double X, Y, Z; { - int i, it=0; - double beta1, beta2[32], beta3, r, rbeta, rdiff, rq, mmf; - double ocf=1.0; // over compensation factor for faster convergence - - double absError=100; - int counter=0; - double dir_water_x=-X; - double dir_water_z=ex.z0-Z; - double dist=pow(dir_water_x*dir_water_x+dir_water_z*dir_water_z,0.5); - double xInInterFace,comp_parallel,comp_perpendicular,dir_air_x,dir_air_z,error_x,error_z; - dir_water_x=dir_water_x/dist; - dir_water_z=dir_water_z/dist; - - // 1-medium case - if (mm.n1==1 && mm.nlay == 1 && mm.n2[0]==1 && mm.n3==1) return (1.0); - - - // interpolation in mmLUT, if selected (requires some global variables) - if (mm.lut) - { - // check, which is the correct image - for (i=0; i 0) return (mmf); - } - - // iterative procedure - r = sqrt ((X-ex.x0)*(X-ex.x0)+(Y-ex.y0)*(Y-ex.y0)); - rq = r; - - do - { - beta1 = atan (rq/(ex.z0-Z)); - for (i=0; i 0.001) || (rdiff < -0.001)) && it < 40); - - if (it >= 40) - { - puts ("Multimed_r_nlay stopped after 40. Iteration"); return (1.0); - } - - if (r != 0) return (rq/r); else return (1.0); - + int i, it=0; + double beta1, beta2[32], beta3, r, rbeta, rdiff, rq, mmf; + double ocf=1.0; // over compensation factor for faster convergence + + double absError=100; + int counter=0; + double dir_water_x=-X; + double dir_water_z=ex.z0-Z; + double dist=pow(dir_water_x*dir_water_x+dir_water_z*dir_water_z,0.5); + + dir_water_x = dir_water_x/dist; + dir_water_z = dir_water_z/dist; + + // 1-medium case + if (mm.n1==1 && mm.nlay == 1 && mm.n2[0]==1 && mm.n3==1) + return (1.0); + + // interpolation in mmLUT, if selected (requires some global variables) + if (mm.lut) { + // check, which is the correct image + for (i=0; i 0) + return (mmf); + } -//----------------------------------------------------------------------------------------- -//----------------------------------------------------------------------------------------- -//----------------------------------------------------------------------------------------- + // iterative procedure + r = sqrt ((X-ex.x0)*(X-ex.x0)+(Y-ex.y0)*(Y-ex.y0)); + rq = r; + + do { + beta1 = atan (rq/(ex.z0-Z)); + for (i=0; i 0.001 || rdiff < -0.001) && it < 40); + + if (it >= 40) { + puts ("Multimed_r_nlay stopped after 40. Iteration"); + return (1.0); + } - //Beat Lüthi, Nov 2007, I will implement the Risö method to avoid all this sin, atan etc - - /* - + if (r != 0) + return (rq/r); + else + return (1.0); + + //----------------------------------------------------------------------------------------- + //----------------------------------------------------------------------------------------- + //----------------------------------------------------------------------------------------- - while(absError>0.00001 && counter<40){ - //sending the water_ray to air + //Beat L¸thi, Nov 2007, I will implement the Risˆ method to avoid all this sin, atan etc + + /* + while(absError>0.00001 && counter<40){ + //sending the water_ray to air xInInterFace=X-Z/dir_water_z*dir_water_x; // //snellNormal_y=cross(dir_water,interF.base[2]); //snellNormal=snellNormal/norm(snellNormal); @@ -498,212 +451,183 @@ double X, Y, Z; //measuring how much cam_origo is missed and correcting water_ray //error=pCamOrigo-xInInterFace-dot(pCamOrigo-xInInterFace,dir_air)*dir_air/norm2(dir_air); error_x=ex.x0-xInInterFace-((ex.x0-xInInterFace)*dir_air_x+ex.z0*dir_air_z)*dir_air_x; - error_z=ex.z0- ((ex.x0-xInInterFace)*dir_air_x+ex.z0*dir_air_z)*dir_air_z; + error_z=ex.z0- ((ex.x0-xInInterFace)*dir_air_x+ex.z0*dir_air_z)*dir_air_z; absError=pow(error_x*error_x+error_z*error_z,0.5); - //dir_water=xInInterFace-particlePoint+0.1*error; + //dir_water=xInInterFace-particlePoint+0.1*error; dir_water_x=xInInterFace-X+0.1*error_x; dir_water_z=-Z+0.1*error_z; //dir_water = dir_water/norm(dir_water); dist=pow(dir_water_x*dir_water_x+dir_water_z*dir_water_z,0.5); dir_water_x=dir_water_x/dist; - dir_water_z=dir_water_z/dist; + dir_water_z=dir_water_z/dist; counter++; - } - return( (Z-ex.z0)/dir_air_z*dir_air_x / X );*/ - + } + return( (Z-ex.z0)/dir_air_z*dir_air_x / X );*/ } +#endif -void back_trans_Point_back(X_t,Y_t,Z_t,mm,G,cross_p,cross_c,X,Y,Z) - -double X_t,Y_t,Z_t,cross_p[],cross_c[]; -mm_np mm; -Glass G; -double *X,*Y,*Z; +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +void back_trans_Point_back(double X_t, double Y_t, double Z_t, + mm_np mm, Glass G, double cross_p[], double cross_c[], + double *X, double *Y, double *Z) { - double nVe,nGl; + double nVe,nGl; nGl=sqrt(pow(G.vec_x,2.)+pow(G.vec_y,2.)+pow(G.vec_z,2.)); nVe=sqrt( pow(cross_p[0]-(cross_c[0]-mm.d[0]*G.vec_x/nGl),2.) - +pow(cross_p[1]-(cross_c[1]-mm.d[0]*G.vec_y/nGl),2.) + +pow(cross_p[1]-(cross_c[1]-mm.d[0]*G.vec_y/nGl),2.) +pow(cross_p[2]-(cross_c[2]-mm.d[0]*G.vec_z/nGl),2.)); - *X=cross_c[0]+X_t*(cross_p[0]-(cross_c[0]-mm.d[0]*G.vec_x/nGl))/nVe+Z_t*G.vec_x/nGl; *Y=cross_c[1]+X_t*(cross_p[1]-(cross_c[1]-mm.d[0]*G.vec_y/nGl))/nVe+Z_t*G.vec_y/nGl; *Z=cross_c[2]+X_t*(cross_p[2]-(cross_c[2]-mm.d[0]*G.vec_z/nGl))/nVe+Z_t*G.vec_z/nGl; } +#endif - - -void trans_Cam_Point_back(ex,mm,gl,X,Y,Z,ex_t,X_t,Y_t,Z_t,cross_p,cross_c) - -Exterior ex; -mm_np mm; -Glass gl; -double X, Y, Z; -Exterior *ex_t; -double *X_t, *Y_t, *Z_t; -double *cross_p, *cross_c; +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +void trans_Cam_Point_back(Exterior ex, mm_np mm, Glass gl, double X, double Y, double Z, + Exterior *ex_t, double *X_t, double *Y_t, double *Z_t, + double *cross_p, double *cross_c) { - //--Beat Lüthi June 07: I change the stuff to a system perpendicular to the interface - double dummy; - double nGl; - - nGl=sqrt(gl.vec_x*gl.vec_x+gl.vec_y*gl.vec_y+gl.vec_z*gl.vec_z); - - dummy=ex.x0*gl.vec_x/nGl+ex.y0*gl.vec_y/nGl+ex.z0*gl.vec_z/nGl-nGl-mm.d[0]; - cross_c[0]=ex.x0-dummy*gl.vec_x/nGl; - cross_c[1]=ex.y0-dummy*gl.vec_y/nGl; - cross_c[2]=ex.z0-dummy*gl.vec_z/nGl; - ex_t->x0=0.; - ex_t->y0=0.; - ex_t->z0=dummy; - - dummy=nGl-X*gl.vec_x/nGl+Y*gl.vec_y/nGl+Z*gl.vec_z/nGl; - cross_p[0]=X+dummy*gl.vec_x/nGl; - cross_p[1]=Y+dummy*gl.vec_y/nGl; - cross_p[2]=Z+dummy*gl.vec_z/nGl; - *Z_t=-dummy-mm.d[0]; - dummy=sqrt( pow(cross_p[0]-(cross_c[0]-mm.d[0]*gl.vec_x/nGl),2.) - +pow(cross_p[1]-(cross_c[1]-mm.d[0]*gl.vec_y/nGl),2.) + //--Beat L¸thi June 07: I change the stuff to a system perpendicular to the interface + double dummy; + double nGl; + + nGl=sqrt(gl.vec_x*gl.vec_x+gl.vec_y*gl.vec_y+gl.vec_z*gl.vec_z); + + dummy=ex.x0*gl.vec_x/nGl+ex.y0*gl.vec_y/nGl+ex.z0*gl.vec_z/nGl-nGl-mm.d[0]; + cross_c[0]=ex.x0-dummy*gl.vec_x/nGl; + cross_c[1]=ex.y0-dummy*gl.vec_y/nGl; + cross_c[2]=ex.z0-dummy*gl.vec_z/nGl; + ex_t->x0=0.; + ex_t->y0=0.; + ex_t->z0=dummy; + + dummy=nGl-X*gl.vec_x/nGl+Y*gl.vec_y/nGl+Z*gl.vec_z/nGl; + cross_p[0]=X+dummy*gl.vec_x/nGl; + cross_p[1]=Y+dummy*gl.vec_y/nGl; + cross_p[2]=Z+dummy*gl.vec_z/nGl; + *Z_t=-dummy-mm.d[0]; + dummy=sqrt( pow(cross_p[0]-(cross_c[0]-mm.d[0]*gl.vec_x/nGl),2.) + +pow(cross_p[1]-(cross_c[1]-mm.d[0]*gl.vec_y/nGl),2.) +pow(cross_p[2]-(cross_c[2]-mm.d[0]*gl.vec_z/nGl),2.)); - *X_t=dummy; - *Y_t=0; - + *X_t=dummy; + *Y_t=0; } +#endif -void trans_Cam_Point(ex,mm,gl,X,Y,Z,ex_t,X_t,Y_t,Z_t,cross_p,cross_c) - -Exterior ex; -mm_np mm; -Glass gl; -double X, Y, Z; -Exterior *ex_t; -double *X_t, *Y_t, *Z_t; -double *cross_p, *cross_c; +void trans_Cam_Point(Exterior ex, mm_np mm, Glass gl, double X, double Y, double Z, + Exterior *ex_t, double *X_t, double *Y_t, double *Z_t, + double *cross_p, double *cross_c) { - //--Beat Lüthi June 07: I change the stuff to a system perpendicular to the interface - double dist_cam_glas,dist_point_glas,dist_o_glas; //glas inside at water - - dist_o_glas=sqrt(gl.vec_x*gl.vec_x+gl.vec_y*gl.vec_y+gl.vec_z*gl.vec_z); - dist_cam_glas = ex.x0*gl.vec_x/dist_o_glas+ex.y0*gl.vec_y/dist_o_glas+ex.z0*gl.vec_z/dist_o_glas-dist_o_glas-mm.d[0]; - dist_point_glas = X *gl.vec_x/dist_o_glas+Y *gl.vec_y/dist_o_glas+Z *gl.vec_z/dist_o_glas-dist_o_glas; - - cross_c[0]=ex.x0-dist_cam_glas*gl.vec_x/dist_o_glas; - cross_c[1]=ex.y0-dist_cam_glas*gl.vec_y/dist_o_glas; - cross_c[2]=ex.z0-dist_cam_glas*gl.vec_z/dist_o_glas; - cross_p[0]=X -dist_point_glas*gl.vec_x/dist_o_glas; - cross_p[1]=Y -dist_point_glas*gl.vec_y/dist_o_glas; - cross_p[2]=Z -dist_point_glas*gl.vec_z/dist_o_glas; - - ex_t->x0=0.; - ex_t->y0=0.; - ex_t->z0=dist_cam_glas+mm.d[0]; - - *X_t=sqrt( pow(cross_p[0]-(cross_c[0]-mm.d[0]*gl.vec_x/dist_o_glas),2.) - +pow(cross_p[1]-(cross_c[1]-mm.d[0]*gl.vec_y/dist_o_glas),2.) + //--Beat L¸thi June 07: I change the stuff to a system perpendicular to the interface + double dist_cam_glas,dist_point_glas,dist_o_glas; //glas inside at water + + dist_o_glas=sqrt(gl.vec_x*gl.vec_x+gl.vec_y*gl.vec_y+gl.vec_z*gl.vec_z); + dist_cam_glas = ex.x0*gl.vec_x/dist_o_glas+ex.y0*gl.vec_y/dist_o_glas+ex.z0*gl.vec_z/dist_o_glas-dist_o_glas-mm.d[0]; + dist_point_glas = X *gl.vec_x/dist_o_glas+Y *gl.vec_y/dist_o_glas+Z *gl.vec_z/dist_o_glas-dist_o_glas; + + cross_c[0]=ex.x0-dist_cam_glas*gl.vec_x/dist_o_glas; + cross_c[1]=ex.y0-dist_cam_glas*gl.vec_y/dist_o_glas; + cross_c[2]=ex.z0-dist_cam_glas*gl.vec_z/dist_o_glas; + cross_p[0]=X -dist_point_glas*gl.vec_x/dist_o_glas; + cross_p[1]=Y -dist_point_glas*gl.vec_y/dist_o_glas; + cross_p[2]=Z -dist_point_glas*gl.vec_z/dist_o_glas; + + ex_t->x0=0.; + ex_t->y0=0.; + ex_t->z0=dist_cam_glas+mm.d[0]; + + *X_t = sqrt( pow(cross_p[0]-(cross_c[0]-mm.d[0]*gl.vec_x/dist_o_glas),2.) + +pow(cross_p[1]-(cross_c[1]-mm.d[0]*gl.vec_y/dist_o_glas),2.) +pow(cross_p[2]-(cross_c[2]-mm.d[0]*gl.vec_z/dist_o_glas),2.)); - *Y_t=0; - *Z_t=dist_point_glas; - + *Y_t = 0; + *Z_t = dist_point_glas; } -void back_trans_Point(X_t,Y_t,Z_t,mm,G,cross_p,cross_c,X,Y,Z) - -double X_t,Y_t,Z_t,cross_p[],cross_c[]; -mm_np mm; -Glass G; -double *X,*Y,*Z; +void back_trans_Point(double X_t, double Y_t, double Z_t, mm_np mm, Glass G, + double cross_p[], double cross_c[], double *X, double *Y, double *Z) { - - double nVe,nGl; + double nVe,nGl; nGl=sqrt(pow(G.vec_x,2.)+pow(G.vec_y,2.)+pow(G.vec_z,2.)); nVe=sqrt( pow(cross_p[0]-(cross_c[0]-mm.d[0]*G.vec_x/nGl),2.) - +pow(cross_p[1]-(cross_c[1]-mm.d[0]*G.vec_y/nGl),2.) + +pow(cross_p[1]-(cross_c[1]-mm.d[0]*G.vec_y/nGl),2.) +pow(cross_p[2]-(cross_c[2]-mm.d[0]*G.vec_z/nGl),2.)); - *X=cross_c[0]-mm.d[0]*G.vec_x/nGl+X_t*(cross_p[0]-(cross_c[0]-mm.d[0]*G.vec_x/nGl))/nVe+Z_t*G.vec_x/nGl; - *Y=cross_c[1]-mm.d[0]*G.vec_y/nGl+X_t*(cross_p[1]-(cross_c[1]-mm.d[0]*G.vec_y/nGl))/nVe+Z_t*G.vec_y/nGl; - *Z=cross_c[2]-mm.d[0]*G.vec_z/nGl+X_t*(cross_p[2]-(cross_c[2]-mm.d[0]*G.vec_z/nGl))/nVe+Z_t*G.vec_z/nGl; - + *X = cross_c[0]-mm.d[0]*G.vec_x/nGl+X_t*(cross_p[0]-(cross_c[0]-mm.d[0]*G.vec_x/nGl))/nVe+Z_t*G.vec_x/nGl; + *Y = cross_c[1]-mm.d[0]*G.vec_y/nGl+X_t*(cross_p[1]-(cross_c[1]-mm.d[0]*G.vec_y/nGl))/nVe+Z_t*G.vec_y/nGl; + *Z = cross_c[2]-mm.d[0]*G.vec_z/nGl+X_t*(cross_p[2]-(cross_c[2]-mm.d[0]*G.vec_z/nGl))/nVe+Z_t*G.vec_z/nGl; } -double multimed_r_nlay_v2 (ex,ex_o,mm,X,Y,Z) +double multimed_r_nlay_v2 (Exterior ex, Exterior ex_o, mm_np mm, + double X, double Y, double Z) /* calculates and returns the radial shift */ -Exterior ex; -Exterior ex_o; -mm_np mm; -double X, Y, Z; { - int i, it=0; - double beta1, beta2[32], beta3, r, rbeta, rdiff, rq, mmf; - double ocf=1.0; // over compensation factor for faster convergence - - double absError=100; - int counter=0; - double dir_water_x=-X; - double dir_water_z=ex.z0-Z; - double dist=pow(dir_water_x*dir_water_x+dir_water_z*dir_water_z,0.5); - double xInInterFace,comp_parallel,comp_perpendicular,dir_air_x,dir_air_z,error_x,error_z; - dir_water_x=dir_water_x/dist; - dir_water_z=dir_water_z/dist; - - - // 1-medium case - if (mm.n1==1 && mm.nlay == 1 && mm.n2[0]==1 && mm.n3==1) return (1.0); - - - // interpolation in mmLUT, if selected (requires some global variables) - if (mm.lut) - { - // check, which is the correct image - for (i=0; i 0) return (mmf); - } - - // iterative procedure - r = sqrt ((X-ex.x0)*(X-ex.x0)+(Y-ex.y0)*(Y-ex.y0)); - rq = r; - - do - { - beta1 = atan (rq/(ex.z0-Z)); - for (i=0; i 0.001) || (rdiff < -0.001)) && it < 40); - - if (it >= 40) - { - puts ("Multimed_r_nlay_v2 stopped after 40. Iteration"); return (1.0); - } - - if (r != 0) return (rq/r); else return (1.0); - + int i, it=0; + double beta1, beta2[32], beta3, r, rbeta, rdiff, rq, mmf; + double ocf=1.0; // over compensation factor for faster convergence + + double absError=100; + int counter=0; + double dir_water_x=-X; + double dir_water_z=ex.z0-Z; + double dist=pow(dir_water_x*dir_water_x+dir_water_z*dir_water_z,0.5); + + dir_water_x=dir_water_x/dist; + dir_water_z=dir_water_z/dist; + + // 1-medium case + if (mm.n1==1 && mm.nlay == 1 && mm.n2[0]==1 && mm.n3==1) + return (1.0); + + // interpolation in mmLUT, if selected (requires some global variables) + if (mm.lut) { + // check, which is the correct image + for (i=0; i 0) return (mmf); + } -//----------------------------------------------------------------------------------------- -//----------------------------------------------------------------------------------------- -//----------------------------------------------------------------------------------------- + // iterative procedure + r = sqrt ((X-ex.x0)*(X-ex.x0)+(Y-ex.y0)*(Y-ex.y0)); + rq = r; + + do { + beta1 = atan (rq/(ex.z0-Z)); + for (i=0; i 0.001 || rdiff < -0.001) && it < 40); + + if (it >= 40) { + puts ("Multimed_r_nlay_v2 stopped after 40. Iteration"); + return (1.0); + } + if (r != 0) + return (rq/r); + else + return (1.0); - //Beat Lüthi, Nov 2007, I will implement the Risö method to avoid all this sin, atan etc - - - /* + //----------------------------------------------------------------------------------------- + //----------------------------------------------------------------------------------------- + //----------------------------------------------------------------------------------------- + + //Beat L¸thi, Nov 2007, I will implement the Risˆ method to avoid all this sin, atan etc + /* - while(absError>0.00001 && counter<40){ - //sending the water_ray to air + while(absError>0.00001 && counter<40){ + //sending the water_ray to air xInInterFace=X-Z/dir_water_z*dir_water_x; // //snellNormal_y=cross(dir_water,interF.base[2]); //snellNormal=snellNormal/norm(snellNormal); @@ -721,371 +645,369 @@ double X, Y, Z; //measuring how much cam_origo is missed and correcting water_ray //error=pCamOrigo-xInInterFace-dot(pCamOrigo-xInInterFace,dir_air)*dir_air/norm2(dir_air); error_x=ex.x0-xInInterFace-((ex.x0-xInInterFace)*dir_air_x+ex.z0*dir_air_z)*dir_air_x; - error_z=ex.z0- ((ex.x0-xInInterFace)*dir_air_x+ex.z0*dir_air_z)*dir_air_z; + error_z=ex.z0- ((ex.x0-xInInterFace)*dir_air_x+ex.z0*dir_air_z)*dir_air_z; absError=pow(error_x*error_x+error_z*error_z,0.5); - //dir_water=xInInterFace-particlePoint+0.1*error; + //dir_water=xInInterFace-particlePoint+0.1*error; dir_water_x=xInInterFace-X+0.1*error_x; dir_water_z=-Z+0.1*error_z; //dir_water = dir_water/norm(dir_water); dist=pow(dir_water_x*dir_water_x+dir_water_z*dir_water_z,0.5); dir_water_x=dir_water_x/dist; - dir_water_z=dir_water_z/dist; + dir_water_z=dir_water_z/dist; counter++; - } - return( (Z-ex.z0)/dir_air_z*dir_air_x / X );*/ - + } + return( (Z-ex.z0)/dir_air_z*dir_air_x / X );*/ } - -void init_mmLUT (i_cam) -int i_cam; +void init_mmLUT (int i_cam) { - register int i,j, nr, nz; - double X,Y,Z, R, X1,Y1,Z1, Zmin, Rmax=0,Zmax, a,b,c; - double x,y, *Ri,*Zi; - double rw = 2; //was 2, has unit [mm]??, Beat Lüthi Aug 1, 2007, is ok - Exterior Ex_t[4]; - double X_t,Y_t,Z_t,cross_p[3],cross_c[3],Zmin_t,Zmax_t; - - /* find extrema of imaged object volume */ - /* ==================================== */ - - /* find extrema in depth */ - - fpp = fopen ("parameters/criteria.par", "r"); - fscanf (fpp, "%lf\n", &X); - fscanf (fpp, "%lf\n", &Zmin); - fscanf (fpp, "%lf\n", &Zmax); - fscanf (fpp, "%lf\n", &X); - fscanf (fpp, "%lf\n", &Z); if (Z < Zmin) Zmin = Z; - fscanf (fpp, "%lf\n", &Z); if (Z > Zmax) Zmax = Z; - fclose (fpp); - - Zmin -= fmod (Zmin, rw); - Zmax += (rw - fmod (Zmax, rw)); - Zmin_t=Zmin; - Zmax_t=Zmax; - - /* intersect with image vertices rays */ - - //--00 - pixel_to_metric (0., 0., imx,imy, pix_x,pix_y, &x,&y, chfield); - x = x - I[i_cam].xh; - y = y - I[i_cam].yh; - correct_brown_affin (x, y, ap[i_cam], &x,&y); - ray_tracing_v2 (x,y, Ex[i_cam], I[i_cam], G[i_cam], mmp, &X1, &Y1, &Z1, &a, &b, &c); - Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - //trans - trans_Cam_Point(Ex[i_cam],mmp,G[i_cam],X,Y,Z,&Ex_t[i_cam],&X_t,&Y_t,&Z_t,&cross_p,&cross_c); - if(Z_tZmax_t)Zmax_t=Z_t; - // - R = sqrt ( (X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) - + (Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); - - if (R > Rmax) Rmax = R; - Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - //trans - trans_Cam_Point(Ex[i_cam],mmp,G[i_cam],X,Y,Z,&Ex_t[i_cam],&X_t,&Y_t,&Z_t,&cross_p,&cross_c); - if(Z_tZmax_t)Zmax_t=Z_t; - // - R = sqrt ( (X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) - + (Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); - - //--0y - if (R > Rmax) Rmax = R; - pixel_to_metric (0., (double) imy, imx,imy, pix_x,pix_y, &x,&y, chfield); - x = x - I[i_cam].xh; - y = y - I[i_cam].yh; - correct_brown_affin (x, y, ap[i_cam], &x,&y); - ray_tracing_v2 (x,y, Ex[i_cam], I[i_cam], G[i_cam], mmp, &X1, &Y1, &Z1, &a, &b, &c); - Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - //trans - trans_Cam_Point(Ex[i_cam],mmp,G[i_cam],X,Y,Z,&Ex_t[i_cam],&X_t,&Y_t,&Z_t,&cross_p,&cross_c); - if(Z_tZmax_t)Zmax_t=Z_t; - // - R = sqrt ( (X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) - + (Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); - - if (R > Rmax) Rmax = R; - Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - //trans - trans_Cam_Point(Ex[i_cam],mmp,G[i_cam],X,Y,Z,&Ex_t[i_cam],&X_t,&Y_t,&Z_t,&cross_p,&cross_c); - if(Z_tZmax_t)Zmax_t=Z_t; - // - R = sqrt ( (X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) - + (Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); - - //--x0 - if (R > Rmax) Rmax = R; - pixel_to_metric ((double) imx, 0., imx,imy, pix_x,pix_y, &x,&y, chfield); - x = x - I[i_cam].xh; - y = y - I[i_cam].yh; - correct_brown_affin (x, y, ap[i_cam], &x,&y); - ray_tracing_v2 (x,y, Ex[i_cam], I[i_cam], G[i_cam], mmp, &X1, &Y1, &Z1, &a, &b, &c); - Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - //trans - trans_Cam_Point(Ex[i_cam],mmp,G[i_cam],X,Y,Z,&Ex_t[i_cam],&X_t,&Y_t,&Z_t,&cross_p,&cross_c); - if(Z_tZmax_t)Zmax_t=Z_t; - // - R = sqrt ( (X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) - + (Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); - - if (R > Rmax) Rmax = R; - Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - //trans - trans_Cam_Point(Ex[i_cam],mmp,G[i_cam],X,Y,Z,&Ex_t[i_cam],&X_t,&Y_t,&Z_t,&cross_p,&cross_c); - if(Z_tZmax_t)Zmax_t=Z_t; - // - R = sqrt ( (X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) - + (Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); - - //--xy - if (R > Rmax) Rmax = R; - pixel_to_metric ((double) imx, (double) imy, - imx,imy, pix_x,pix_y, &x,&y, chfield); - x = x - I[i_cam].xh; - y = y - I[i_cam].yh; - correct_brown_affin (x, y, ap[i_cam], &x,&y); - ray_tracing_v2 (x,y, Ex[i_cam], I[i_cam], G[i_cam], mmp, &X1, &Y1, &Z1, &a, &b, &c); - Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - //trans - trans_Cam_Point(Ex[i_cam],mmp,G[i_cam],X,Y,Z,&Ex_t[i_cam],&X_t,&Y_t,&Z_t,&cross_p,&cross_c); - if(Z_tZmax_t)Zmax_t=Z_t; - // - R = sqrt ( (X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) - + (Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); - - if (R > Rmax) Rmax = R; - Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - //trans - trans_Cam_Point(Ex[i_cam],mmp,G[i_cam],X,Y,Z,&Ex_t[i_cam],&X_t,&Y_t,&Z_t,&cross_p,&cross_c); - if(Z_tZmax_t)Zmax_t=Z_t; - // - R = sqrt ( (X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) - + (Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); - - if (R > Rmax) Rmax = R; - - /* round values (-> enlarge) */ - Rmax += (rw - fmod (Rmax, rw)); - - /* get # of rasterlines in r,z */ - nr = Rmax/rw + 1; - nz = (Zmax_t-Zmin_t)/rw + 1; + register int i,j, nr, nz; + double X,Y,Z, R, X1,Y1,Z1, Zmin, Rmax=0,Zmax, a,b,c; + double x,y, *Ri,*Zi; + double rw = 2; //was 2, has unit [mm]??, Beat L¸thi Aug 1, 2007, is ok + Exterior Ex_t[4]; + double X_t,Y_t,Z_t,cross_p[3],cross_c[3],Zmin_t,Zmax_t; + + /* find extrema of imaged object volume */ + /* ==================================== */ + + /* find extrema in depth */ + + fpp = fopen_rp("parameters/criteria.par"); // replaced fopen(), ad holten 12-2012 + if (!fpp) return; + fscanf (fpp, "%lf\n", &X); + fscanf (fpp, "%lf\n", &Zmin); + fscanf (fpp, "%lf\n", &Zmax); + fscanf (fpp, "%lf\n", &X); + fscanf (fpp, "%lf\n", &Z); if (Z < Zmin) Zmin = Z; + fscanf (fpp, "%lf\n", &Z); if (Z > Zmax) Zmax = Z; + fclose (fpp); + + Zmin -= fmod (Zmin, rw); + Zmax += (rw - fmod (Zmax, rw)); + Zmin_t=Zmin; + Zmax_t=Zmax; + + /* intersect with image vertices rays */ + + //--00 + pixel_to_metric (0., 0., imx,imy, pix_x,pix_y, &x,&y, chfield); + x = x - I[i_cam].xh; + y = y - I[i_cam].yh; + correct_brown_affin (x, y, ap[i_cam], &x,&y); + ray_tracing_v2 (x,y, Ex[i_cam], I[i_cam], G[i_cam], mmp, &X1, &Y1, &Z1, &a, &b, &c); + Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + + //trans + trans_Cam_Point(Ex[i_cam], mmp,G[i_cam], X,Y,Z, &Ex_t[i_cam], + &X_t, &Y_t, &Z_t, cross_p, cross_c); + + if(Z_tZmax_t) Zmax_t=Z_t; + // + R = sqrt((X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) + +(Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); - /* create twodimensional mmLUT structure */ - //trans - trans_Cam_Point(Ex[i_cam],mmp,G[i_cam],X,Y,Z,&Ex_t[i_cam],&X_t,&Y_t,&Z_t,&cross_p,&cross_c); - - mmLUT[i_cam].origin.x = Ex_t[i_cam].x0; - mmLUT[i_cam].origin.y = Ex_t[i_cam].y0; - mmLUT[i_cam].origin.z = Zmin_t; - mmLUT[i_cam].nr = nr; - mmLUT[i_cam].nz = nz; - mmLUT[i_cam].rw = rw; - mmLUT[i_cam].data = (double *) malloc (nr*nz * sizeof (double)); - - /* fill mmLUT structure */ - /* ==================== */ - - Ri = (double *) malloc (nr * sizeof (double)); - for (i=0; i Rmax) Rmax = R; + Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; //trans - trans_Cam_Point(Ex[i_cam],mmp,G[i_cam],X,Y,Z,&Ex_t[i_cam],&X_t,&Y_t,&Z_t,&cross_p,&cross_c); - mmLUT[i_cam].data[i*nz + j] - = multimed_r_nlay_v2 (Ex_t[i_cam], Ex[i_cam], mmp, - Ri[i]+Ex_t[i_cam].x0, Ex_t[i_cam].y0, Zi[j]); - } -} + trans_Cam_Point(Ex[i_cam], mmp, G[i_cam], X,Y,Z, &Ex_t[i_cam], &X_t, &Y_t, &Z_t, cross_p, cross_c); + if(Z_tZmax_t) Zmax_t=Z_t; + // + R = sqrt ((X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) + +(Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); + + //--0y + if (R > Rmax) Rmax = R; + pixel_to_metric (0., (double) imy, imx,imy, pix_x,pix_y, &x,&y, chfield); + x = x - I[i_cam].xh; + y = y - I[i_cam].yh; + correct_brown_affin (x, y, ap[i_cam], &x,&y); + ray_tracing_v2 (x,y, Ex[i_cam], I[i_cam], G[i_cam], mmp, &X1, &Y1, &Z1, &a, &b, &c); + Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + //trans + trans_Cam_Point(Ex[i_cam], mmp, G[i_cam], X,Y,Z, &Ex_t[i_cam], &X_t, &Y_t, &Z_t, cross_p, cross_c); + if(Z_tZmax_t) Zmax_t=Z_t; + // + R = sqrt ((X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) + + (Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); + if (R > Rmax) Rmax = R; + Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + //trans + trans_Cam_Point(Ex[i_cam], mmp, G[i_cam], X,Y,Z, &Ex_t[i_cam], &X_t, &Y_t, &Z_t, cross_p, cross_c); + if(Z_tZmax_t) Zmax_t=Z_t; + // + R = sqrt ((X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) + + (Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); + + //--x0 + if (R > Rmax) Rmax = R; + pixel_to_metric ((double) imx, 0., imx,imy, pix_x,pix_y, &x,&y, chfield); + x = x - I[i_cam].xh; + y = y - I[i_cam].yh; + correct_brown_affin (x, y, ap[i_cam], &x,&y); + ray_tracing_v2 (x,y, Ex[i_cam], I[i_cam], G[i_cam], mmp, &X1, &Y1, &Z1, &a, &b, &c); + Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + //trans + trans_Cam_Point(Ex[i_cam], mmp, G[i_cam], X,Y,Z, &Ex_t[i_cam], &X_t, &Y_t, &Z_t, cross_p, cross_c); + if(Z_tZmax_t) Zmax_t=Z_t; + // + R = sqrt ((X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) + + (Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); + if (R > Rmax) Rmax = R; + Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + //trans + trans_Cam_Point(Ex[i_cam],mmp,G[i_cam],X,Y,Z,&Ex_t[i_cam],&X_t,&Y_t,&Z_t, cross_p, cross_c); + if(Z_tZmax_t) Zmax_t=Z_t; + // + R = sqrt ((X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) + + (Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); -double get_mmf_from_mmLUT (i_cam, X,Y,Z) -int i_cam; -double X,Y,Z; -{ - int i, ir,iz, nr,nz, v4[4]; - double R, sr,sz, rw, mmf=1; - - rw = mmLUT[i_cam].rw; - - Z -= mmLUT[i_cam].origin.z; sz = Z/rw; iz = (int) sz; sz -= iz; - X -= mmLUT[i_cam].origin.x; - Y -= mmLUT[i_cam].origin.y; - R = sqrt (X*X + Y*Y); sr = R/rw; ir = (int) sr; sr -= ir; - - nz = mmLUT[i_cam].nz; - nr = mmLUT[i_cam].nr; - - /* check whether point is inside camera's object volume */ - if (ir > nr) return (0); - if (iz < 0 || iz > nz) return (0); - - /* bilinear interpolation in r/z box */ - /* ================================= */ - - /* get vertices of box */ - v4[0] = ir*nz + iz; - v4[1] = ir*nz + (iz+1); - v4[2] = (ir+1)*nz + iz; - v4[3] = (ir+1)*nz + (iz+1); - - /* 2. check wther point is inside camera's object volume */ - /* important for epipolar line computation */ - for (i=0; i<4; i++) - if (v4[i] < 0 || v4[i] > nr*nz) return (0); - - /* interpolate */ - mmf = mmLUT[i_cam].data[v4[0]] * (1-sr)*(1-sz) - + mmLUT[i_cam].data[v4[1]] * (1-sr)*sz - + mmLUT[i_cam].data[v4[2]] * sr*(1-sz) - + mmLUT[i_cam].data[v4[3]] * sr*sz; - - return (mmf); -} + //--xy + if (R > Rmax) Rmax = R; + pixel_to_metric ((double) imx, (double) imy, + imx,imy, pix_x,pix_y, &x,&y, chfield); + x = x - I[i_cam].xh; + y = y - I[i_cam].yh; + correct_brown_affin (x, y, ap[i_cam], &x,&y); + ray_tracing_v2 (x,y, Ex[i_cam], I[i_cam], G[i_cam], mmp, &X1, &Y1, &Z1, &a, &b, &c); + Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + //trans + trans_Cam_Point(Ex[i_cam],mmp,G[i_cam],X,Y,Z,&Ex_t[i_cam],&X_t,&Y_t,&Z_t,cross_p,cross_c); + if(Z_tZmax_t) Zmax_t=Z_t; + // + R = sqrt ((X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) + + (Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); + + if (R > Rmax) Rmax = R; + Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + //trans + trans_Cam_Point(Ex[i_cam],mmp,G[i_cam],X,Y,Z,&Ex_t[i_cam],&X_t,&Y_t,&Z_t,cross_p,cross_c); + if(Z_tZmax_t) Zmax_t=Z_t; + // + R = sqrt ((X_t-Ex_t[i_cam].x0)*(X_t-Ex_t[i_cam].x0) + + (Y_t-Ex_t[i_cam].y0)*(Y_t-Ex_t[i_cam].y0)); + + if (R > Rmax) Rmax = R; + + /* round values (-> enlarge) */ + Rmax += (rw - fmod (Rmax, rw)); + + /* get # of rasterlines in r,z */ + nr = (int)(Rmax/rw + 1); + nz = (int)((Zmax_t-Zmin_t)/rw + 1); + /* create twodimensional mmLUT structure */ + //trans + trans_Cam_Point(Ex[i_cam],mmp,G[i_cam],X,Y,Z,&Ex_t[i_cam],&X_t,&Y_t,&Z_t,cross_p,cross_c); + + mmLUT[i_cam].origin.x = Ex_t[i_cam].x0; + mmLUT[i_cam].origin.y = Ex_t[i_cam].y0; + mmLUT[i_cam].origin.z = Zmin_t; + mmLUT[i_cam].nr = nr; + mmLUT[i_cam].nz = nz; + mmLUT[i_cam].rw = rw; + mmLUT[i_cam].data = (double *) malloc (nr*nz * sizeof (double)); + + /* fill mmLUT structure */ + /* ==================== */ + Ri = (double *) malloc (nr * sizeof (double)); + for (i=0; i Zmax) Zmax = Z; - fclose (fpp); - - *zmin=Zmin; - *zmax=Zmax; - - for (i_cam=0;i_cam *xmax) *xmax=X; - if ( X < *xmin) *xmin=X; - if ( Y > *ymax) *ymax=Y; - if ( Y < *ymin) *ymin=Y; - - if (R > Rmax) Rmax = R; - Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - R = sqrt ( (X-Ex[i_cam].x0)*(X-Ex[i_cam].x0) - + (Y-Ex[i_cam].y0)*(Y-Ex[i_cam].y0)); + int i, ir,iz, nr,nz, v4[4]; + double R, sr,sz, rw, mmf=1; - if ( X > *xmax) *xmax=X; - if ( X < *xmin) *xmin=X; - if ( Y > *ymax) *ymax=Y; - if ( Y < *ymin) *ymin=Y; - - if (R > Rmax) Rmax = R; - pixel_to_metric (0.0, (double) imy, imx,imy, pix_x,pix_y, &x,&y, chfield); - x = x - I[i_cam].xh; - y = y - I[i_cam].yh; - correct_brown_affin (x, y, ap[i_cam], &x,&y); - ray_tracing_v2 (x,y, Ex[i_cam], I[i_cam], G[i_cam], mmp, &X1, &Y1, &Z1, &a, &b, &c); - Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - R = sqrt ( (X-Ex[i_cam].x0)*(X-Ex[i_cam].x0) - + (Y-Ex[i_cam].y0)*(Y-Ex[i_cam].y0)); + rw = mmLUT[i_cam].rw; - if ( X > *xmax) *xmax=X; - if ( X < *xmin) *xmin=X; - if ( Y > *ymax) *ymax=Y; - if ( Y < *ymin) *ymin=Y; - - if (R > Rmax) Rmax = R; - Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - R = sqrt ( (X-Ex[i_cam].x0)*(X-Ex[i_cam].x0) - + (Y-Ex[i_cam].y0)*(Y-Ex[i_cam].y0)); + Z -= mmLUT[i_cam].origin.z; sz = Z/rw; iz = (int) sz; sz -= iz; + X -= mmLUT[i_cam].origin.x; + Y -= mmLUT[i_cam].origin.y; + R = sqrt (X*X + Y*Y); sr = R/rw; ir = (int) sr; sr -= ir; - if ( X > *xmax) *xmax=X; - if ( X < *xmin) *xmin=X; - if ( Y > *ymax) *ymax=Y; - if ( Y < *ymin) *ymin=Y; - - if (R > Rmax) Rmax = R; - - pixel_to_metric ((double) imx, 0., imx,imy, pix_x,pix_y, &x,&y, chfield); - x = x - I[i_cam].xh; - y = y - I[i_cam].yh; - correct_brown_affin (x, y, ap[i_cam], &x,&y); - ray_tracing_v2 (x,y, Ex[i_cam], I[i_cam], G[i_cam], mmp, &X1, &Y1, &Z1, &a, &b, &c); - Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - R = sqrt ( (X-Ex[i_cam].x0)*(X-Ex[i_cam].x0) + nz = mmLUT[i_cam].nz; + nr = mmLUT[i_cam].nr; + + /* check whether point is inside camera's object volume */ + if (ir > nr) return (0); + if (iz < 0 || iz > nz) return (0); + + /* bilinear interpolation in r/z box */ + /* ================================= */ + + /* get vertices of box */ + v4[0] = ir*nz + iz; + v4[1] = ir*nz + (iz+1); + v4[2] = (ir+1)*nz + iz; + v4[3] = (ir+1)*nz + (iz+1); + + /* 2. check wther point is inside camera's object volume */ + /* important for epipolar line computation */ + for (i=0; i<4; i++) + if (v4[i] < 0 || v4[i] > nr*nz) return (0); + + /* interpolate */ + mmf = mmLUT[i_cam].data[v4[0]] * (1-sr)*(1-sz) + + mmLUT[i_cam].data[v4[1]] * (1-sr)*sz + + mmLUT[i_cam].data[v4[2]] * sr*(1-sz) + + mmLUT[i_cam].data[v4[3]] * sr*sz; + + return (mmf); +} + + +void volumedimension (double *xmax, double *xmin, double *ymax, double *ymin, + double *zmax, double *zmin) +{ + int i_cam; + double X,Y,Z, R, X1,Y1,Z1, Zmin, Rmax=0,Zmax, a,b,c; + double x,y; + + /* find extrema of imaged object volume */ + /* ==================================== */ + + fpp = fopen_rp("parameters/criteria.par"); // replaced fopen, ad holten, 12-2012 + if (!fpp) return; + fscanf (fpp, "%lf\n", &X); + fscanf (fpp, "%lf\n", &Zmin); + fscanf (fpp, "%lf\n", &Zmax); + fscanf (fpp, "%lf\n", &X); + fscanf (fpp, "%lf\n", &Z); if (Z < Zmin) Zmin = Z; + fscanf (fpp, "%lf\n", &Z); if (Z > Zmax) Zmax = Z; + fclose (fpp); + + *zmin=Zmin; + *zmax=Zmax; + + for (i_cam=0;i_cam *xmax) *xmax=X; + if (X < *xmin) *xmin=X; + if (Y > *ymax) *ymax=Y; + if (Y < *ymin) *ymin=Y; + + if (R > Rmax) Rmax = R; + Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + R = sqrt ((X-Ex[i_cam].x0)*(X-Ex[i_cam].x0) + + (Y-Ex[i_cam].y0)*(Y-Ex[i_cam].y0)); + + if (X > *xmax) *xmax=X; + if (X < *xmin) *xmin=X; + if (Y > *ymax) *ymax=Y; + if (Y < *ymin) *ymin=Y; + + if (R > Rmax) Rmax = R; + pixel_to_metric (0.0, (double) imy, imx,imy, pix_x,pix_y, &x,&y, chfield); + x = x - I[i_cam].xh; + y = y - I[i_cam].yh; + correct_brown_affin (x, y, ap[i_cam], &x,&y); + ray_tracing_v2 (x,y, Ex[i_cam], I[i_cam], G[i_cam], mmp, &X1, &Y1, &Z1, &a, &b, &c); + Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + R = sqrt ((X-Ex[i_cam].x0)*(X-Ex[i_cam].x0) + + (Y-Ex[i_cam].y0)*(Y-Ex[i_cam].y0)); + + if (X > *xmax) *xmax=X; + if (X < *xmin) *xmin=X; + if (Y > *ymax) *ymax=Y; + if (Y < *ymin) *ymin=Y; + + if (R > Rmax) Rmax = R; + Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + R = sqrt ( (X-Ex[i_cam].x0)*(X-Ex[i_cam].x0) + (Y-Ex[i_cam].y0)*(Y-Ex[i_cam].y0)); - if ( X > *xmax) *xmax=X; - if ( X < *xmin) *xmin=X; - if ( Y > *ymax) *ymax=Y; - if ( Y < *ymin) *ymin=Y; - - if (R > Rmax) Rmax = R; - Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - R = sqrt ( (X-Ex[i_cam].x0)*(X-Ex[i_cam].x0) - + (Y-Ex[i_cam].y0)*(Y-Ex[i_cam].y0)); - - if ( X > *xmax) *xmax=X; - if ( X < *xmin) *xmin=X; - if ( Y > *ymax) *ymax=Y; - if ( Y < *ymin) *ymin=Y; - - if (R > Rmax) Rmax = R; - pixel_to_metric ((double) imx, (double) imy, - imx,imy, pix_x,pix_y, &x,&y, chfield); - x = x - I[i_cam].xh; - y = y - I[i_cam].yh; - correct_brown_affin (x, y, ap[i_cam], &x,&y); - ray_tracing_v2 (x,y, Ex[i_cam], I[i_cam], G[i_cam], mmp, &X1, &Y1, &Z1, &a, &b, &c); - Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - R = sqrt ( (X-Ex[i_cam].x0)*(X-Ex[i_cam].x0) - + (Y-Ex[i_cam].y0)*(Y-Ex[i_cam].y0)); - - if ( X > *xmax) *xmax=X; - if ( X < *xmin) *xmin=X; - if ( Y > *ymax) *ymax=Y; - if ( Y < *ymin) *ymin=Y; - - if (R > Rmax) Rmax = R; - Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; - R = sqrt ( (X-Ex[i_cam].x0)*(X-Ex[i_cam].x0) - + (Y-Ex[i_cam].y0)*(Y-Ex[i_cam].y0)); - - if ( X > *xmax) *xmax=X; - if ( X < *xmin) *xmin=X; - if ( Y > *ymax) *ymax=Y; - if ( Y < *ymin) *ymin=Y; - } + if (X > *xmax) *xmax=X; + if (X < *xmin) *xmin=X; + if (Y > *ymax) *ymax=Y; + if (Y < *ymin) *ymin=Y; + + if (R > Rmax) Rmax = R; + + pixel_to_metric ((double) imx, 0., imx,imy, pix_x,pix_y, &x,&y, chfield); + x = x - I[i_cam].xh; + y = y - I[i_cam].yh; + correct_brown_affin (x, y, ap[i_cam], &x,&y); + ray_tracing_v2 (x,y, Ex[i_cam], I[i_cam], G[i_cam], mmp, &X1, &Y1, &Z1, &a, &b, &c); + Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + R = sqrt ((X-Ex[i_cam].x0)*(X-Ex[i_cam].x0) + + (Y-Ex[i_cam].y0)*(Y-Ex[i_cam].y0)); + if (X > *xmax) *xmax=X; + if (X < *xmin) *xmin=X; + if (Y > *ymax) *ymax=Y; + if (Y < *ymin) *ymin=Y; + + if (R > Rmax) Rmax = R; + Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + R = sqrt ((X-Ex[i_cam].x0)*(X-Ex[i_cam].x0) + + (Y-Ex[i_cam].y0)*(Y-Ex[i_cam].y0)); + + if (X > *xmax) *xmax=X; + if (X < *xmin) *xmin=X; + if (Y > *ymax) *ymax=Y; + if (Y < *ymin) *ymin=Y; + + if (R > Rmax) Rmax = R; + pixel_to_metric ((double) imx, (double) imy, + imx,imy, pix_x,pix_y, &x,&y, chfield); + x = x - I[i_cam].xh; + y = y - I[i_cam].yh; + correct_brown_affin (x, y, ap[i_cam], &x,&y); + ray_tracing_v2 (x,y, Ex[i_cam], I[i_cam], G[i_cam], mmp, &X1, &Y1, &Z1, &a, &b, &c); + Z = Zmin; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + R = sqrt ((X-Ex[i_cam].x0)*(X-Ex[i_cam].x0) + + (Y-Ex[i_cam].y0)*(Y-Ex[i_cam].y0)); + + if (X > *xmax) *xmax=X; + if (X < *xmin) *xmin=X; + if (Y > *ymax) *ymax=Y; + if (Y < *ymin) *ymin=Y; + + if (R > Rmax) Rmax = R; + Z = Zmax; X = X1 + (Z-Z1) * a/c; Y = Y1 + (Z-Z1) * b/c; + R = sqrt ((X-Ex[i_cam].x0)*(X-Ex[i_cam].x0) + + (Y-Ex[i_cam].y0)*(Y-Ex[i_cam].y0)); + + if (X > *xmax) *xmax=X; + if (X < *xmin) *xmin=X; + if (Y > *ymax) *ymax=Y; + if (Y < *ymin) *ymin=Y; + } } diff --git a/PTV/src_c/orientation.c b/PTV/src_c/orientation.c index eb47155..66bd1a4 100644 --- a/PTV/src_c/orientation.c +++ b/PTV/src_c/orientation.c @@ -1,21 +1,21 @@ /******************************************************************* -Routine: orientation.c +Routine: orientation.c -Author/Copyright: Hans-Gerd Maas +Author/Copyright: Hans-Gerd Maas -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: summer 1988 +Creation Date: summer 1988 -Description: computes 6 parameters of exterior orientation - and different sets of additional parameters - from given approximate values, fixpoint- and image - coordinates in ASCII files +Description: computes 6 parameters of exterior orientation + and different sets of additional parameters + from given approximate values, fixpoint- and image + coordinates in ASCII files -Functiones used: img_affin, rotation_matrix, adjlib.a +Functiones used: img_affin, rotation_matrix, adjlib.a Routines contained: @@ -32,2719 +32,2581 @@ See the file license.txt for copying permission. #include "ptv.h" -void orient (interp, Ex0, I0, G0, ap0, mm, nfix, fix, crd, Ex, I, G, ap, nr) -Tcl_Interp* interp; -Exterior Ex0, *Ex; /* exterior orientation, approx and result */ -Interior I0, *I; /* interior orientation, approx and result */ -Glass G0, *G; /* glass orientation, approx and result */ -ap_52 ap0, *ap; /* add. parameters, approx and result */ -mm_np mm; /* multimedia parameters */ -int nfix; /* # of object points */ -coord_3d fix[]; /* object point data */ -coord_2d crd[]; /* image coordinates */ -int nr; /* image number for residual display */ -{ - int i,j,n, itnum, stopflag, n_obs=0; - int useflag, ccflag, scxflag, sheflag, interfflag, xhflag, yhflag, - k1flag, k2flag, k3flag, p1flag, p2flag; - int intx1, intx2, inty1, inty2; - double dm = 0.00001, drad = 0.0000001; - double X[2200][16], Xh[2200][16], y[2200], yh[2200], ident[10], - XPX[16][16], XPy[16], beta[16], Xbeta[2200], - resi[2200], omega=0, sigma0, sigmabeta[16], - P[2200], p, sumP, pixnr[4400]; - double Xp, Yp, Zp, xp, yp, xpd, ypd, r, qq; - FILE *fp1; - int dummy, multi; - - - /* read, which parameters shall be used */ - fp1 = fopen_r ("parameters/orient.par"); - fscanf (fp1,"%d", &useflag); - fscanf (fp1,"%d", &ccflag); - fscanf (fp1,"%d", &xhflag); - fscanf (fp1,"%d", &yhflag); - fscanf (fp1,"%d", &k1flag); - fscanf (fp1,"%d", &k2flag); - fscanf (fp1,"%d", &k3flag); - fscanf (fp1,"%d", &p1flag); - fscanf (fp1,"%d", &p2flag); - fscanf (fp1,"%d", &scxflag); - fscanf (fp1,"%d", &sheflag); - fscanf (fp1,"%d", &interfflag); - fclose (fp1); - - - /* init X, y (set to zero) */ - for (i=0; i<2200; i++) - { - for (j=0; j<16; j++) X[i][j] = 0; - y[i] = 0; P[i] = 0; - } - - /* init identities */ - ident[0] = I0.cc; ident[1] = I0.xh; ident[2] = I0.yh; - ident[3]=ap0.k1; ident[4]=ap0.k2; ident[5]=ap0.k3; - ident[6]=ap0.p1; ident[7]=ap0.p2; - ident[8]=ap0.scx; ident[9]=ap0.she; - - /* main loop, program runs through it, until none of the beta values - comes over a threshold and no more points are thrown out - because of their residuals */ - - puts ("\n\nbegin of iterations"); - itnum = 0; stopflag = 0; - while ((stopflag == 0) && (itnum < 20)) - { - printf ("\n\n%2d. iteration\n", ++itnum); - - for (i=0, n=0; i beta :\n"); + for (i=0; i<16; i++) { + //printf ("%10.6f ",beta[i]); + if (fabs (beta[i]) > 0.01) stopflag = 0; /* more iterations */ + } + printf ("\n\n"); + + Ex0.x0 += beta[0]; Ex0.y0 += beta[1]; Ex0.z0 += beta[2]; + Ex0.omega += beta[3]; Ex0.phi += beta[4]; Ex0.kappa += beta[5]; + I0.cc += beta[6]; I0.xh += beta[7]; I0.yh += beta[8]; + ap0.k1 += beta[9]; ap0.k2 += beta[10]; ap0.k3 += beta[11]; + ap0.p1 += beta[12]; ap0.p2 += beta[13]; + ap0.scx += beta[14]; ap0.she += beta[15]; } - n_obs = n; - - - /* identities */ - - for (i=0; i<10; i++) X[n_obs+i][6+i] = 1; - y[n_obs+0] = ident[0] - I0.cc; y[n_obs+1] = ident[1] - I0.xh; - y[n_obs+2] = ident[2] - I0.yh; y[n_obs+3] = ident[3] - ap0.k1; - y[n_obs+4] = ident[4] - ap0.k2; y[n_obs+5] = ident[5] - ap0.k3; - y[n_obs+6] = ident[6] - ap0.p1; y[n_obs+7] = ident[7] - ap0.p2; - y[n_obs+8] = ident[8] - ap0.scx; y[n_obs+9] = ident[9] - ap0.she; + /* compute residuals etc. */ + matmul (Xbeta, X, beta, n_obs, 16, 1); - - - /* weights */ - for (i=0; i beta :\n"); - for (i=0; i<16; i++) + //if(multi==0){ + for (i=0; i 0.01) stopflag = 0; /* more iterations */ + n = (int) pixnr[i/2]; + intx1 = (int) pix[nr][n].x; + inty1 = (int) pix[nr][n].y; + intx2 = (int) (intx1 + resi[i]*5000); + inty2 = (int) (inty1 + resi[i+1]*5000); + + drawcross (interp, intx1, inty1, 3, nr , "orange"); + drawvector (interp, intx1, inty1, intx2, inty2, 1, nr , "red"); } - printf ("\n\n"); - - Ex0.x0 += beta[0]; Ex0.y0 += beta[1]; Ex0.z0 += beta[2]; - Ex0.omega += beta[3]; Ex0.phi += beta[4]; Ex0.kappa += beta[5]; - I0.cc += beta[6]; I0.xh += beta[7]; I0.yh += beta[8]; - ap0.k1 += beta[9]; ap0.k2 += beta[10]; ap0.k3 += beta[11]; - ap0.p1 += beta[12]; ap0.p2 += beta[13]; - ap0.scx += beta[14]; ap0.she += beta[15]; - } - - - - /* compute residuals etc. */ - - matmul ( Xbeta, X, beta, n_obs, 16, 1); - - omega = 0; - for (i=0; i-1){ - pix[i_img][count].x=t4[3][i_img][adress[i_img]].x; - pix[i_img][count].y=t4[3][i_img][adress[i_img]].y; + FILEIN = fopen_rp (filein); // replaced fopen_r(), ad holten 12-2012 + if (! FILEIN) return; + fscanf(FILEIN, "%d\n", &num_points); + for (i=0;i=n_img; j--) address[j] = -1; + + // then fill in crd stuff + for (i_img=0;i_img-1) { + pix[i_img][count].x=t4[3][i_img][address[i_img]].x; + pix[i_img][count].y=t4[3][i_img][address[i_img]].y; } else{ - pix[i_img][count].x=-999; - pix[i_img][count].y=-999; + pix[i_img][count].x=-999; + pix[i_img][count].y=-999; } - if(pix[i_img][count].x>-999 && pix[i_img][count].y>-999){ - pixel_to_metric (pix[i_img][count].x, pix[i_img][count].y, - imx,imy, pix_x, pix_y, - &crd[i_img][count].x, &crd[i_img][count].y, - chfield); + if (pix[i_img][count].x>-999 && pix[i_img][count].y>-999) { + pixel_to_metric (pix[i_img][count].x, pix[i_img][count].y, + imx,imy, pix_x, pix_y, + &crd[i_img][count].x, &crd[i_img][count].y, + chfield); } else{ - crd[i_img][count].x=-1e10; - crd[i_img][count].y=-1e10; + crd[i_img][count].x=-1e10; + crd[i_img][count].y=-1e10; } crd[i_img][count].pnr=count; } - count ++; + count++; } - fclose (FILEIN); - - } - nfix=count; + nfix = count; } -void eval_ori (db_scale,weight_scale,n_img, nfix, residual) - -double db_scale; -double weight_scale; -int n_img,nfix; -double *residual; - +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +void eval_ori (double db_scale, double weight_scale, int n_img, int nfix, double *residual) { - int i_img,i,count_inner=0,count_outer=0,a,b,pair_count=0,count_dist=0; - double xa,ya,xb,yb,temp,m,bb,d_inner=0.,d_outer=0.,av_dist=0.; - double X1,X2,Y1,Y2,Z1,Z2,dist,dist_error; + int i_img,i,count_inner=0,count_outer=0,a,b,pair_count=0,count_dist=0; + double xa,ya,xb,yb,temp,m,bb,d_inner=0.,d_outer=0.,av_dist=0.; + double X1,X2,Y1,Y2,Z1,Z2,dist,dist_error; - for(i=0;i-999 && crd[b][i].x>-999){ - epi_mm (crd[a][i].x,crd[a][i].y, - Ex[a], I[a], G[a], Ex[b], I[b], G[b], mmp, - &xa, &ya, &xb, &yb); - if (xa == xb) xa += 1e-10; - m = (yb-ya)/(xb-xa); bb = ya - m*xa; - - if (xa > xb){ - temp = xa; xa = xb; xb = temp; - } - if (ya > yb){ - temp = ya; ya = yb; yb = temp; - } + for (a=0; a-999 && crd[b][i].x>-999) { + epi_mm (crd[a][i].x,crd[a][i].y, + Ex[a], I[a], G[a], Ex[b], I[b], G[b], mmp, + &xa, &ya, &xb, &yb); + if (xa == xb) xa += 1e-10; + m = (yb-ya)/(xb-xa); bb = ya - m*xa; + + if (xa > xb) { + temp = xa; xa = xb; xb = temp; + } + if (ya > yb) { + temp = ya; ya = yb; yb = temp; + } count_inner++; - d_inner += fabs ((crd[b][i].y - m*crd[b][i].x - bb) / sqrt(m*m+1)); + d_inner += fabs ((crd[b][i].y - m*crd[b][i].x - bb) / sqrt(m*m+1)); } } } - d_inner/=(double)count_inner; - d_outer+=d_inner; + d_inner /= (double)count_inner; + d_outer += d_inner; count_outer++; - ///here I could introduce penalty for scale + /// here I could introduce penalty for scale pair_count++; - if(pair_count==2){ - pair_count=0; + if (pair_count == 2) { + pair_count = 0; /* hack due to problems with approx in det_lsq: */ - X1 = 0.0; Y1 = 0.0; Z1 = (Zmin_lay[0]+Zmax_lay[0])/2.0; - for (i_img=0; i_img-999){ - x = crd[0][i].x - I[0].xh; - y = crd[0][i].y - I[0].yh; - //correct_brown_affin (x, y, ap[0], &x, &y); - ray_tracing_v2 (x,y, Ex[0], I[0], G[0], mmp, &X[0], &Y[0], &Z[0], &a[0], &b[0], &c[0]); - } - if(crd[1][i].x>-999){ - x = crd[1][i].x - I[1].xh; - y = crd[1][i].y - I[1].yh; - //correct_brown_affin (x, y, ap[1], &x, &y); - ray_tracing_v2 (x,y, Ex[1], I[1], G[1], mmp, &X[1], &Y[1], &Z[1], &a[1], &b[1], &c[1]); - } - if(crd[2][i].x>-999){ - x = crd[2][i].x - I[2].xh; - y = crd[2][i].y - I[2].yh; - //correct_brown_affin (x, y, ap[2], &x, &y); - ray_tracing_v2 (x,y, Ex[2], I[2], G[2], mmp, &X[2], &Y[2], &Z[2], &a[2], &b[2], &c[2]); - } - if(crd[3][i].x>-999){ - x = crd[3][i].x - I[3].xh; - y = crd[3][i].y - I[3].yh; - //correct_brown_affin (x, y, ap[3], &x, &y); - ray_tracing_v2 (x,y, Ex[3], I[3], G[3], mmp, &X[3], &Y[3], &Z[3], &a[3], &b[3], &c[3]); + // replaced the next code + // if (crd[0][i].x>-999) { + // x = crd[0][i].x - I[0].xh; + // y = crd[0][i].y - I[0].yh; + // //correct_brown_affin (x, y, ap[0], &x, &y); + // ray_tracing_v2 (x,y, Ex[0], I[0], G[0], mmp, &X[0], &Y[0], &Z[0], &a[0], &b[0], &c[0]); + // } + // if (crd[1][i].x>-999) { + // x = crd[1][i].x - I[1].xh; + // y = crd[1][i].y - I[1].yh; + // //correct_brown_affin (x, y, ap[1], &x, &y); + // ray_tracing_v2 (x,y, Ex[1], I[1], G[1], mmp, &X[1], &Y[1], &Z[1], &a[1], &b[1], &c[1]); + // } + // if (crd[2][i].x>-999) { + // x = crd[2][i].x - I[2].xh; + // y = crd[2][i].y - I[2].yh; + // //correct_brown_affin (x, y, ap[2], &x, &y); + // ray_tracing_v2 (x,y, Ex[2], I[2], G[2], mmp, &X[2], &Y[2], &Z[2], &a[2], &b[2], &c[2]); + // } + // if (crd[3][i].x>-999) { + // x = crd[3][i].x - I[3].xh; + // y = crd[3][i].y - I[3].yh; + // //correct_brown_affin (x, y, ap[3], &x, &y); + // ray_tracing_v2 (x,y, Ex[3], I[3], G[3], mmp, &X[3], &Y[3], &Z[3], &a[3], &b[3], &c[3]); + // } + // by ad holten, 12-2012 + for (j=0; j<4; j++) { + if (crd[j][i].x > -999) { + x = crd[j][i].x - I[j].xh; + y = crd[j][i].y - I[j].yh; + // correct_brown_affin (x, y, ap[3], &x, &y); + ray_tracing_v2 (x,y, Ex[j], I[j], G[j], mmp, &X[j], &Y[j], &Z[j], &a[j], &b[j], &c[j]); + } } count_inner=0; - X_pos=0.;Y_pos=0.;Z_pos=0.; - for (n=0;n-999 && crd[m][i].x>-999){ - mid_point(X[n],Y[n],Z[n],a[n],b[n],c[n],X[m],Y[m],Z[m],a[m],b[m],c[m],&dist,&XX,&YY,&ZZ); + X_pos=0.; Y_pos=0.; Z_pos=0.; + for (n=0; n-999 && crd[m][i].x>-999) { + mid_point(X[n],Y[n],Z[n],a[n],b[n],c[n],X[m],Y[m],Z[m],a[m],b[m],c[m],&dist,&XX,&YY,&ZZ); count_inner++; - d_inner += dist; - X_pos+=XX;Y_pos+=YY;Z_pos+=ZZ; + d_inner += dist; + X_pos+=XX; Y_pos+=YY; Z_pos+=ZZ; } } } - d_inner/=(double)count_inner; - X_pos/=(double)count_inner;Y_pos/=(double)count_inner;Z_pos/=(double)count_inner; + d_inner /= (double)count_inner; + X_pos /= (double)count_inner; Y_pos /= (double)count_inner; Z_pos /= (double)count_inner; //end of new det_lsq - if(Z_pos>G[0].vec_z){ - d_inner=Z_pos-G[0].vec_z+d_inner; - } + if(Z_pos > G[0].vec_z) + d_inner = Z_pos-G[0].vec_z+d_inner; + *d_outer +=d_inner; count_outer++; if(pair_count==0) { - X1=X_pos;Y1=Y_pos;Z1=Z_pos; + X1 = X_pos; Y1 = Y_pos; Z1 = Z_pos; } if(pair_count==1) { - X2=X_pos;Y2=Y_pos;Z2=Z_pos; + X2 = X_pos; Y2 = Y_pos; Z2 = Z_pos; } ///here I introduce penalty for scale pair_count++; if(pair_count==2){ - pair_count=0; - dist=sqrt(pow(X2-X1,2.)+pow(Y2-Y1,2.)+pow(Z2-Z1,2.)); - av_dist+=dist; - if(dist beta :\n"); + for (i=0; i 0.0001) stopflag = 0; /* more iterations */////Achtung + if (fabs (beta[i]) > 0.01) convergeflag = 0; + } + //printf ("\n\n"); - stopflag = 1; - convergeflag = 1; - //puts ("\n==> beta :\n"); - for (i=0; i 0.0001) stopflag = 0; /* more iterations */////Achtung - if (fabs (beta[i]) > 0.01) convergeflag = 0; + resi[i] = Xbeta[i] - y[i]; + omega += resi[i] * P[i] * resi[i]; + } + sigma0 = sqrt (omega / (n_obs - numbers)); + + for (i=0; i beta :\n"); - for (i=0; i 0.0001) stopflag = 0; /* more iterations */////Achtung - if (fabs (beta[i]) > 0.01) convergeflag = 0; - } - //printf ("\n\n"); + stopflag = 1; + convergeflag = 1; + //puts ("\n==> beta :\n"); + for (i=0; i 0.0001) stopflag = 0; /* more iterations */////Achtung + if (fabs (beta[i]) > 0.01) convergeflag = 0; + } + //printf ("\n\n"); - if ( ! ccflag) beta[6]=0; - if ( ! xhflag) beta[7]=0; - if ( ! yhflag) beta[8]=0; - if ( ! k1flag) beta[9]=0; - if ( ! k2flag) beta[10]=0; - if ( ! k3flag) beta[11]=0; - if ( ! p1flag) beta[12]=0; - if ( ! p2flag) beta[13]=0; - if ( ! scxflag)beta[14]=0; - if ( ! sheflag) beta[15]=0; - - Ex0.x0 += beta[0]; Ex0.y0 += beta[1]; Ex0.z0 += beta[2]; - Ex0.omega += beta[3]; Ex0.phi += beta[4]; Ex0.kappa += beta[5]; - I0.cc += beta[6]; I0.xh += beta[7]; I0.yh += beta[8]; - ap0.k1 += beta[9]; ap0.k2 += beta[10]; ap0.k3 += beta[11]; - ap0.p1 += beta[12]; ap0.p2 += beta[13]; - ap0.scx += beta[14]; ap0.she += beta[15]; - if(interfflag){ - //G0.vec_x += beta[16]; - //G0.vec_y += beta[17]; - G0.vec_x+=e1_x*nGl*beta[16];G0.vec_y+=e1_y*nGl*beta[16];G0.vec_z+=e1_z*nGl*beta[16]; - G0.vec_x+=e2_x*nGl*beta[17];G0.vec_y+=e2_y*nGl*beta[17];G0.vec_z+=e2_z*nGl*beta[17]; - //G0.vec_x+=G0.vec_x*nGl*beta[18];G0.vec_y+=G0.vec_y*nGl*beta[18];G0.vec_z+=G0.vec_z*nGl*beta[18]; - } - beta[0]=beta[0]; - } - - - - /* compute residuals etc. */ - - matmul_v2 ( Xbeta, X, beta, n_obs, numbers, 1, n_obs, 19); - omega = 0; - for (i=0; i 0.1) stopflag = 0; - for (i=0, n=0; idm); } - n_obs = n; - - /* Gauss Markoff Model */ - - ata (X, XPX, n_obs, 6); - matinv (XPX, 6); - atl (XPy, X, y, n_obs, 6); - matmul (beta, XPX, XPy, 6,6,1); - - stopflag = 1; - for (i=0; i<6; i++) if (fabs (beta[i]) > 0.1) stopflag = 0; - - Ex0.x0 += beta[0]; Ex0.y0 += beta[1]; Ex0.z0 += beta[2]; - Ex0.omega += beta[3]; Ex0.phi += beta[4]; - Ex0.kappa += beta[5]; - } - - if (stopflag) - { - *Ex = Ex0; - rotation_matrix (*Ex, Ex->dm); - } - else puts ("raw orientation impossible"); + else + puts ("raw orientation impossible"); } +#endif - -void raw_orient_v3 (Ex0, I, G0, ap, mm, nfix, fix, crd, Ex,G,only_show) -Exterior Ex0, *Ex; -Interior I; -Glass G0,*G; -ap_52 ap; -mm_np mm; -int nfix; -coord_3d fix[]; -coord_2d crd[]; +void raw_orient_v3 (Exterior Ex0, Interior I, Glass G0, ap_52 ap, mm_np mm, int nfix, + coord_3d fix[], coord_2d crd[], Exterior *Ex, Glass *G, int only_show) { - double X[10][6], y[10], - XPX[6][6], XPy[6], beta[6]; - double Xp, Yp, Zp, xp, yp, xpd, ypd; - int i,j,n, itnum, stopflag, n_obs=0; - double dm = 0.0001, drad = 0.000001; - - - /* init X, y (set to zero) */ - for (i=0; i<10; i++) - { - for (j=0; j<6; j++) X[i][j] = 0; - y[i] = 0; - } - - ap.k1 = 0; ap.k2 = 0; ap.k3 = 0; ap.p1 = 0; ap.p2 = 0; - ap.scx = 1; ap.she = 0; - - - /* main loop, program runs through it, until none of the beta values - comes over a threshold and no more points are thrown out - because of their residuals */ + double X[10][6], y[10], + XPX[6][6], XPy[6], beta[6]; + double Xp, Yp, Zp, xp, yp, xpd, ypd; + int i,j,n, itnum, stopflag, n_obs=0; + double dm = 0.0001, drad = 0.000001; + + /* init X, y (set to zero) */ + for (i=0; i<10; i++) + { + for (j=0; j<6; j++) X[i][j] = 0; + y[i] = 0; + } - itnum = 0; stopflag = 0; + ap.k1 = 0; ap.k2 = 0; ap.k3 = 0; ap.p1 = 0; ap.p2 = 0; + ap.scx = 1; ap.she = 0; -///////////make a menu so one see the raw guess!!!!! - if(only_show==1) stopflag=1; -/////// Beat Lüthi 9. Mai 2007 + /* main loop, program runs through it, until none of the beta values + comes over a threshold and no more points are thrown out + because of their residuals */ - + itnum = 0; stopflag = 0; - while ((stopflag == 0) && (itnum < 20)) - { - ++itnum; + ///////////make a menu so one see the raw guess!!!!! + if(only_show==1) stopflag=1; + /////// Beat L¸thi 9. Mai 2007 - for (i=0, n=0; i 0.1 ) stopflag = 0; + } - n += 2; + Ex0.x0 += beta[0]; Ex0.y0 += beta[1]; Ex0.z0 += beta[2]; + Ex0.omega += beta[3]; Ex0.phi += beta[4]; + Ex0.kappa += beta[5]; + //G0.vec_x += beta[6];G0.vec_y += beta[7];//G0.vec_z += beta[8]; + stopflag =stopflag ; } - n_obs = n; - - /* Gauss Markoff Model */ - - ata_v2 (X, XPX, n_obs, 6, 6); - matinv (XPX, 6); - atl (XPy, X, y, n_obs, 6); - matmul (beta, XPX, XPy, 6,6,1); - stopflag = 1; - for (i=0; i<6; i++){ - if (fabs (beta[i]) > 0.1 ) stopflag = 0; - } - - Ex0.x0 += beta[0]; Ex0.y0 += beta[1]; Ex0.z0 += beta[2]; - Ex0.omega += beta[3]; Ex0.phi += beta[4]; - Ex0.kappa += beta[5]; - //G0.vec_x += beta[6];G0.vec_y += beta[7];//G0.vec_z += beta[8]; - stopflag =stopflag ; - - } - - if (stopflag) - { - *Ex = Ex0; - *G = G0; - rotation_matrix (*Ex, Ex->dm); - } - else { - puts ("raw orientation impossible"); - } + if (stopflag) + { + *Ex = Ex0; + *G = G0; + rotation_matrix (*Ex, Ex->dm); + } + else { + puts ("raw orientation impossible"); + } } -void raw_orient_v4 (Ex0, I, G0, ap, mm, nfix, fix, crd, Ex,only_show) //Beat Nov 2008 to have calibration without knowing coord of target points -//Beat Lüthi, Nov 2008 - - -Exterior Ex0[4]; -Interior I[4]; -Glass G0[4]; -ap_52 ap[4]; -mm_np mm; - -Exterior Ex[4]; - -int nfix; -coord_3d fix[]; -coord_2d crd[4][12]; - +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +void raw_orient_v4 (Exterior Ex0[4], Interior I[4], Glass G0[4], ap_52 ap[4], mm_np mm, int nfix, + coord_3d fix[], coord_2d crd[4][12], Exterior Ex[4], int only_show) +// Beat Nov 2008 to have calibration without knowing coord of target points +// Beat L¸thi, Nov 2008 { - double X[30][6], y[30], - XPX[6][6], XPy[6], beta[6]; - double Xp, Yp, Zp, xp, yp, xpd, ypd; - int i,j,n, itnum, stopflag, n_obs=0,cam,count; - double dm = 0.0001, drad = 0.000001; - - - /* init X, y (set to zero) */ - for (i=0; i<30; i++) - { - for (j=0; j<6; j++) X[i][j] = 0; - y[i] = 0; - } - for (i=0; i 0.1 ) stopflag = 0; + } + if (itnum < 5900000) stopflag=0; + + + Ex0[cam].x0 += beta[0]; Ex0[cam].y0 += beta[1]; Ex0[cam].z0 += beta[2]; + Ex0[cam].omega += beta[3]; Ex0[cam].phi += beta[4]; + Ex0[cam].kappa += beta[5]; + //G0.vec_x += beta[6];G0.vec_y += beta[7];//G0.vec_z += beta[8]; + stopflag =stopflag ; + } //end of cam loop + + count++; + if (itnum < 5900000 && count>10) { //(stopflag == 1) && + for (i=0, n=0; i 0.1 ) stopflag = 0; - } - if (itnum < 5900000) stopflag=0; - - - Ex0[cam].x0 += beta[0]; Ex0[cam].y0 += beta[1]; Ex0[cam].z0 += beta[2]; - Ex0[cam].omega += beta[3]; Ex0[cam].phi += beta[4]; - Ex0[cam].kappa += beta[5]; - //G0.vec_x += beta[6];G0.vec_y += beta[7];//G0.vec_z += beta[8]; - stopflag =stopflag ; - }//end of cam loop - - count++; - if ((itnum < 5900000) && (count>10)){ //(stopflag == 1) && - for (i=0, n=0; i 0.1 ) stopflag = 0; + } + if (itnum < 5900000) stopflag=0; + + Ex0[cam].x0 += beta[0]; Ex0[cam].y0 += beta[1]; Ex0[cam].z0 += beta[2]; + Ex0[cam].omega += beta[3]; Ex0[cam].phi += beta[4]; + Ex0[cam].kappa += beta[5]; + } //end of cam loop + + count++; + if (itnum < 5900000 && count>10) { //(stopflag == 1) && + for (i=0, n=0; i 0.1 ) stopflag = 0; - } - if (itnum < 5900000) stopflag=0; - - - Ex0[cam].x0 += beta[0]; Ex0[cam].y0 += beta[1]; Ex0[cam].z0 += beta[2]; - Ex0[cam].omega += beta[3]; Ex0[cam].phi += beta[4]; - Ex0[cam].kappa += beta[5]; - }//end of cam loop - - count++; - if ((itnum < 5900000) && (count>10)){ //(stopflag == 1) && - for (i=0, n=0; ivec_x=dot*b[0]; Gq->vec_y=dot*b[1]; Gq->vec_z=dot*b[2]; -} \ No newline at end of file +} +#endif \ No newline at end of file diff --git a/PTV/src_c/peakfitting.c b/PTV/src_c/peakfitting.c index f224af6..266cd2b 100644 --- a/PTV/src_c/peakfitting.c +++ b/PTV/src_c/peakfitting.c @@ -1,14 +1,14 @@ /**************************************************************************** -Routine: peakfitting.c +Routine: peakfitting.c -Author/Copyright: Hans-Gerd Maas +Author/Copyright: Hans-Gerd Maas -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: Feb. 1990 +Creation Date: Feb. 1990 Description: @@ -22,406 +22,397 @@ See the file license.txt for copying permission. #include "ptv.h" -int peak_fit_new ( interp, img, par_file, xmin,xmax,ymin,ymax, pix, nr) +void check_touch (peak *tpeak, int p1, int p2); -Tcl_Interp* interp; -unsigned char *img; /* image data */ -char par_file[]; /* name of parameter file */ -int xmin,xmax,ymin,ymax; /* search area */ -target pix[]; /* pixel coord array, global */ -int nr; /* image number for display */ -/* newest peak fitting technique for particle coordinate determination */ -/* labeling with discontinuity, +int peak_fit_new (Tcl_Interp* interp, unsigned char *img, char par_file[], + int xmin, int xmax, int ymin, int ymax, target pix[], int nr) +/* + unsigned char *img image data + char par_file[] name of parameter file + int xmin,xmax,ymin,ymax search area + target pix[] pixel coord array, global + int nr image number for display +*/ +/* newest peak fitting technique for particle coordinate determination */ +/* labeling with discontinuity, reunification with distance and profile criteria */ { - int n_peaks=0; /* # of peaks detected */ - int n_wait; /* size of waitlist for connectivity */ - int x8[8], y8[8]; /* neighbours for connectivity */ - int p2; /* considered point number */ - int sumg_min, gvthres[4], thres, disco, nxmin,nxmax, nymin,nymax, nnmin, nnmax,rel_disc,dummy; - /* parameters for target acceptance */ - int pnr, sumg, xn, yn; /* collecting variables for center of gravity */ - int n_target=0; /* # of targets detected */ - int intx1, inty1; /* pixels for profile test and crosses */ - int unify; /* flag for unification of targets */ - int unified=0; /* # of unified targets */ - int non_unified=0; /* # of tested, but not unified targets */ - register int i, j, k, l, m, n; /* loop variables */ - unsigned char gv, gvref; /* current and reference greyvalue */ - unsigned char gv1, gv2; /* greyvalues for profile test */ - void check_touch (); /* marks touch events */ - double x1,x2,y1,y2,s12; /* values for profile test */ - double x, y; /* preliminary coordinates */ - short *label_img; /* target number labeling */ - peak *peaks, *ptr_peak; /* detected peaks */ - targpix waitlist[2048]; /* pix to be tested for connectivity */ - FILE *fpp; /* parameter file pointer */ - - - /* read image name, threshold and shape limits from parameter file */ - -/*------------------------------------------------------------------------*/ - - fpp = fopen_r (par_file); - fscanf (fpp, "%d", &gvthres[0]); /* threshold for binarization 1.image */ - fscanf (fpp, "%d", &gvthres[1]); /* threshold for binarization 2.image */ - fscanf (fpp, "%d", &gvthres[2]); /* threshold for binarization 3.image */ - fscanf (fpp, "%d", &gvthres[3]); /* threshold for binarization 4.image */ - fscanf (fpp, "%d", &disco); /* max discontinuity */ - fscanf (fpp, "%d %d", &nnmin, &nnmax); /* min. and max. number of */ - fscanf (fpp, "%d %d", &nxmin, &nxmax); /* pixels per target, */ - fscanf (fpp, "%d %d", &nymin, &nymax); /* abs, in x, in y */ - fscanf (fpp, "%d", &sumg_min); /* min. sumg */ - fscanf (fpp, "%d", &cr_sz); /* size of crosses */ - fscanf (fpp, "%d", &dummy); /* size of crosses */ - fscanf (fpp, "%d", &dummy); /* size of crosses */ - fscanf (fpp, "%d", &rel_disc); - fclose (fpp); - - /* give thres value refering to image number */ - thres=gvthres[nr]; - - /*------------------------------------------------------------------------*/ - - /* allocate memory */ - - /*------------------------------------------------------------------------*/ - - label_img = (short *) calloc (imgsize, sizeof(short)); - peaks = (peak *) malloc (4*nmax * sizeof(peak)); - ptr_peak = peaks; - - - /*------------------------------------------------------------------------*/ - - /* 1.: connectivity analysis with peak search and discontinuity criterion */ - - /*------------------------------------------------------------------------*/ - - puts("Searching local maxima, connectivity analysis, peak factor 2 set"); - - for (i=ymin; i= *(img + n-1) - && gv >= *(img + n+1) - && gv >= *(img + n-imx) - && gv >= *(img + n+imx) - && gv >= *(img + n-imx-1) - && gv >= *(img + n+imx-1) - && gv >= *(img + n-imx+1) - && gv >= *(img + n+imx+1) ) + int n_peaks=0; /* # of peaks detected */ + int n_wait; /* size of waitlist for connectivity */ + int x8[8], y8[8]; /* neighbours for connectivity */ + int p2; /* considered point number */ + int sumg_min, gvthres[4], thres, disco, nxmin,nxmax, nymin,nymax, nnmin, nnmax,rel_disc,dummy; + /* parameters for target acceptance */ + int pnr, sumg, xn, yn; /* collecting variables for center of gravity */ + int n_target=0; /* # of targets detected */ + int intx1, inty1; /* pixels for profile test and crosses */ + int unify; /* flag for unification of targets */ + int unified=0; /* # of unified targets */ + int non_unified=0; /* # of tested, but not unified targets */ + register int i, j, k, l, m, n; /* loop variables */ + unsigned char gv, gvref; /* current and reference greyvalue */ + unsigned char gv1, gv2; /* greyvalues for profile test */ + void check_touch (); /* marks touch events */ + double x1,x2,y1,y2,s12; /* values for profile test */ + double x, y; /* preliminary coordinates */ + short *label_img; /* target number labeling */ + peak *peaks, *ptr_peak; /* detected peaks */ + targpix waitlist[2048]; /* pix to be tested for connectivity */ + FILE *fpp; /* parameter file pointer */ + + /* read image name, threshold and shape limits from parameter file */ + + /*------------------------------------------------------------------------*/ + + fpp = fopen_rp (par_file); // replaced fopen_r(), ad holten 12-2012 + if (!fpp) return 0; + fscanf (fpp, "%d", &gvthres[0]); /* threshold for binarization 1.image */ + fscanf (fpp, "%d", &gvthres[1]); /* threshold for binarization 2.image */ + fscanf (fpp, "%d", &gvthres[2]); /* threshold for binarization 3.image */ + fscanf (fpp, "%d", &gvthres[3]); /* threshold for binarization 4.image */ + fscanf (fpp, "%d", &disco); /* max discontinuity */ + fscanf (fpp, "%d %d", &nnmin, &nnmax); /* min. and max. number of */ + fscanf (fpp, "%d %d", &nxmin, &nxmax); /* pixels per target, */ + fscanf (fpp, "%d %d", &nymin, &nymax); /* abs, in x, in y */ + fscanf (fpp, "%d", &sumg_min); /* min. sumg */ + fscanf (fpp, "%d", &cr_sz); /* size of crosses */ + fscanf (fpp, "%d", &dummy); + fscanf (fpp, "%d", &dummy); + fscanf (fpp, "%d", &rel_disc); + fclose (fpp); + + /* give thres value refering to image number */ + thres = gvthres[nr]; + + /*------------------------------------------------------------------------*/ + /* allocate memory */ + /*------------------------------------------------------------------------*/ + + label_img = (short*) calloc (imgsize, sizeof(short)); + peaks = (peak*) malloc (4*nmax * sizeof(peak)); + ptr_peak = peaks; + + /*------------------------------------------------------------------------*/ + /* 1.: connectivity analysis with peak search and discontinuity criterion */ + /*------------------------------------------------------------------------*/ + + puts("Searching local maxima, connectivity analysis, peak factor 2 set"); + + //for (i=ymin; i imy-1) ymax = imy-1; + + for (i=ymin; ipos = n; - ptr_peak->status = 1; - ptr_peak->xmin = j; ptr_peak->xmax = j; - ptr_peak->ymin = i; ptr_peak->ymax = i; - ptr_peak->unr = 0; - ptr_peak->n = 0; - ptr_peak->sumg = 0; - ptr_peak->x = 0; - ptr_peak->y = 0; - ptr_peak->n_touch = 0; - for (k=0; k<4; k++) ptr_peak->touch[k] = 0; - ptr_peak++; - - waitlist[0].x = j; waitlist[0].y = i; n_wait = 1; - - while (n_wait > 0) - { - gvref = *(img + imx*(waitlist[0].y) + (waitlist[0].x)); - - x8[0] = waitlist[0].x - 1; y8[0] = waitlist[0].y; - x8[1] = waitlist[0].x + 1; y8[1] = waitlist[0].y; - x8[2] = waitlist[0].x; y8[2] = waitlist[0].y - 1; - x8[3] = waitlist[0].x; y8[3] = waitlist[0].y + 1; - /* - x8[4] = waitlist[0].x - 1; y8[4] = waitlist[0].y - 1; - x8[5] = waitlist[0].x + 1; y8[5] = waitlist[0].y - 1; - x8[6] = waitlist[0].x - 1; y8[6] = waitlist[0].y + 1; - x8[7] = waitlist[0].x + 1; y8[7] = waitlist[0].y + 1; - */ - - /* - for (k=0; k<8; k++) - */ - for (k=0; k<4; k++) + n = i*imx + j; + + /* compare with threshold */ + gv = *(img + n); if (gv <= 2*thres) continue; + + /* skip already labeled pixel */ + if (*(label_img + n) != 0) continue; + + /* check, wether pixel is a local maximum */ + if ( gv >= *(img + n-1 ) + && gv >= *(img + n+1 ) + && gv >= *(img + n-imx ) + && gv >= *(img + n+imx ) + && gv >= *(img + n-imx-1) + && gv >= *(img + n+imx-1) + && gv >= *(img + n-imx+1) + && gv >= *(img + n+imx+1) ) { - yn = y8[k]; - xn = x8[k]; - if (xn<0 || xn>imx || yn<0 || yn>imy) continue; - n = imx*yn + xn; - if (*(label_img + n) != 0) continue; - gv = *(img + n); - - /* conditions for threshold, discontinuity, image borders */ - /* and peak fitting */ - if ( (gv > thres) - && (xn>=xmin)&&(xn=ymin)&&(yn= *(img + imx*(yn-1) + xn)) - && (gvref + disco >= *(img + imx*(yn+1) + xn)) - && (gvref + disco >= *(img + imx*yn + (xn-1))) - && (gvref + disco >= *(img + imx*yn + (xn+1))) - /* - && (gvref + disco >= *(img + imx*(yn-1) + xn-1)) - && (gvref + disco >= *(img + imx*(yn-1) + xn+1)) - && (gvref + disco >= *(img + imx*(yn+1) + (xn-1))) - && (gvref + disco >= *(img + imx*(yn+1) + (xn+1))) */ - ) - { - *(label_img + imx*yn + xn) = n_peaks; - - waitlist[n_wait].x = xn; waitlist[n_wait].y = yn; - n_wait++; - } + /* label peak in label_img, initialize peak */ + n_peaks++; + *(label_img + n) = n_peaks; + ptr_peak->pos = n; + ptr_peak->status = 1; + ptr_peak->xmin = j; ptr_peak->xmax = j; + ptr_peak->ymin = i; ptr_peak->ymax = i; + ptr_peak->unr = 0; + ptr_peak->n = 0; + ptr_peak->sumg = 0; + ptr_peak->x = 0; + ptr_peak->y = 0; + ptr_peak->n_touch = 0; + for (k=0; k<4; k++) ptr_peak->touch[k] = 0; + ptr_peak++; + + waitlist[0].x = j; waitlist[0].y = i; n_wait = 1; + + while (n_wait > 0) + { + gvref = *(img + imx*(waitlist[0].y) + (waitlist[0].x)); + + x8[0] = waitlist[0].x - 1; y8[0] = waitlist[0].y; + x8[1] = waitlist[0].x + 1; y8[1] = waitlist[0].y; + x8[2] = waitlist[0].x; y8[2] = waitlist[0].y - 1; + x8[3] = waitlist[0].x; y8[3] = waitlist[0].y + 1; + /* + x8[4] = waitlist[0].x - 1; y8[4] = waitlist[0].y - 1; + x8[5] = waitlist[0].x + 1; y8[5] = waitlist[0].y - 1; + x8[6] = waitlist[0].x - 1; y8[6] = waitlist[0].y + 1; + x8[7] = waitlist[0].x + 1; y8[7] = waitlist[0].y + 1; + */ + + /* + for (k=0; k<8; k++) + */ + for (k=0; k<4; k++) + { + yn = y8[k]; + xn = x8[k]; + // if (xn<0 || xn>imx || yn<0 || yn>imy) continue; // Bug repaired, ad holten, 12-2012 + if (xn<1 || xn>imx-2 || yn<1 || yn>imy-2) continue; + + n = imx*yn + xn; + if (*(label_img + n) != 0) continue; + gv = *(img + n); + + /* conditions for threshold, discontinuity and peak fitting */ + if ( (gv > thres) + && (gv <= gvref+disco) + // removed the image borders test from Beat, ad holten, 2012 + // && (xn>=xmin)&&(xn=ymin)&&(yn= *(img + imx*(yn-1) + xn)) + && (gvref + disco >= *(img + imx*(yn+1) + xn)) + && (gvref + disco >= *(img + imx*yn + (xn-1))) + && (gvref + disco >= *(img + imx*yn + (xn+1))) + /* + && (gvref + disco >= *(img + imx*(yn-1) + xn-1)) + && (gvref + disco >= *(img + imx*(yn-1) + xn+1)) + && (gvref + disco >= *(img + imx*(yn+1) + (xn-1))) + && (gvref + disco >= *(img + imx*(yn+1) + (xn+1))) */ + ) + { + *(label_img + imx*yn + xn) = n_peaks; + waitlist[n_wait].x = xn; waitlist[n_wait].y = yn; + n_wait++; + } + } + + n_wait--; + for (m=0; m 0) + for (i=ymin; in++; - ptr_peak->sumg += gv; - ptr_peak->x += (j * gv); - ptr_peak->y += (i * gv); - if (j < ptr_peak->xmin) ptr_peak->xmin = j; - if (j > ptr_peak->xmax) ptr_peak->xmax = j; - if (i < ptr_peak->ymin) ptr_peak->ymin = i; - if (i > ptr_peak->ymax) ptr_peak->ymax = i; - - - /* get touch events */ - - if (i>0 && j>1) check_touch (ptr_peak, pnr, *(label_img+n-imx-1)); - if (i>0) check_touch (ptr_peak, pnr, *(label_img+n-imx)); - if (i>0 && j0) check_touch (ptr_peak, pnr, *(label_img+n-1)); - if (j0) check_touch (ptr_peak, pnr, *(label_img+n+imx-1)); - if (i= n_peaks) continue; /* workaround memory overwrite problem */ - if ( p2 <0) continue; /* workaround memory overwrite problem */ - if (peaks[p2].unr != 0) continue; /* target already unified */ - - /* point 2 */ - x2 = peaks[p2].x / peaks[p2].sumg; - y2 = peaks[p2].y / peaks[p2].sumg; - - gv2 = *(img + peaks[p2].pos); - - s12 = sqrt ((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1)); - - /* consider profile dot for dot */ - /* if any points is by more than disco below profile, do not unify */ - if (s12 < 2.0) unify = 1; - else for (unify=1, l=1; l peaks[p2].xmax) peaks[p2].xmax = peaks[i].xmax; - if (peaks[i].ymax > peaks[p2].ymax) peaks[p2].ymax = peaks[i].ymax; + if (*(label_img+n) > 0) + { + /* process pixel */ + pnr = *(label_img+n); + gv = *(img+n); + ptr_peak = peaks + pnr - 1; + ptr_peak->n++; + ptr_peak->sumg += gv; + ptr_peak->x += (j * gv); + ptr_peak->y += (i * gv); + if (j < ptr_peak->xmin) ptr_peak->xmin = j; + if (j > ptr_peak->xmax) ptr_peak->xmax = j; + if (i < ptr_peak->ymin) ptr_peak->ymin = i; + if (i > ptr_peak->ymax) ptr_peak->ymax = i; + + /* get touch events */ + // bugs repaired, ad holten 12-2012 i = Y-axis, j = X-axis !!! + // if (i>0 && j>1) check_touch (ptr_peak, pnr, *(label_img+n-imx-1)); + // if (i>0) check_touch (ptr_peak, pnr, *(label_img+n-imx)); + // if (i>0 && j0) check_touch (ptr_peak, pnr, *(label_img+n-1)); + // if (j0) check_touch (ptr_peak, pnr, *(label_img+n+imx-1)); + // if (i0 && j>0) check_touch (ptr_peak, pnr, *(label_img+n-imx-1)); + if (i>0) check_touch (ptr_peak, pnr, *(label_img+n-imx)); + if (i>0 && j0) check_touch (ptr_peak, pnr, *(label_img+n-1)); + if (j0) check_touch (ptr_peak, pnr, *(label_img+n+imx-1)); + if (i 32) continue; - if (peaks[i].ymin == ymin && (xmax-xmin) > 32) continue; - if (peaks[i].xmax == xmax-1 && (xmax-xmin) > 32) continue; - if (peaks[i].ymax == ymax-1 && (xmax-xmin) > 32) continue; + /* profile criterion */ + /* point 1 */ + x1 = peaks[i].x / peaks[i].sumg; + y1 = peaks[i].y / peaks[i].sumg; + gv1 = *(img + peaks[i].pos); - if ( peaks[i].unr == 0 - && peaks[i].sumg > sumg_min - && (peaks[i].xmax - peaks[i].xmin + 1) >= nxmin - && (peaks[i].ymax - peaks[i].ymin + 1) >= nymin - && (peaks[i].xmax - peaks[i].xmin) < nxmax - && (peaks[i].ymax - peaks[i].ymin) < nymax - && peaks[i].n >= nnmin - && peaks[i].n <= nnmax) - { - sumg = peaks[i].sumg; - - /* target coordinates */ - pix[n_target].x = 0.5 + peaks[i].x / sumg; - pix[n_target].y = 0.5 + peaks[i].y / sumg; - - /* draw cross */ - if (display) - drawcross (interp, (int) pix[n_target].x, (int) pix[n_target].y, - cr_sz, nr, "blue"); - - /* target shape parameters */ - pix[n_target].sumg = sumg; - pix[n_target].n = peaks[i].n; - pix[n_target].nx = peaks[i].xmax - peaks[i].xmin + 1; - pix[n_target].ny = peaks[i].ymax - peaks[i].ymin + 1; - pix[n_target].tnr = -1; - pix[n_target].pnr = n_target++; + /* consider all touching points */ + for (j=0; j= n_peaks) continue; /* workaround memory overwrite problem */ + if (p2 < 0) continue; /* workaround memory overwrite problem */ + if (peaks[p2].unr != 0) continue; /* target already unified */ + + /* point 2 */ + x2 = peaks[p2].x / peaks[p2].sumg; + y2 = peaks[p2].y / peaks[p2].sumg; + + gv2 = *(img + peaks[p2].pos); + + s12 = sqrt ((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1)); + + /* consider profile dot for dot */ + /* if any points is by more than disco below profile, do not unify */ + if (s12 < 2.0) + unify = 1; + else { + for (unify=1, l=1; l peaks[p2].xmax) peaks[p2].xmax = peaks[i].xmax; + if (peaks[i].ymax > peaks[p2].ymax) peaks[p2].ymax = peaks[i].ymax; + } } - } + /*------------------------------------------------------------------------*/ + /* 4.: process targets */ + /*------------------------------------------------------------------------*/ + puts("Calculate centers of gravity"); - /* get number of touch events */ - if (examine==10) - { - for (x=0,i=0; i 32) continue; + if (peaks[i].ymin == ymin && (xmax-xmin) > 32) continue; + if (peaks[i].xmax == xmax-1 && (xmax-xmin) > 32) continue; + if (peaks[i].ymax == ymax-1 && (xmax-xmin) > 32) continue; + + if ( peaks[i].unr == 0 + && peaks[i].sumg > sumg_min + && (peaks[i].xmax - peaks[i].xmin + 1) >= nxmin + && (peaks[i].ymax - peaks[i].ymin + 1) >= nymin + && (peaks[i].xmax - peaks[i].xmin) < nxmax + && (peaks[i].ymax - peaks[i].ymin) < nymax + && peaks[i].n >= nnmin + && peaks[i].n <= nnmax) + { + sumg = peaks[i].sumg; + + /* target coordinates */ + pix[n_target].x = 0.5 + peaks[i].x / sumg; + pix[n_target].y = 0.5 + peaks[i].y / sumg; + + /* draw cross */ + if (display) + drawcross(interp, (int) pix[n_target].x, (int) pix[n_target].y, + cr_sz, nr, "blue"); + + /* target shape parameters */ + pix[n_target].sumg = sumg; + pix[n_target].n = peaks[i].n; + pix[n_target].nx = peaks[i].xmax - peaks[i].xmin + 1; + pix[n_target].ny = peaks[i].ymax - peaks[i].ymin + 1; + pix[n_target].tnr = -1; + pix[n_target].pnr = n_target++; + } } - x /= n_target; - printf ("Average number of pix per target: %6.3f\n", x); - for (sumg=0,i=0; in_touch; m++) - { - if (tpeak->touch[m] == p2) done = 1; - } - - /* mark touch event */ - if (done == 0) - { - tpeak->touch[tpeak->n_touch] = p2; - tpeak->n_touch++; - /* don't allow for more than 4 touchs */ - if (tpeak->n_touch > 3) tpeak->n_touch = 3; - } + int m, done; + + if (p2 == 0) return; /* p2 not labeled */ + if (p2 == p1) return; /* p2 belongs to p1 */ + + /* check wether p1, p2 are already marked as touching */ + for (done=0, m=0; mn_touch; m++) + if (tpeak->touch[m] == p2) done = 1; + + /* mark touch event */ + if (done == 0) { + tpeak->touch[tpeak->n_touch] = p2; + tpeak->n_touch++; + /* don't allow for more than 4 touchs */ + if (tpeak->n_touch > 3) + tpeak->n_touch = 3; + } } diff --git a/PTV/src_c/pointpos.c b/PTV/src_c/pointpos.c index 0c767cb..6a7a5eb 100644 --- a/PTV/src_c/pointpos.c +++ b/PTV/src_c/pointpos.c @@ -1,1032 +1,999 @@ /**************************************************************************** -Routine: pointpos.c +Routine: pointpos.c -Author/Copyright: Hans-Gerd Maas +Author/Copyright: Hans-Gerd Maas -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: July 1988, June 1989 +Creation Date: July 1988, June 1989 -Description: point positioning with least squares adjustment - (multimedia, 2 or 3 channels) +Description: point positioning with least squares adjustment + (multimedia, 2 or 3 channels) -Routines contained: +Routines contained: ****************************************************************************/ + + #include "ptv.h" -void det_lsq_old (Ex, I, ap, mm, x1, y1, x2, y2, x3, y3, x4, y4, Xp, Yp, Zp) -Exterior Ex[4]; -Interior I[4]; -ap_52 ap[4]; -mm_np mm; -double x1, y1, x2, y2, x3, y3, x4, y4, *Xp,*Yp,*Zp; +#ifdef EVER_CALLED // added, ad holten 12-2012 +void det_lsq_old (Exterior Ex[4], Interior I[4], ap_52 ap[4], mm_np mm, + double x1, double y1, double x2, double y2, + double x3, double y3, double x4, double y4, + double *Xp, double *Yp, double *Zp) { - int i, j, itnum, stopflag, n_obs; - double X[8][3], XtX[3][3], y[8], Xty[3], beta[3], - Xbeta[8], edach[8], omega=0., sigma0, sigmabeta[3]; - double Zx, Zy, N, mmf; - double xp1, yp1; - double xp[4], yp[4]; - - double multimed_r_nlay (); - - /* static parameters for examination of Qxx, Qvv */ - static int Sn; - static double Ssigma0, SQxx[3][3], SQvv[8][8]; - double Xt[3][8], buf[3][8], Qvv[8][8]; - - xp[0] = x1; xp[1] = x2; xp[2] = x3; xp[3] = x4; - yp[0] = y1; yp[1] = y2; yp[2] = y3; yp[3] = y4; - - /* approximate values: rt_is with the uncorrected image coordinates */ - /* of the first 2 rays */ - - /* - for (i=0; i -1e10 && yp[i] > -1e10) - { - ray_tracing (xp[i], yp[i], Ex[i], I[i], - mm, &X1, &Y1, &Z1, &a1, &b1, &c1); break; - } - for (i=i+1; i -1e10 && yp[i] > -1e10) - { - ray_tracing (xp[i], yp[i], Ex[i], I[i], - mm, &X2, &Y2, &Z2, &a2, &b2, &c2); break; - } - - intersect_rt (X1,Y1,Z1,a1,b1,c1, X2,Y2,Z2,a2,b2,c2, Xp, Yp, Zp); - */ - - /* hack due to problems with approx in det_lsq: */ - - /************************************************************************/ - /* least squares adjustment */ - itnum = 0; stopflag = 0; - while ((stopflag == 0) && (itnum < 10)) - { - /********************************************************************/ - /* X-matrix + observation vector from image coordinates */ - - for (i=0; i<4; i++) + int i, j, itnum, stopflag, n_obs; + double X[8][3], XtX[3][3], y[8], Xty[3], beta[3], + Xbeta[8], edach[8], omega=0., sigma0, sigmabeta[3]; + double Zx, Zy, N, mmf; + double xp1, yp1; + double xp[4], yp[4]; + + double multimed_r_nlay (); + + /* static parameters for examination of Qxx, Qvv */ + static int Sn; + static double Ssigma0, SQxx[3][3], SQvv[8][8]; + double Xt[3][8], buf[3][8], Qvv[8][8]; + + xp[0] = x1; xp[1] = x2; xp[2] = x3; xp[3] = x4; + yp[0] = y1; yp[1] = y2; yp[2] = y3; yp[3] = y4; + + // /* approximate values: rt_is with the uncorrected image coordinates */ + // /* of the first 2 rays */ + + // /* + // for (i=0; i -1e10 && yp[i] > -1e10) + // { + // ray_tracing (xp[i], yp[i], Ex[i], I[i], + // mm, &X1, &Y1, &Z1, &a1, &b1, &c1); break; + // } + // for (i=i+1; i -1e10 && yp[i] > -1e10) + // { + // ray_tracing (xp[i], yp[i], Ex[i], I[i], + // mm, &X2, &Y2, &Z2, &a2, &b2, &c2); break; + // } + + // intersect_rt (X1,Y1,Z1,a1,b1,c1, X2,Y2,Z2,a2,b2,c2, Xp, Yp, Zp); + // */ + + // /* hack due to problems with approx in det_lsq: */ + + /************************************************************************/ + /* least squares adjustment */ + itnum = 0; stopflag = 0; + while (stopflag == 0 && itnum < 10) { - if (xp[i] > -1e10 && yp[i] > -1e10) - { - /* derivatives d(x(i),y(i)) / d(X,Y,Z) (Kraus) */ - - Zx = Ex[i].dm[0][0] * (*Xp - Ex[i].x0) - + Ex[i].dm[1][0] * (*Yp - Ex[i].y0) - + Ex[i].dm[2][0] * (*Zp - Ex[i].z0); - Zy = Ex[i].dm[0][1] * (*Xp - Ex[i].x0) - + Ex[i].dm[1][1] * (*Yp - Ex[i].y0) - + Ex[i].dm[2][1] * (*Zp - Ex[i].z0); - N = Ex[i].dm[0][2] * (*Xp - Ex[i].x0) - + Ex[i].dm[1][2] * (*Yp - Ex[i].y0) - + Ex[i].dm[2][2] * (*Zp - Ex[i].z0); - - for (j=0; j<3; j++) - X[2*i][j] = ((-I[i].cc)/(N*N)) - * (N*Ex[i].dm[j][0] - Zx*Ex[i].dm[j][2]); - for (j=0; j<3; j++) - X[2*i+1][j] = ((-I[i].cc)/(N*N)) - * (N*Ex[i].dm[j][1] - Zy*Ex[i].dm[j][2]); - - /* multimedia factor (radial shift) */ - - mmf = multimed_r_nlay (Ex[i], mm, *Xp, *Yp, *Zp); - - X[2*i][0] *= mmf; X[2*i][1] *= mmf; - X[2*i+1][0] *= mmf; X[2*i+1][1] *= mmf; - - /* observation vector */ - img_coord_old (*Xp,*Yp,*Zp, Ex[i], I[i], ap[i], mm, &xp1, &yp1); - - y[2*i] = xp[i] - xp1; y[2*i+1] = yp[i] - yp1; - } + /********************************************************************/ + /* X-matrix + observation vector from image coordinates */ - else /* set values to 0 */ - { - for (j=0; j<3; j++) + for (i=0; i<4; i++) { - X[2*i][j] = 0; X[2*i+1][j] = 0; + if (xp[i] > -1e10 && yp[i] > -1e10) + { + /* derivatives d(x(i),y(i)) / d(X,Y,Z) (Kraus) */ + + Zx = Ex[i].dm[0][0] * (*Xp - Ex[i].x0) + + Ex[i].dm[1][0] * (*Yp - Ex[i].y0) + + Ex[i].dm[2][0] * (*Zp - Ex[i].z0); + Zy = Ex[i].dm[0][1] * (*Xp - Ex[i].x0) + + Ex[i].dm[1][1] * (*Yp - Ex[i].y0) + + Ex[i].dm[2][1] * (*Zp - Ex[i].z0); + N = Ex[i].dm[0][2] * (*Xp - Ex[i].x0) + + Ex[i].dm[1][2] * (*Yp - Ex[i].y0) + + Ex[i].dm[2][2] * (*Zp - Ex[i].z0); + + for (j=0; j<3; j++) + X[2*i][j] = ((-I[i].cc)/(N*N)) + * (N*Ex[i].dm[j][0] - Zx*Ex[i].dm[j][2]); + for (j=0; j<3; j++) + X[2*i+1][j] = ((-I[i].cc)/(N*N)) + * (N*Ex[i].dm[j][1] - Zy*Ex[i].dm[j][2]); + + /* multimedia factor (radial shift) */ + mmf = multimed_r_nlay (Ex[i], mm, *Xp, *Yp, *Zp); + + X[2*i][0] *= mmf; X[2*i][1] *= mmf; + X[2*i+1][0] *= mmf; X[2*i+1][1] *= mmf; + + /* observation vector */ + img_coord_old (*Xp,*Yp,*Zp, Ex[i], I[i], ap[i], mm, &xp1, &yp1); + + y[2*i] = xp[i] - xp1; y[2*i+1] = yp[i] - yp1; + } + + else /* set values to 0 */ + { + for (j=0; j<3; j++) + { + X[2*i][j] = 0; X[2*i+1][j] = 0; + } + y[2*i] = 0; y[2*i+1] = 0; + } } - y[2*i] = 0; y[2*i+1] = 0; - } + + /********************************************************************/ + + /* Gaussian algorithm */ + + ata (X, XtX, 8, 3); + matinv (XtX, 3); + atl (Xty, X, y, 8, 3); + matmul (beta, XtX, Xty, 3,3,1); + + stopflag = 1; + for (i=0; i<3; i++) + if (fabs(beta[i]) > 0.005) stopflag = 0; + + *Xp += beta[0]; *Yp += beta[1]; *Zp += beta[2]; itnum++; } - - /********************************************************************/ - - /* Gaussian algorithm */ - - ata (X, XtX, 8, 3); - matinv (XtX, 3); - atl (Xty, X, y, 8, 3); - matmul (beta, XtX, Xty, 3,3,1); - - stopflag = 1; - for (i=0; i<3; i++) - if (fabs(beta[i]) > 0.005) stopflag = 0; - - *Xp += beta[0]; *Yp += beta[1]; *Zp += beta[2]; itnum++; - } - /************************************************************************/ + /************************************************************************/ - /* residuals */ + /* residuals */ - /* number of observations for sigma0 */ - for (i=0, n_obs=0; i -1e10 && yp[i] > -1e10) n_obs += 2; - - matmul (Xbeta, X, beta, 8, 3, 1); - for (i=0; i -1e10 && yp[i] > -1e10) n_obs += 2; + + matmul (Xbeta, X, beta, 8, 3, 1); + for (i=0; i-999) { + flag[0]=1; + x = x1 - I[0].xh; + y = y1 - I[0].yh; + //correct_brown_affin (x, y, ap[0], &x, &y); + ray_tracing_v2 (x,y, Ex[0], I[0], G[0], mmp, &X[0], &Y[0], &Z[0], &a[0], &b[0], &c[0]); + } + if (x2>-999) { + flag[1]=1; + x = x2 - I[1].xh; + y = y2 - I[1].yh; + //correct_brown_affin (x, y, ap[1], &x, &y); + ray_tracing_v2 (x,y, Ex[1], I[1], G[1], mmp, &X[1], &Y[1], &Z[1], &a[1], &b[1], &c[1]); + } + if (x3>-999) { + flag[2]=1; + x = x3 - I[2].xh; + y = y3 - I[2].yh; + //correct_brown_affin (x, y, ap[2], &x, &y); + ray_tracing_v2 (x,y, Ex[2], I[2], G[2], mmp, &X[2], &Y[2], &Z[2], &a[2], &b[2], &c[2]); + } + if (x4>-999) { + flag[3]=1; + x = x4 - I[3].xh; + y = y4 - I[3].yh; + //correct_brown_affin (x, y, ap[3], &x, &y); + ray_tracing_v2 (x,y, Ex[3], I[3], G[3], mmp, &X[3], &Y[3], &Z[3], &a[3], &b[3], &c[3]); + } - int i,count_inner=0,n,m,flag[4]; - double d_inner=0.,x,y; - double X[4],Y[4],Z[4],a[4],b[4],c[4],dist,dist_error,X_pos[6],Y_pos[6],Z_pos[6],XX,YY,ZZ,si0,sqX,sqY,sqZ; - - //new det_lsq function, bloody fast! - flag[0]=0;flag[1]=0;flag[2]=0;flag[3]=0; - if(x1>-999){ - flag[0]=1; - x = x1 - I[0].xh; - y = y1 - I[0].yh; - //correct_brown_affin (x, y, ap[0], &x, &y); - ray_tracing_v2 (x,y, Ex[0], I[0], G[0], mmp, &X[0], &Y[0], &Z[0], &a[0], &b[0], &c[0]); - } - if(x2>-999){ - flag[1]=1; - x = x2 - I[1].xh; - y = y2 - I[1].yh; - //correct_brown_affin (x, y, ap[1], &x, &y); - ray_tracing_v2 (x,y, Ex[1], I[1], G[1], mmp, &X[1], &Y[1], &Z[1], &a[1], &b[1], &c[1]); - } - if(x3>-999){ - flag[2]=1; - x = x3 - I[2].xh; - y = y3 - I[2].yh; - //correct_brown_affin (x, y, ap[2], &x, &y); - ray_tracing_v2 (x,y, Ex[2], I[2], G[2], mmp, &X[2], &Y[2], &Z[2], &a[2], &b[2], &c[2]); - } - if(x4>-999){ - flag[3]=1; - x = x4 - I[3].xh; - y = y4 - I[3].yh; - //correct_brown_affin (x, y, ap[3], &x, &y); - ray_tracing_v2 (x,y, Ex[3], I[3], G[3], mmp, &X[3], &Y[3], &Z[3], &a[3], &b[3], &c[3]); - } - - count_inner=0; - for (n=0;n -1e10 && yp[i] > -1e10) - { - ray_tracing (xp[i], yp[i], Ex[i], I[i], - mm, &X1, &Y1, &Z1, &a1, &b1, &c1); break; - } - for (i=i+1; i -1e10 && yp[i] > -1e10) - { - ray_tracing (xp[i], yp[i], Ex[i], I[i], - mm, &X2, &Y2, &Z2, &a2, &b2, &c2); break; - } - - intersect_rt (X1,Y1,Z1,a1,b1,c1, X2,Y2,Z2,a2,b2,c2, Xp, Yp, Zp); - */ - - /* hack due to problems with approx in det_lsq: */ - - /************************************************************************/ - /* least squares adjustment */ - itnum = 0; stopflag = 0; - while ((stopflag == 0) && (itnum < 10)) - { - /********************************************************************/ - /* X-matrix + observation vector from image coordinates */ - - for (i=0; i<4; i++) + int i, j, itnum, stopflag, n_obs; + double X[8][3], XtX[3][3], y[8], Xty[3], beta[3], + Xbeta[8], edach[8], omega=0., sigma0, sigmabeta[3]; + double Zx, Zy, N, mmf; + double xp1, yp1; + double xp[4], yp[4]; + Exterior Ex_t[4]; + double Xp_t,Yp_t,Zp_t; + double X_t[8][3],cross_p[3],cross_c[3]; + + double multimed_r_nlay (); + double multimed_r_nlay_v2 (); + + /* static parameters for examination of Qxx, Qvv */ + static int Sn; + static double Ssigma0, SQxx[3][3], SQvv[8][8]; + double Xt[3][8], buf[3][8], Qvv[8][8]; + + xp[0] = x1; xp[1] = x2; xp[2] = x3; xp[3] = x4; + yp[0] = y1; yp[1] = y2; yp[2] = y3; yp[3] = y4; + + // /* approximate values: rt_is with the uncorrected image coordinates */ + // /* of the first 2 rays */ + // + // /* + // for (i=0; i -1e10 && yp[i] > -1e10) + // { + // ray_tracing (xp[i], yp[i], Ex[i], I[i], + // mm, &X1, &Y1, &Z1, &a1, &b1, &c1); break; + // } + // for (i=i+1; i -1e10 && yp[i] > -1e10) + // { + // ray_tracing (xp[i], yp[i], Ex[i], I[i], + // mm, &X2, &Y2, &Z2, &a2, &b2, &c2); break; + // } + // + // intersect_rt (X1,Y1,Z1,a1,b1,c1, X2,Y2,Z2,a2,b2,c2, Xp, Yp, Zp); + // */ + // + // /* hack due to problems with approx in det_lsq: */ + + /************************************************************************/ + /* least squares adjustment */ + itnum = 0; stopflag = 0; + while ((stopflag == 0) && (itnum < 10)) { - if (xp[i] > -1e10 && yp[i] > -1e10) - { - /* derivatives d(x(i),y(i)) / d(X,Y,Z) (Kraus) */ - - Zx = Ex[i].dm[0][0] * (*Xp - Ex[i].x0) - + Ex[i].dm[1][0] * (*Yp - Ex[i].y0) - + Ex[i].dm[2][0] * (*Zp - Ex[i].z0); - Zy = Ex[i].dm[0][1] * (*Xp - Ex[i].x0) - + Ex[i].dm[1][1] * (*Yp - Ex[i].y0) - + Ex[i].dm[2][1] * (*Zp - Ex[i].z0); - N = Ex[i].dm[0][2] * (*Xp - Ex[i].x0) - + Ex[i].dm[1][2] * (*Yp - Ex[i].y0) - + Ex[i].dm[2][2] * (*Zp - Ex[i].z0); - - for (j=0; j<3; j++) - X[2*i][j] = ((-I[i].cc)/(N*N)) - * (N*Ex[i].dm[j][0] - Zx*Ex[i].dm[j][2]); - for (j=0; j<3; j++) - X[2*i+1][j] = ((-I[i].cc)/(N*N)) - * (N*Ex[i].dm[j][1] - Zy*Ex[i].dm[j][2]); - - /* multimedia factor (radial shift) */ - //trans - - trans_Cam_Point(Ex[i],mm,G[i],*Xp,*Yp,*Zp,&Ex_t[i],&Xp_t,&Yp_t,&Zp_t,&cross_p,&cross_c); - mmf = multimed_r_nlay_v2 (Ex_t[i], Ex[i], mm, Xp_t, Yp_t, Zp_t); - - X[2*i][0] *= mmf; X[2*i][1] *= mmf;/// - X[2*i+1][0] *= mmf; X[2*i+1][1] *= mmf;/// - - /* observation vector */ - img_coord (*Xp,*Yp,*Zp, Ex[i], I[i], G[i], ap[i], mm, &xp1, &yp1); - - y[2*i] = xp[i] - xp1; y[2*i+1] = yp[i] - yp1; - } + /********************************************************************/ + /* X-matrix + observation vector from image coordinates */ - else /* set values to 0 */ - { - for (j=0; j<3; j++) + for (i=0; i<4; i++) { - X[2*i][j] = 0; X[2*i+1][j] = 0; + if (xp[i] > -1e10 && yp[i] > -1e10) + { + /* derivatives d(x(i),y(i)) / d(X,Y,Z) (Kraus) */ + + Zx = Ex[i].dm[0][0] * (*Xp - Ex[i].x0) + + Ex[i].dm[1][0] * (*Yp - Ex[i].y0) + + Ex[i].dm[2][0] * (*Zp - Ex[i].z0); + Zy = Ex[i].dm[0][1] * (*Xp - Ex[i].x0) + + Ex[i].dm[1][1] * (*Yp - Ex[i].y0) + + Ex[i].dm[2][1] * (*Zp - Ex[i].z0); + N = Ex[i].dm[0][2] * (*Xp - Ex[i].x0) + + Ex[i].dm[1][2] * (*Yp - Ex[i].y0) + + Ex[i].dm[2][2] * (*Zp - Ex[i].z0); + + for (j=0; j<3; j++) + X[2*i][j] = ((-I[i].cc)/(N*N)) + * (N*Ex[i].dm[j][0] - Zx*Ex[i].dm[j][2]); + for (j=0; j<3; j++) + X[2*i+1][j] = ((-I[i].cc)/(N*N)) + * (N*Ex[i].dm[j][1] - Zy*Ex[i].dm[j][2]); + + /* multimedia factor (radial shift) */ + //trans + // removed badly placed &'s, ad holten 12-2012 + trans_Cam_Point(Ex[i],mm,G[i],*Xp,*Yp,*Zp,&Ex_t[i],Xp_t,Yp_t,Zp_t,cross_p,cross_c); + mmf = multimed_r_nlay_v2 (Ex_t[i], Ex[i], mm, Xp_t, Yp_t, Zp_t); + + X[2*i][0] *= mmf; X[2*i][1] *= mmf;/// + X[2*i+1][0] *= mmf; X[2*i+1][1] *= mmf;/// + + /* observation vector */ + img_coord (*Xp,*Yp,*Zp, Ex[i], I[i], G[i], ap[i], mm, &xp1, &yp1); + + y[2*i] = xp[i] - xp1; y[2*i+1] = yp[i] - yp1; + } + else /* set values to 0 */ + { + for (j=0; j<3; j++) + { + X[2*i][j] = 0; X[2*i+1][j] = 0; + } + y[2*i] = 0; y[2*i+1] = 0; + } } - y[2*i] = 0; y[2*i+1] = 0; - } + + /********************************************************************/ + + /* Gaussian algorithm */ + + ata (X, XtX, 8, 3); + matinv (XtX, 3); + atl (Xty, X, y, 8, 3); + matmul (beta, XtX, Xty, 3,3,1); + + stopflag = 1; + for (i=0; i<3; i++) + if (fabs(beta[i]) > 0.002) stopflag = 0; + + *Xp += beta[0]; *Yp += beta[1]; *Zp += beta[2]; itnum++; } - - /********************************************************************/ - - /* Gaussian algorithm */ - - ata (X, XtX, 8, 3); - matinv (XtX, 3); - atl (Xty, X, y, 8, 3); - matmul (beta, XtX, Xty, 3,3,1); - - stopflag = 1; - for (i=0; i<3; i++) - if (fabs(beta[i]) > 0.002) stopflag = 0; - - *Xp += beta[0]; *Yp += beta[1]; *Zp += beta[2]; itnum++; - } - - /************************************************************************/ + + /************************************************************************/ - /* residuals */ + /* residuals */ - /* number of observations for sigma0 */ - for (i=0, n_obs=0; i -1e10 && yp[i] > -1e10) n_obs += 2; - - matmul (Xbeta, X, beta, 8, 3, 1); - for (i=0; i -1e10 && yp[i] > -1e10) n_obs += 2; + + matmul (Xbeta, X, beta, 8, 3, 1); + for (i=0; i 0.005) stopflag = 0; - - *Xp += beta[0]; *Yp += beta[1]; *Zp += beta[2]; itnum++; - } - - - /************************************************************************/ - - - /* residuals */ + /********************************************************************/ + /* derivatives d(x''',y''') / d(X,Y,Z) (Kraus) */ + + Zx = Ex[2].dm[0][0] * (*Xp - Ex[2].x0) + + Ex[2].dm[1][0] * (*Yp - Ex[2].y0) + + Ex[2].dm[2][0] * (*Zp - Ex[2].z0); + Zy = Ex[2].dm[0][1] * (*Xp - Ex[2].x0) + + Ex[2].dm[1][1] * (*Yp - Ex[2].y0) + + Ex[2].dm[2][1] * (*Zp - Ex[2].z0); + N = Ex[2].dm[0][2] * (*Xp - Ex[2].x0) + + Ex[2].dm[1][2] * (*Yp - Ex[2].y0) + + Ex[2].dm[2][2] * (*Zp - Ex[2].z0); + + X[4][0] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[0][0] - Zx*Ex[2].dm[0][2]); + X[4][1] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[1][0] - Zx*Ex[2].dm[1][2]); + X[4][2] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[2][0] - Zx*Ex[2].dm[2][2]); + X[5][0] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[0][1] - Zy*Ex[2].dm[0][2]); + X[5][1] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[1][1] - Zy*Ex[2].dm[1][2]); + X[5][2] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[2][1] - Zy*Ex[2].dm[2][2]); + //trans + // added & to Xp_t, Yp_t, Zp_t, stripped & from cross_p, cross_c, ad holten 12-2012 + trans_Cam_Point(Ex[2],mm,G[2],*Xp,*Yp,*Zp,&Ex_t[2], &Xp_t, &Yp_t, &Zp_t,cross_p,cross_c); + // stripped * from Xp_t, Yp_t, Zp_t. ad holten 12-2012 + mmf = multimed_r_nlay_v2 (Ex_t[2], Ex[2], mm, Xp_t, Yp_t, Zp_t); + + X[4][0] *= mmf; X[4][1] *= mmf; + X[5][0] *= mmf; X[5][1] *= mmf; + + img_coord (*Xp,*Yp,*Zp, Ex[0],I[0], G[0], ap[0], mm, &xp1, &yp1); + img_coord (*Xp,*Yp,*Zp, Ex[1],I[1], G[1], ap[1], mm, &xp2, &yp2); + img_coord (*Xp,*Yp,*Zp, Ex[2],I[2], G[2], ap[2], mm, &xp3, &yp3); + + y[0] = x1 - xp1; y[2] = x2 - xp2; y[4] = x3 - xp3; + y[1] = y1 - yp1; y[3] = y2 - yp2; y[5] = y3 - yp3; + + + /********************************************************************/ + /* Gaussian algorithm */ + + ata (X, XtX, 6, 3); + matinv (XtX, 3); + atl (Xty, X, y, 6, 3); + matmul (beta, XtX, Xty, 3,3,1); + + stopflag = 1; + for (i=0; i<3; i++) + if (fabs(beta[i]) > 0.005) stopflag = 0; + + *Xp += beta[0]; *Yp += beta[1]; *Zp += beta[2]; itnum++; + } - matmul (Xbeta, X, beta, 6, 3, 1); - for (i=0; i<6; i++) - { - edach[i] = Xbeta[i] - y[i]; - omega += edach[i] * edach[i]; - } - sigma0 = sqrt(omega /(6-3)); - for (i=0; i<3; i++) sigmabeta[i] = sigma0 * sqrt(XtX[i][i]); + /************************************************************************/ - mean_sigma0 += sigma0 * sigma0; - rmsX += sigmabeta[0] * sigmabeta[0]; - rmsY += sigmabeta[1] * sigmabeta[1]; - rmsZ += sigmabeta[2] * sigmabeta[2]; -} + /* residuals */ + + matmul (Xbeta, X, beta, 6, 3, 1); + for (i=0; i<6; i++) + { + edach[i] = Xbeta[i] - y[i]; + omega += edach[i] * edach[i]; + } + sigma0 = sqrt(omega /(6-3)); + for (i=0; i<3; i++) sigmabeta[i] = sigma0 * sqrt(XtX[i][i]); + mean_sigma0 += sigma0 * sigma0; + rmsX += sigmabeta[0] * sigmabeta[0]; + rmsY += sigmabeta[1] * sigmabeta[1]; + rmsZ += sigmabeta[2] * sigmabeta[2]; +} -void det_lsq_4 (Ex, I, G, ap, mm, x1, y1, x2, y2, x3, y3, x4, y4, Xp, Yp, Zp) -Exterior Ex[4]; -Interior I[4]; -Glass G[4]; -ap_52 ap[4]; -mm_np mm; -double x1, y1, x2, y2, x3, y3, x4, y4, *Xp,*Yp,*Zp; +void det_lsq_4 (Exterior Ex[4], Interior I[4], Glass G[4], ap_52 ap[4], mm_np mm, + double x1, double y1, double x2, double y2, + double x3, double y3, double x4, double y4, + double *Xp, double *Yp, double *Zp) { - int i, itnum, stopflag; - double X[8][3], XtX[3][3], y[8], Xty[3], beta[3], - Xbeta[8], edach[8], omega=0., sigma0, sigmabeta[3]; - double Zx, Zy, N, mmf; - double xp1, yp1, xp2, yp2, xp3, yp3, xp4, yp4; - double X1,Y1,Z1, a1,b1,c1,X2,Y2,Z2, a2,b2,c2; - - double multimed_r_nlay_v2 (); - Exterior Ex_t[4]; - double X_t[8][3]; - double *Xp_t,*Yp_t,*Zp_t,cross_p[3],cross_c[3]; - - - /* approximate values (rt_is with the uncorrected image coordinates) */ - ray_tracing_v2 (x1, y1, Ex[0], I[0],G[0], - mm, &X1, &Y1, &Z1, &a1, &b1, &c1); - ray_tracing_v2 (x2, y2, Ex[1], I[1],G[1], - mm, &X2, &Y2, &Z2, &a2, &b2, &c2); - intersect_rt (X1,Y1,Z1,a1,b1,c1, X2,Y2,Z2,a2,b2,c2, Xp, Yp, Zp); - - /************************************************************************/ - - itnum = 0; stopflag = 0; - while ((stopflag == 0) && (itnum < 10)) - { - - /********************************************************************/ - /* derivatives d(x',y') / d(X,Y,Z) (Kraus) */ - - Zx = Ex[0].dm[0][0] * (*Xp - Ex[0].x0) - + Ex[0].dm[1][0] * (*Yp - Ex[0].y0) - + Ex[0].dm[2][0] * (*Zp - Ex[0].z0); - Zy = Ex[0].dm[0][1] * (*Xp - Ex[0].x0) - + Ex[0].dm[1][1] * (*Yp - Ex[0].y0) - + Ex[0].dm[2][1] * (*Zp - Ex[0].z0); - N = Ex[0].dm[0][2] * (*Xp - Ex[0].x0) - + Ex[0].dm[1][2] * (*Yp - Ex[0].y0) - + Ex[0].dm[2][2] * (*Zp - Ex[0].z0); - - X[0][0] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[0][0] - Zx*Ex[0].dm[0][2]); - X[0][1] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[1][0] - Zx*Ex[0].dm[1][2]); - X[0][2] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[2][0] - Zx*Ex[0].dm[2][2]); - X[1][0] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[0][1] - Zy*Ex[0].dm[0][2]); - X[1][1] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[1][1] - Zy*Ex[0].dm[1][2]); - X[1][2] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[2][1] - Zy*Ex[0].dm[2][2]); - //trans - trans_Cam_Point(Ex[0],mm,G[0],*Xp,*Yp,*Zp,&Ex_t[0],&Xp_t,&Yp_t,&Zp_t,&cross_p,&cross_c); - mmf = multimed_r_nlay_v2 (Ex_t[0], Ex[0], mm, *Xp_t, *Yp_t, *Zp_t); - - - X[0][0] *= mmf; X[0][1] *= mmf;/// - X[1][0] *= mmf; X[1][1] *= mmf;/// - - - /********************************************************************/ - /* derivatives d(x'',y'') / d(X,Y,Z) (Kraus) */ - - Zx = Ex[1].dm[0][0] * (*Xp - Ex[1].x0) - + Ex[1].dm[1][0] * (*Yp - Ex[1].y0) - + Ex[1].dm[2][0] * (*Zp - Ex[1].z0); - Zy = Ex[1].dm[0][1] * (*Xp - Ex[1].x0) - + Ex[1].dm[1][1] * (*Yp - Ex[1].y0) - + Ex[1].dm[2][1] * (*Zp - Ex[1].z0); - N = Ex[1].dm[0][2] * (*Xp - Ex[1].x0) - + Ex[1].dm[1][2] * (*Yp - Ex[1].y0) - + Ex[1].dm[2][2] * (*Zp - Ex[1].z0); - - X[2][0] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[0][0] - Zx*Ex[1].dm[0][2]); - X[2][1] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[1][0] - Zx*Ex[1].dm[1][2]); - X[2][2] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[2][0] - Zx*Ex[1].dm[2][2]); - X[3][0] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[0][1] - Zy*Ex[1].dm[0][2]); - X[3][1] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[1][1] - Zy*Ex[1].dm[1][2]); - X[3][2] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[2][1] - Zy*Ex[1].dm[2][2]); - //trans - trans_Cam_Point(Ex[1],mm,G[1],*Xp,*Yp,*Zp,&Ex_t[1],&Xp_t,&Yp_t,&Zp_t,&cross_p,&cross_c); - mmf = multimed_r_nlay_v2 (Ex_t[1], Ex[1], mm, *Xp_t, *Yp_t, *Zp_t); - - - X[2][0] *= mmf; X[2][1] *= mmf;/// - X[3][0] *= mmf; X[3][1] *= mmf;/// + int i, itnum, stopflag; + double X[8][3], XtX[3][3], y[8], Xty[3], beta[3], + Xbeta[8], edach[8], omega=0., sigma0, sigmabeta[3]; + double Zx, Zy, N, mmf; + double xp1, yp1, xp2, yp2, xp3, yp3, xp4, yp4; + double X1,Y1,Z1, a1,b1,c1,X2,Y2,Z2, a2,b2,c2; + + Exterior Ex_t[4]; + double Xp_t, Yp_t, Zp_t, cross_p[3],cross_c[3]; // stripped * from *Xp_t,*Yp_t,*Zp_t, ad holten, 12-2012 + + + /* approximate values (rt_is with the uncorrected image coordinates) */ + ray_tracing_v2 (x1, y1, Ex[0], I[0],G[0], + mm, &X1, &Y1, &Z1, &a1, &b1, &c1); + ray_tracing_v2 (x2, y2, Ex[1], I[1],G[1], + mm, &X2, &Y2, &Z2, &a2, &b2, &c2); + intersect_rt (X1,Y1,Z1,a1,b1,c1, X2,Y2,Z2,a2,b2,c2, Xp, Yp, Zp); + + /************************************************************************/ + + itnum = 0; stopflag = 0; + while (stopflag == 0 && itnum < 10) + { + + /********************************************************************/ + /* derivatives d(x',y') / d(X,Y,Z) (Kraus) */ + + Zx = Ex[0].dm[0][0] * (*Xp - Ex[0].x0) + + Ex[0].dm[1][0] * (*Yp - Ex[0].y0) + + Ex[0].dm[2][0] * (*Zp - Ex[0].z0); + Zy = Ex[0].dm[0][1] * (*Xp - Ex[0].x0) + + Ex[0].dm[1][1] * (*Yp - Ex[0].y0) + + Ex[0].dm[2][1] * (*Zp - Ex[0].z0); + N = Ex[0].dm[0][2] * (*Xp - Ex[0].x0) + + Ex[0].dm[1][2] * (*Yp - Ex[0].y0) + + Ex[0].dm[2][2] * (*Zp - Ex[0].z0); + + X[0][0] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[0][0] - Zx*Ex[0].dm[0][2]); + X[0][1] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[1][0] - Zx*Ex[0].dm[1][2]); + X[0][2] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[2][0] - Zx*Ex[0].dm[2][2]); + X[1][0] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[0][1] - Zy*Ex[0].dm[0][2]); + X[1][1] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[1][1] - Zy*Ex[0].dm[1][2]); + X[1][2] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[2][1] - Zy*Ex[0].dm[2][2]); + //trans + // added & to Xp_t, Yp_t, Zp_t, stripped & from cross_p, cross_c, ad holten 12-2012 + trans_Cam_Point(Ex[0],mm,G[0],*Xp,*Yp,*Zp,&Ex_t[0], &Xp_t, &Yp_t, &Zp_t,cross_p,cross_c); + // stripped * from Xp_t, Yp_t, Zp_t. ad holten 12-2012 + mmf = multimed_r_nlay_v2 (Ex_t[0], Ex[0], mm, Xp_t, Yp_t, Zp_t); + + X[0][0] *= mmf; X[0][1] *= mmf;/// + X[1][0] *= mmf; X[1][1] *= mmf;/// + + + /********************************************************************/ + /* derivatives d(x'',y'') / d(X,Y,Z) (Kraus) */ + + Zx = Ex[1].dm[0][0] * (*Xp - Ex[1].x0) + + Ex[1].dm[1][0] * (*Yp - Ex[1].y0) + + Ex[1].dm[2][0] * (*Zp - Ex[1].z0); + Zy = Ex[1].dm[0][1] * (*Xp - Ex[1].x0) + + Ex[1].dm[1][1] * (*Yp - Ex[1].y0) + + Ex[1].dm[2][1] * (*Zp - Ex[1].z0); + N = Ex[1].dm[0][2] * (*Xp - Ex[1].x0) + + Ex[1].dm[1][2] * (*Yp - Ex[1].y0) + + Ex[1].dm[2][2] * (*Zp - Ex[1].z0); + + X[2][0] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[0][0] - Zx*Ex[1].dm[0][2]); + X[2][1] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[1][0] - Zx*Ex[1].dm[1][2]); + X[2][2] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[2][0] - Zx*Ex[1].dm[2][2]); + X[3][0] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[0][1] - Zy*Ex[1].dm[0][2]); + X[3][1] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[1][1] - Zy*Ex[1].dm[1][2]); + X[3][2] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[2][1] - Zy*Ex[1].dm[2][2]); + //trans + // added & to Xp_t, Yp_t, Zp_t, stripped & from cross_p, cross_c, ad holten 12-2012 + trans_Cam_Point(Ex[1],mm,G[1],*Xp,*Yp,*Zp,&Ex_t[1], &Xp_t, &Yp_t, &Zp_t, cross_p,cross_c); + // stripped * from Xp_t, Yp_t, Zp_t. ad holten 12-2012 + mmf = multimed_r_nlay_v2 (Ex_t[1], Ex[1], mm, Xp_t, Yp_t, Zp_t); + + X[2][0] *= mmf; X[2][1] *= mmf;/// + X[3][0] *= mmf; X[3][1] *= mmf;/// - - /********************************************************************/ - /* derivatives d(x''',y''') / d(X,Y,Z) (Kraus) */ - - Zx = Ex[2].dm[0][0] * (*Xp - Ex[2].x0) - + Ex[2].dm[1][0] * (*Yp - Ex[2].y0) - + Ex[2].dm[2][0] * (*Zp - Ex[2].z0); - Zy = Ex[2].dm[0][1] * (*Xp - Ex[2].x0) - + Ex[2].dm[1][1] * (*Yp - Ex[2].y0) - + Ex[2].dm[2][1] * (*Zp - Ex[2].z0); - N = Ex[2].dm[0][2] * (*Xp - Ex[2].x0) - + Ex[2].dm[1][2] * (*Yp - Ex[2].y0) - + Ex[2].dm[2][2] * (*Zp - Ex[2].z0); - - X[4][0] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[0][0] - Zx*Ex[2].dm[0][2]); - X[4][1] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[1][0] - Zx*Ex[2].dm[1][2]); - X[4][2] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[2][0] - Zx*Ex[2].dm[2][2]); - X[5][0] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[0][1] - Zy*Ex[2].dm[0][2]); - X[5][1] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[1][1] - Zy*Ex[2].dm[1][2]); - X[5][2] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[2][1] - Zy*Ex[2].dm[2][2]); - //trans - trans_Cam_Point(Ex[2],mm,G[2],*Xp,*Yp,*Zp,&Ex_t[2],&Xp_t,&Yp_t,&Zp_t,&cross_p,&cross_c); - mmf = multimed_r_nlay_v2 (Ex_t[2], Ex[2], mm, *Xp_t, *Yp_t, *Zp_t); - X[4][0] *= mmf; X[4][1] *= mmf;/// - X[5][0] *= mmf; X[5][1] *= mmf;/// - - - /********************************************************************/ - /* derivatives d(x'''',y'''') / d(X,Y,Z) (Kraus) */ - - Zx = Ex[3].dm[0][0] * (*Xp - Ex[3].x0) - + Ex[3].dm[1][0] * (*Yp - Ex[3].y0) - + Ex[3].dm[2][0] * (*Zp - Ex[3].z0); - Zy = Ex[3].dm[0][1] * (*Xp - Ex[3].x0) - + Ex[3].dm[1][1] * (*Yp - Ex[3].y0) - + Ex[3].dm[2][1] * (*Zp - Ex[3].z0); - N = Ex[3].dm[0][2] * (*Xp - Ex[3].x0) - + Ex[3].dm[1][2] * (*Yp - Ex[3].y0) - + Ex[3].dm[2][2] * (*Zp - Ex[3].z0); - - X[6][0] = ((-I[3].cc)/(N*N)) * (N*Ex[3].dm[0][0] - Zx*Ex[3].dm[0][2]); - X[6][1] = ((-I[3].cc)/(N*N)) * (N*Ex[3].dm[1][0] - Zx*Ex[3].dm[1][2]); - X[6][2] = ((-I[3].cc)/(N*N)) * (N*Ex[3].dm[2][0] - Zx*Ex[3].dm[2][2]); - X[7][0] = ((-I[3].cc)/(N*N)) * (N*Ex[3].dm[0][1] - Zy*Ex[3].dm[0][2]); - X[7][1] = ((-I[3].cc)/(N*N)) * (N*Ex[3].dm[1][1] - Zy*Ex[3].dm[1][2]); - X[7][2] = ((-I[3].cc)/(N*N)) * (N*Ex[3].dm[2][1] - Zy*Ex[3].dm[2][2]); - //trans - trans_Cam_Point(Ex[3],mm,G[3],*Xp,*Yp,*Zp,&Ex_t[3],&Xp_t,&Yp_t,&Zp_t,&cross_p,&cross_c); - mmf = multimed_r_nlay_v2 (Ex_t[3], Ex[3], mm, *Xp_t, *Yp_t, *Zp_t); + /********************************************************************/ + /* derivatives d(x''',y''') / d(X,Y,Z) (Kraus) */ + + Zx = Ex[2].dm[0][0] * (*Xp - Ex[2].x0) + + Ex[2].dm[1][0] * (*Yp - Ex[2].y0) + + Ex[2].dm[2][0] * (*Zp - Ex[2].z0); + Zy = Ex[2].dm[0][1] * (*Xp - Ex[2].x0) + + Ex[2].dm[1][1] * (*Yp - Ex[2].y0) + + Ex[2].dm[2][1] * (*Zp - Ex[2].z0); + N = Ex[2].dm[0][2] * (*Xp - Ex[2].x0) + + Ex[2].dm[1][2] * (*Yp - Ex[2].y0) + + Ex[2].dm[2][2] * (*Zp - Ex[2].z0); + + X[4][0] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[0][0] - Zx*Ex[2].dm[0][2]); + X[4][1] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[1][0] - Zx*Ex[2].dm[1][2]); + X[4][2] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[2][0] - Zx*Ex[2].dm[2][2]); + X[5][0] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[0][1] - Zy*Ex[2].dm[0][2]); + X[5][1] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[1][1] - Zy*Ex[2].dm[1][2]); + X[5][2] = ((-I[2].cc)/(N*N)) * (N*Ex[2].dm[2][1] - Zy*Ex[2].dm[2][2]); + //trans + // added & to Xp_t, Yp_t, Zp_t, stripped & from cross_p, cross_c, ad holten 12-2012 + trans_Cam_Point(Ex[2],mm,G[2],*Xp,*Yp,*Zp,&Ex_t[2], &Xp_t, &Yp_t, &Zp_t,cross_p,cross_c); + // stripped * from Xp_t, Yp_t, Zp_t. ad holten 12-2012 + mmf = multimed_r_nlay_v2 (Ex_t[2], Ex[2], mm, Xp_t, Yp_t, Zp_t); + + X[4][0] *= mmf; X[4][1] *= mmf; + X[5][0] *= mmf; X[5][1] *= mmf; - - X[6][0] *= mmf; X[6][1] *= mmf;/// - X[7][0] *= mmf; X[7][1] *= mmf;/// + /********************************************************************/ + /* derivatives d(x'''',y'''') / d(X,Y,Z) (Kraus) */ + + Zx = Ex[3].dm[0][0] * (*Xp - Ex[3].x0) + + Ex[3].dm[1][0] * (*Yp - Ex[3].y0) + + Ex[3].dm[2][0] * (*Zp - Ex[3].z0); + Zy = Ex[3].dm[0][1] * (*Xp - Ex[3].x0) + + Ex[3].dm[1][1] * (*Yp - Ex[3].y0) + + Ex[3].dm[2][1] * (*Zp - Ex[3].z0); + N = Ex[3].dm[0][2] * (*Xp - Ex[3].x0) + + Ex[3].dm[1][2] * (*Yp - Ex[3].y0) + + Ex[3].dm[2][2] * (*Zp - Ex[3].z0); + + X[6][0] = ((-I[3].cc)/(N*N)) * (N*Ex[3].dm[0][0] - Zx*Ex[3].dm[0][2]); + X[6][1] = ((-I[3].cc)/(N*N)) * (N*Ex[3].dm[1][0] - Zx*Ex[3].dm[1][2]); + X[6][2] = ((-I[3].cc)/(N*N)) * (N*Ex[3].dm[2][0] - Zx*Ex[3].dm[2][2]); + X[7][0] = ((-I[3].cc)/(N*N)) * (N*Ex[3].dm[0][1] - Zy*Ex[3].dm[0][2]); + X[7][1] = ((-I[3].cc)/(N*N)) * (N*Ex[3].dm[1][1] - Zy*Ex[3].dm[1][2]); + X[7][2] = ((-I[3].cc)/(N*N)) * (N*Ex[3].dm[2][1] - Zy*Ex[3].dm[2][2]); + //trans + // added & to Xp_t, Yp_t, Zp_t, stripped & from cross_p, cross_c, ad holten 12-2012 + trans_Cam_Point(Ex[3],mm,G[3],*Xp,*Yp,*Zp,&Ex_t[3], &Xp_t, &Yp_t, &Zp_t,cross_p,cross_c); + // stripped * from Xp_t, Yp_t, Zp_t. ad holten 12-2012 + mmf = multimed_r_nlay_v2 (Ex_t[3], Ex[3], mm, Xp_t, Yp_t, Zp_t); + + X[6][0] *= mmf; X[6][1] *= mmf;/// + X[7][0] *= mmf; X[7][1] *= mmf;/// - - /********************************************************************/ - - img_coord (*Xp,*Yp,*Zp, Ex[0],I[0], G[0], ap[0], mm, &xp1, &yp1); - img_coord (*Xp,*Yp,*Zp, Ex[1],I[1], G[1], ap[1], mm, &xp2, &yp2); - img_coord (*Xp,*Yp,*Zp, Ex[2],I[2], G[2], ap[2], mm, &xp3, &yp3); - img_coord (*Xp,*Yp,*Zp, Ex[3],I[3], G[3], ap[3], mm, &xp4, &yp4); - - y[0] = x1 - xp1; y[1] = y1 - yp1; - y[2] = x2 - xp2; y[3] = y2 - yp2; - y[4] = x3 - xp3; y[5] = y3 - yp3; - y[6] = x4 - xp4; y[7] = y4 - yp4; - - - /********************************************************************/ - /* Gaussian algorithm */ - - ata (X, XtX, 8, 3); - matinv (XtX, 3); - atl (Xty, X, y, 8, 3); - matmul (beta, XtX, Xty, 3,3,1); - - stopflag = 1; - for (i=0; i<3; i++) - if (fabs(beta[i]) > 0.005) stopflag = 0; - - *Xp += beta[0]; *Yp += beta[1]; *Zp += beta[2]; itnum++; - } - - - /************************************************************************/ - - - /* residuals */ - - matmul (Xbeta, X, beta, 8, 3, 1); - for (i=0; i<8; i++) - { - edach[i] = Xbeta[i] - y[i]; - omega += edach[i] * edach[i]; - } - sigma0 = sqrt(omega /(8-3)); - for (i=0; i<3; i++) sigmabeta[i] = sigma0 * sqrt(XtX[i][i]); - + /********************************************************************/ + + img_coord (*Xp,*Yp,*Zp, Ex[0],I[0], G[0], ap[0], mm, &xp1, &yp1); + img_coord (*Xp,*Yp,*Zp, Ex[1],I[1], G[1], ap[1], mm, &xp2, &yp2); + img_coord (*Xp,*Yp,*Zp, Ex[2],I[2], G[2], ap[2], mm, &xp3, &yp3); + img_coord (*Xp,*Yp,*Zp, Ex[3],I[3], G[3], ap[3], mm, &xp4, &yp4); + + y[0] = x1 - xp1; y[1] = y1 - yp1; + y[2] = x2 - xp2; y[3] = y2 - yp2; + y[4] = x3 - xp3; y[5] = y3 - yp3; + y[6] = x4 - xp4; y[7] = y4 - yp4; + + + /********************************************************************/ + /* Gaussian algorithm */ + + ata (X, XtX, 8, 3); + matinv (XtX, 3); + atl (Xty, X, y, 8, 3); + matmul (beta, XtX, Xty, 3,3,1); + + stopflag = 1; + for (i=0; i<3; i++) + if (fabs(beta[i]) > 0.005) stopflag = 0; + + *Xp += beta[0]; *Yp += beta[1]; *Zp += beta[2]; itnum++; + } + + /************************************************************************/ - mean_sigma0 += sigma0 * sigma0; - rmsX += sigmabeta[0] * sigmabeta[0]; - rmsY += sigmabeta[1] * sigmabeta[1]; - rmsZ += sigmabeta[2] * sigmabeta[2]; + /* residuals */ + + matmul (Xbeta, X, beta, 8, 3, 1); + for (i=0; i<8; i++) + { + edach[i] = Xbeta[i] - y[i]; + omega += edach[i] * edach[i]; + } + sigma0 = sqrt(omega /(8-3)); + for (i=0; i<3; i++) sigmabeta[i] = sigma0 * sqrt(XtX[i][i]); + + mean_sigma0 += sigma0 * sigma0; + rmsX += sigmabeta[0] * sigmabeta[0]; + rmsY += sigmabeta[1] * sigmabeta[1]; + rmsZ += sigmabeta[2] * sigmabeta[2]; } -void det_lsq_2 (Ex, I, G, ap, mm, x1, y1, x2, y2, Xp, Yp, Zp) -Exterior Ex[2]; -Interior I[2]; -Glass G[2]; -ap_52 ap[2]; -mm_np mm; -double x1, y1, x2, y2, *Xp,*Yp,*Zp; +void det_lsq_2 (Exterior Ex[2], Interior I[2], Glass G[2], ap_52 ap[2], mm_np mm, + double x1, double y1, double x2, double y2, + double *Xp, double *Yp, double *Zp) { - int i, itnum, stopflag; - double X[4][3], XtX[3][3], y[4], Xty[3], beta[3], - Xbeta[4], edach[4], omega=0., sigma0, sigmabeta[3]; - double Zx, Zy, N, mmf; - double xp1, yp1, xp2, yp2; - double X1,Y1,Z1, a1,b1,c1, X2,Y2,Z2, a2,b2,c2; - - double multimed_r_nlay_v2 (); - Exterior Ex_t[2]; - double X_t[4][3]; - double *Xp_t,*Yp_t,*Zp_t,cross_p[3],cross_c[3]; - - - /* approximate values (rt_is with the uncorrected image coordinates) */ - ray_tracing_v2 (x1, y1, Ex[0], I[0],G[0], - mm, &X1, &Y1, &Z1, &a1, &b1, &c1); - ray_tracing_v2 (x2, y2, Ex[1],I[1],G[1], - mm, &X2, &Y2, &Z2, &a2, &b2, &c2); - intersect_rt (X1,Y1,Z1,a1,b1,c1, X2,Y2,Z2,a2,b2,c2, Xp, Yp, Zp); - /************************************************************************/ - - itnum = 0; stopflag = 0; - while ((stopflag == 0) && (itnum < 10)) - { - - /********************************************************************/ - /* derivatives d(x',y') / d(X,Y,Z) (Kraus) */ - - Zx = Ex[0].dm[0][0] * (*Xp - Ex[0].x0) - + Ex[0].dm[1][0] * (*Yp - Ex[0].y0) - + Ex[0].dm[2][0] * (*Zp - Ex[0].z0); - Zy = Ex[0].dm[0][1] * (*Xp - Ex[0].x0) - + Ex[0].dm[1][1] * (*Yp - Ex[0].y0) - + Ex[0].dm[2][1] * (*Zp - Ex[0].z0); - N = Ex[0].dm[0][2] * (*Xp - Ex[0].x0) - + Ex[0].dm[1][2] * (*Yp - Ex[0].y0) - + Ex[0].dm[2][2] * (*Zp - Ex[0].z0); - - X[0][0] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[0][0] - Zx*Ex[0].dm[0][2]); - X[0][1] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[1][0] - Zx*Ex[0].dm[1][2]); - X[0][2] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[2][0] - Zx*Ex[0].dm[2][2]); - X[1][0] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[0][1] - Zy*Ex[0].dm[0][2]); - X[1][1] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[1][1] - Zy*Ex[0].dm[1][2]); - X[1][2] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[2][1] - Zy*Ex[0].dm[2][2]); - - //trans - trans_Cam_Point(Ex[0],mm,G[0],*Xp,*Yp,*Zp,&Ex_t[0],&Xp_t,&Yp_t,&Zp_t,&cross_p,&cross_c); - mmf = multimed_r_nlay_v2 (Ex_t[0], Ex[0], mm, *Xp_t, *Yp_t, *Zp_t); - // do stuff - X[0][0] *= mmf; X[0][1] *= mmf; X[1][0] *= mmf; X[1][1] *= mmf; - - - /********************************************************************/ - /* derivatives d(x'',y'') / d(X,Y,Z) (Kraus) */ - - Zx = Ex[1].dm[0][0] * (*Xp - Ex[1].x0) - + Ex[1].dm[1][0] * (*Yp - Ex[1].y0) - + Ex[1].dm[2][0] * (*Zp - Ex[1].z0); - Zy = Ex[1].dm[0][1] * (*Xp - Ex[1].x0) - + Ex[1].dm[1][1] * (*Yp - Ex[1].y0) - + Ex[1].dm[2][1] * (*Zp - Ex[1].z0); - N = Ex[1].dm[0][2] * (*Xp - Ex[1].x0) - + Ex[1].dm[1][2] * (*Yp - Ex[1].y0) - + Ex[1].dm[2][2] * (*Zp - Ex[1].z0); - - X[2][0] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[0][0] - Zx*Ex[1].dm[0][2]); - X[2][1] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[1][0] - Zx*Ex[1].dm[1][2]); - X[2][2] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[2][0] - Zx*Ex[1].dm[2][2]); - X[3][0] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[0][1] - Zy*Ex[1].dm[0][2]); - X[3][1] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[1][1] - Zy*Ex[1].dm[1][2]); - X[3][2] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[2][1] - Zy*Ex[1].dm[2][2]); - - //trans - trans_Cam_Point(Ex[1],mm,G[1],*Xp,*Yp,*Zp,&Ex_t[1],&Xp_t,&Yp_t,&Zp_t,&cross_p,&cross_c); - mmf = multimed_r_nlay_v2 (Ex_t[1], Ex[1], mm, *Xp_t, *Yp_t, *Zp_t); + int i, itnum, stopflag; + double X[4][3], XtX[3][3], y[4], Xty[3], beta[3], + Xbeta[4], edach[4], omega=0., sigma0, sigmabeta[3]; + double Zx, Zy, N, mmf; + double xp1, yp1, xp2, yp2; + double X1,Y1,Z1, a1,b1,c1, X2,Y2,Z2, a2,b2,c2; + + Exterior Ex_t[2]; + double X_t[4][3]; + double Xp_t, Yp_t, Zp_t, cross_p[3],cross_c[3]; // stripped * from *Xp_t, *Yp_t, *Zp_t, ad holten 12-2012 + + /* approximate values (rt_is with the uncorrected image coordinates) */ + ray_tracing_v2 (x1, y1, Ex[0], I[0],G[0], + mm, &X1, &Y1, &Z1, &a1, &b1, &c1); + ray_tracing_v2 (x2, y2, Ex[1],I[1],G[1], + mm, &X2, &Y2, &Z2, &a2, &b2, &c2); + intersect_rt (X1,Y1,Z1,a1,b1,c1, X2,Y2,Z2,a2,b2,c2, Xp, Yp, Zp); + /************************************************************************/ + + itnum = 0; stopflag = 0; + while ((stopflag == 0) && (itnum < 10)) + { + /********************************************************************/ + /* derivatives d(x',y') / d(X,Y,Z) (Kraus) */ + + Zx = Ex[0].dm[0][0] * (*Xp - Ex[0].x0) + + Ex[0].dm[1][0] * (*Yp - Ex[0].y0) + + Ex[0].dm[2][0] * (*Zp - Ex[0].z0); + Zy = Ex[0].dm[0][1] * (*Xp - Ex[0].x0) + + Ex[0].dm[1][1] * (*Yp - Ex[0].y0) + + Ex[0].dm[2][1] * (*Zp - Ex[0].z0); + N = Ex[0].dm[0][2] * (*Xp - Ex[0].x0) + + Ex[0].dm[1][2] * (*Yp - Ex[0].y0) + + Ex[0].dm[2][2] * (*Zp - Ex[0].z0); + + X[0][0] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[0][0] - Zx*Ex[0].dm[0][2]); + X[0][1] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[1][0] - Zx*Ex[0].dm[1][2]); + X[0][2] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[2][0] - Zx*Ex[0].dm[2][2]); + X[1][0] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[0][1] - Zy*Ex[0].dm[0][2]); + + X[1][1] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[1][1] - Zy*Ex[0].dm[1][2]); + X[1][2] = ((-I[0].cc)/(N*N)) * (N*Ex[0].dm[2][1] - Zy*Ex[0].dm[2][2]); + + //trans + // added & to Xp_t, Yp_t, Zp_t, stripped & from cross_p, cross_c, ad holten 12-2012 + trans_Cam_Point(Ex[0],mm,G[0],*Xp,*Yp,*Zp,&Ex_t[0], &Xp_t, &Yp_t, &Zp_t,cross_p,cross_c); + // stripped *'s from Xp_t, Yp_t, Zp_t, ad holten 12-2012 + mmf = multimed_r_nlay_v2 (Ex_t[0], Ex[0], mm, Xp_t, Yp_t, Zp_t); + // do stuff + X[0][0] *= mmf; X[0][1] *= mmf; X[1][0] *= mmf; X[1][1] *= mmf; - trans_Cam_Point(Ex[i],mm,G[i],X[2][0],X[2][1],X[2][2],&Ex_t[i], - &X_t[2][0],&X_t[2][1],&X_t[2][2],&cross_p,&cross_c); - X_t[2][0] *= mmf; X_t[2][1] *= mmf;/// - back_trans_Point(X_t[2][0],X_t[2][1],X_t[2][2],mm, G[i],cross_p,cross_c, - &X[2][0],&X[2][1],&X[2][2]); - - trans_Cam_Point(Ex[i],mm,G[i],X[3][0],X[3][1],X[3][2],&Ex_t[i], - &X_t[3][0],&X_t[3][1],&X_t[3][2],&cross_p,&cross_c); - X_t[3][0] *= mmf; X_t[3][1] *= mmf;/// - back_trans_Point(X_t[3][0],X_t[3][1],X_t[3][2],mm, G[i],cross_p,cross_c, - &X[3][0],&X[3][1],&X[3][2]); - - - /********************************************************************/ - - img_coord (*Xp,*Yp,*Zp, Ex[0],I[0], G[0], ap[0], mm, &xp1, &yp1); - img_coord (*Xp,*Yp,*Zp, Ex[1],I[1], G[1], ap[1], mm, &xp2, &yp2); - - y[0] = x1 - xp1; y[2] = x2 - xp2; - y[1] = y1 - yp1; y[3] = y2 - yp2; - - - /********************************************************************/ - /* Gaussian algorithm */ - - ata (X, XtX, 4, 3); - matinv (XtX, 3); - atl (Xty, X, y, 4, 3); - matmul (beta, XtX, Xty, 3,3,1); - - stopflag = 1; - for (i=0; i<3; i++) - if (fabs(beta[i]) > 0.005) stopflag = 0; - - *Xp += beta[0]; *Yp += beta[1]; *Zp += beta[2]; itnum++; - } - - - /************************************************************************/ - /* residuals */ - - matmul (Xbeta, X, beta, 4, 3, 1); - for (i=0; i<4; i++) - { - edach[i] = Xbeta[i] - y[i]; - omega += edach[i] * edach[i]; - } - sigma0 = sqrt(omega /(4-3)); - for (i=0; i<3; i++) sigmabeta[i] = sigma0 * sqrt(XtX[i][i]); - + + /********************************************************************/ + /* derivatives d(x'',y'') / d(X,Y,Z) (Kraus) */ + + Zx = Ex[1].dm[0][0] * (*Xp - Ex[1].x0) + + Ex[1].dm[1][0] * (*Yp - Ex[1].y0) + + Ex[1].dm[2][0] * (*Zp - Ex[1].z0); + Zy = Ex[1].dm[0][1] * (*Xp - Ex[1].x0) + + Ex[1].dm[1][1] * (*Yp - Ex[1].y0) + + Ex[1].dm[2][1] * (*Zp - Ex[1].z0); + N = Ex[1].dm[0][2] * (*Xp - Ex[1].x0) + + Ex[1].dm[1][2] * (*Yp - Ex[1].y0) + + Ex[1].dm[2][2] * (*Zp - Ex[1].z0); + + X[2][0] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[0][0] - Zx*Ex[1].dm[0][2]); + X[2][1] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[1][0] - Zx*Ex[1].dm[1][2]); + X[2][2] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[2][0] - Zx*Ex[1].dm[2][2]); + X[3][0] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[0][1] - Zy*Ex[1].dm[0][2]); + X[3][1] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[1][1] - Zy*Ex[1].dm[1][2]); + X[3][2] = ((-I[1].cc)/(N*N)) * (N*Ex[1].dm[2][1] - Zy*Ex[1].dm[2][2]); + + //trans + // added & to Xp_t, Yp_t, Zp_t, stripped & from cross_p, cross_c, ad holten 12-2012 + trans_Cam_Point(Ex[1], mm, G[1], *Xp, *Yp, *Zp, &Ex_t[1], &Xp_t, &Yp_t, &Zp_t, cross_p, cross_c); + // stripped *'s from Xp_t, Yp_t, Zp_t, ad holten 12-2012 + mmf = multimed_r_nlay_v2 (Ex_t[1], Ex[1], mm, Xp_t, Yp_t, Zp_t); + + // more BUGS, undefind index i used in the next code, assuming 0 and 1 are to be used + // trans_Cam_Point(Ex[i], mm, G[i], X[2][0], X[2][1], X[2][2], &Ex_t[i], + // &X_t[2][0], &X_t[2][1], &X_t[2][2], cross_p, cross_c); + // X_t[2][0] *= mmf; X_t[2][1] *= mmf; + // back_trans_Point(X_t[2][0], X_t[2][1], X_t[2][2],mm, G[i], cross_p, cross_c, + // &X[2][0], &X[2][1], &X[2][2]); + // + // trans_Cam_Point(Ex[i], mm,G[i], X[3][0], X[3][1], X[3][2], &Ex_t[i], + // &X_t[3][0], &X_t[3][1], &X_t[3][2], cross_p, cross_c); + // X_t[3][0] *= mmf; X_t[3][1] *= mmf; + // back_trans_Point(X_t[3][0], X_t[3][1], X_t[3][2],mm, G[i],cross_p,cross_c, + // &X[3][0],&X[3][1],&X[3][2]); + + // i replaced by 0, stripped &'s from cross_p, cross_c, ad holten 12-2012 + trans_Cam_Point(Ex[0], mm, G[0], X[2][0], X[2][1], X[2][2], &Ex_t[0], + &X_t[2][0], &X_t[2][1], &X_t[2][2], cross_p, cross_c); + X_t[2][0] *= mmf; X_t[2][1] *= mmf; + back_trans_Point(X_t[2][0], X_t[2][1], X_t[2][2],mm, G[0], cross_p, cross_c, + &X[2][0], &X[2][1], &X[2][2]); + + // i replaced by 1, i replaced by 0, ad holten 12-2012 + trans_Cam_Point(Ex[1], mm,G[1], X[3][0], X[3][1], X[3][2], &Ex_t[1], + &X_t[3][0], &X_t[3][1], &X_t[3][2], cross_p, cross_c); + X_t[3][0] *= mmf; X_t[3][1] *= mmf; + back_trans_Point(X_t[3][0], X_t[3][1], X_t[3][2],mm, G[1],cross_p,cross_c, + &X[3][0],&X[3][1],&X[3][2]); + + /********************************************************************/ + + img_coord (*Xp,*Yp,*Zp, Ex[0],I[0], G[0], ap[0], mm, &xp1, &yp1); + img_coord (*Xp,*Yp,*Zp, Ex[1],I[1], G[1], ap[1], mm, &xp2, &yp2); + + y[0] = x1 - xp1; y[2] = x2 - xp2; + y[1] = y1 - yp1; y[3] = y2 - yp2; + + /********************************************************************/ + /* Gaussian algorithm */ + + ata (X, XtX, 4, 3); + matinv (XtX, 3); + atl (Xty, X, y, 4, 3); + matmul (beta, XtX, Xty, 3,3,1); + + stopflag = 1; + for (i=0; i<3; i++) + if (fabs(beta[i]) > 0.005) stopflag = 0; + *Xp += beta[0]; *Yp += beta[1]; *Zp += beta[2]; itnum++; + } + + /************************************************************************/ + /* residuals */ - mean_sigma0 += sigma0 * sigma0; - rmsX += sigmabeta[0] * sigmabeta[0]; - rmsY += sigmabeta[1] * sigmabeta[1]; - rmsZ += sigmabeta[2] * sigmabeta[2]; + matmul (Xbeta, X, beta, 4, 3, 1); + for (i=0; i<4; i++) + { + edach[i] = Xbeta[i] - y[i]; + omega += edach[i] * edach[i]; + } + sigma0 = sqrt(omega /(4-3)); + for (i=0; i<3; i++) sigmabeta[i] = sigma0 * sqrt(XtX[i][i]); + + mean_sigma0 += sigma0 * sigma0; + rmsX += sigmabeta[0] * sigmabeta[0]; + rmsY += sigmabeta[1] * sigmabeta[1]; + rmsZ += sigmabeta[2] * sigmabeta[2]; } diff --git a/PTV/src_c/ptv.c b/PTV/src_c/ptv.c index 62149e6..1c05c64 100644 --- a/PTV/src_c/ptv.c +++ b/PTV/src_c/ptv.c @@ -5,8 +5,8 @@ ** Author: Heinrich Stueer ** ** Description: Main modul of track. -**Dies ist eine abgespeckte Version vom Malik und Papantoniou (allerdings mit -**ein paar Anderungen) +** Dies ist eine abgespeckte Version vom Malik und Papantoniou (allerdings mit +** ein paar Anderungen) ** Created: 12.02.1998 ** Changes: ** @@ -18,818 +18,830 @@ See the file license.txt for copying permission. */ +/* ----- recent changes ------------------------------------------------------- + ad holten, 12-2012 + replaced: + if (filenumber < 10) sprintf (filein, "res/rt_is.%1d", filenumber); + else if (filenumber < 100) sprintf (filein, "res/rt_is.%2d", filenumber); + else sprintf (filein, "res/rt_is.%3d", filenumber); + by + sprintf (filein, "res/rt_is.%d", filenumber); + + replaced: + fp = fopen (filein, "r"); + if (! fp) printf("Can't open ascii file: %s\n", filein); + by + fp = fopen_rp (filein); (fopen_rp prints an error message on failure) + if (!fp) return; +------------------------------------------------------------------------------*/ + #include "ptv.h" +void level1(void); +void level2(void); +void level3(void); +void neighbours(float seekx[], float radi[], int nliste[], int *innliste, int set); + + + int seq_track_proc_c(ClientData clientData, Tcl_Interp* interp, int argc, const char** argv) { - int step, i, k; - - /*Alloc space*/ - for (i=0; i<4; i++) - mega[i]=(P *) calloc(sizeof(P),M); - - for (i=0; i<4; i++) - c4[i]=(corres *) calloc(sizeof(corres),M); - - for (i=0; i<4; i++) - for (k=0; k -1) { - liste[inliste]=i; - inliste++; - } - /*calculate possible tracks for t2 and t3 and calculate decision criteria*/ - if(inliste > 0) { - for(i=0; i < inliste; i++) { - for(j=0; j < 3; j++) - seekx[j] = 2*mega[1][liste[i]].x[j]-mega[0][mega[1][liste[i]].prev].x[j]; - - /*find neighbours in next timestep t = 2*/ - inn2liste = 0; - neighbours(seekx, trad, n2liste, &inn2liste, 2); - /* if no neighour is found no link will be established*/ - - /*calculate decision criteria*/ - if(inn2liste > 0) { - for(k=0; k acc) - esti = acc; - }/*for(l....)*/ - mega[1][liste[i]].decis[k] = esti; - mega[1][liste[i]].linkdecis[k] = n2liste[k]; - } /*else(inn2liste >0*/ - }/*for (k...)*/ - mega[1][liste[i]].inlist = inn2liste; - if(inn2liste > 1) - sort(inn2liste, mega[1][liste[i]].decis, mega[1][liste[i]].linkdecis); - - }/*if(inn1liste > 0)*/ - - }/*for(i=0....)*/ - - /*establish links by streaming completly through the data*/ - - do { - finish = 0; - - for(i=0; i 0) { - /*in the following is a sorted list of decis assumed*/ - flag = 1; - j = 0; - do { - - if(mega[2][mega[1][liste[i]].linkdecis[j]].prev < 0) { - /*found possible link*/ - mega[1][liste[i]].next = mega[1][liste[i]].linkdecis[j]; - mega[2][mega[1][liste[i]].linkdecis[j]].prev = liste[i]; - mega[2][mega[1][liste[i]].linkdecis[j]].prio = 0; - mega[1][liste[i]].finaldecis = mega[1][liste[i]].decis[j]; - flag = 0; - }/*if(p2 == -1)*/ - - /*test exiting link if would be better*/ - else if(mega[1][mega[2][mega[1][liste[i]].linkdecis[j]].prev].finaldecis - > mega[1][liste[i]].decis[j]) - { - /*current link is better and reset other link*/ - mega[1][mega[2][mega[1][liste[i]].linkdecis[j]].prev].next = -2; - - mega[1][liste[i]].next = mega[1][liste[i]].linkdecis[j]; - mega[2][mega[1][liste[i]].linkdecis[j]].prev = liste[i]; - mega[2][mega[1][liste[i]].linkdecis[j]].prio = 0; - mega[1][liste[i]].finaldecis = mega[1][liste[i]].decis[j]; - flag = 0; - finish = 1; + int i, ii, j, k, l; + float trad[3]; + int liste[M], inliste, n2liste[POSI], inn2liste, n3liste[POSI], inn3liste; + float seekx[3], esti, acc; + int finish, flag; + + /* Define some varibles. This is done every time tracking is called + (my be not necessary but is not a big problem + trad is radius of correlation neigbourhood, trad1 + is search radius for next timestep with link*/ + + trad[0] = (float) tpar.dvxmax; + trad[1] = (float) tpar.dvymax; + trad[2] = (float) tpar.dvzmax; + + /* BEGIN TRACKING*/ + + /* First start with highest priority this is if particle has already + link to previous + timstep current timstep is t[1]*/ + /*first search for tracks with previous links*/ + /*links named -1 or -2 are no links*/ + + for(inliste=0, i=0; i -1) { + liste[inliste]=i; + inliste++; } - - else { - j++; /* if first choice is not possible then try next */ - } - }while((j < mega[1][liste[i]].inlist) && flag); - - }/*if(mega[1]....)*/ - else { - mega[1][liste[i]].next=-1; /*No link could be established*/ - }/*else*/ - }/*if(mega[1] .next < 0)*/ - }/*for(i=0....)*/ - - }while(finish); - - }/*if(inlist >0)*/ - - /*END OF FIRST TRAIL*/ + /*calculate possible tracks for t2 and t3 and calculate decision criteria*/ + if (inliste > 0) { + for(i=0; i < inliste; i++) { + for(j=0; j < 3; j++) + seekx[j] = 2*mega[1][liste[i]].x[j]-mega[0][mega[1][liste[i]].prev].x[j]; + + /*find neighbours in next timestep t = 2*/ + inn2liste = 0; + neighbours(seekx, trad, n2liste, &inn2liste, 2); + /* if no neighour is found no link will be established*/ + + /*calculate decision criteria*/ + if(inn2liste > 0) { + for(k=0; k acc) + esti = acc; + } /*for(l....)*/ + mega[1][liste[i]].decis[k] = esti; + mega[1][liste[i]].linkdecis[k] = n2liste[k]; + } /*else(inn2liste >0*/ + } /*for (k...)*/ + mega[1][liste[i]].inlist = inn2liste; + if(inn2liste > 1) + sort(inn2liste, mega[1][liste[i]].decis, mega[1][liste[i]].linkdecis); + } /*if(inn1liste > 0)*/ + + } /*for(i=0....)*/ + + /*establish links by streaming completly through the data*/ + + do { + finish = 0; + + for(i=0; i 0) { + /*in the following is a sorted list of decis assumed*/ + flag = 1; + j = 0; + do { + if(mega[2][mega[1][liste[i]].linkdecis[j]].prev < 0) { + /*found possible link*/ + mega[1][liste[i]].next = mega[1][liste[i]].linkdecis[j]; + mega[2][mega[1][liste[i]].linkdecis[j]].prev = liste[i]; + mega[2][mega[1][liste[i]].linkdecis[j]].prio = 0; + mega[1][liste[i]].finaldecis = mega[1][liste[i]].decis[j]; + flag = 0; + } /*if(p2 == -1)*/ + + /*test exiting link if would be better*/ + else if(mega[1][mega[2][mega[1][liste[i]].linkdecis[j]].prev].finaldecis + > mega[1][liste[i]].decis[j]) + { + /*current link is better and reset other link*/ + mega[1][mega[2][mega[1][liste[i]].linkdecis[j]].prev].next = -2; + + mega[1][liste[i]].next = mega[1][liste[i]].linkdecis[j]; + mega[2][mega[1][liste[i]].linkdecis[j]].prev = liste[i]; + mega[2][mega[1][liste[i]].linkdecis[j]].prio = 0; + mega[1][liste[i]].finaldecis = mega[1][liste[i]].decis[j]; + flag = 0; + finish = 1; + } + + else { + j++; /* if first choice is not possible then try next */ + } + } while((j < mega[1][liste[i]].inlist) && flag); + } /*if(mega[1]....)*/ + else { + mega[1][liste[i]].next=-1; /*No link could be established*/ + } /*else*/ + } /*if(mega[1] .next < 0)*/ + } /*for(i=0....)*/ + } while(finish); + } /*if(inlist >0)*/ + + /*END OF FIRST TRAIL*/ } /*second if no previous link but in neigbouhood exist previous links*/ void level2(void) { - int i, ii, j, k, l; - float trad[3]; - int liste[M], inliste, n1liste[POSI], inn1liste; - int n2liste[POSI], inn2liste, n3liste[POSI], inn3liste; - float seekx[3], esti, acc, vel[3]; - int finish, flag, nvel; - - - /* Define some varibles. This is done every time tracking is called - (my be not necessary but is not a big problem*/ - - - trad[0]= tpar.dvxmax; - trad[1]= tpar.dvymax; - trad[2]= tpar.dvzmax; - - /* BEGIN TRACKING*/ - /* Secondly start with second priority this is if particle has already no link to - previous timstep but in Particles in neigbourhoud have. Current timstep is t[1]*/ - - /*first search for tracks with no previous links ancd no next link*/ - /*links named -1 or -2 are no links*/ - - for(inliste=0, i=0; i 1) { - for(vel[0]=0.0, vel[1]=0.0, vel[2]=0.0, nvel=0, j=0; j -1){ - for(l=0; l<3; l++) - vel[l] += mega[1][n1liste[j]].x[l]- mega[0][mega[1][n1liste[j]].prev].x[l]; - nvel++; - } - } - - if(nvel > 0) { - /*intermediate storage of center of position in next frame */ - for(l=0; l<3; l++) - mega[1][i].decis[l] = vel[l]/(float)nvel; - liste[inliste]=i; - inliste++; - } - } - } - - /*calculate possible tracks for t2 and t3 and calculate decision criteria*/ - if(inliste > 0) { - for(i=0; i < inliste; i++) { - for(j=0; j < 3; j++) - seekx[j] = mega[1][liste[i]].x[j] + mega[1][liste[i]].decis[j]; - - /*find neighbours in next timestep t = 2*/ - inn2liste = 0; - neighbours(seekx, trad, n2liste, &inn2liste, 2); - /* if no neighour is found no link will be established*/ - - /*calculate decision criteria*/ - if(inn2liste > 0) { - for(k=0; k acc) - esti = acc; - }/*for(l....)*/ - mega[1][liste[i]].decis[k] = esti; - mega[1][liste[i]].linkdecis[k] = n2liste[k]; - } /*else(inn2liste >0*/ - }/*for (k...)*/ - mega[1][liste[i]].inlist = inn2liste; - if(inn2liste > 1) - sort(inn2liste, mega[1][liste[i]].decis, mega[1][liste[i]].linkdecis); - - }/*if(inn1liste > 0)*/ - - }/*for(i=0....)*/ - - /*establish links by streaming completly through the data*/ - - do { - finish = 0; - - for(i=0; i 0) { - /*in the following is a sorted list of decis assumed*/ - flag = 1; - j = 0; - do { - - if(mega[2][mega[1][liste[i]].linkdecis[j]].prev < 0) { - /*found possible link*/ - mega[1][liste[i]].next = mega[1][liste[i]].linkdecis[j]; - mega[2][mega[1][liste[i]].linkdecis[j]].prev = liste[i]; - mega[2][mega[1][liste[i]].linkdecis[j]].prio = 1; - mega[1][liste[i]].finaldecis = mega[1][liste[i]].decis[j]; - flag = 0; - }/*if(p2 == -1)*/ - - /*test exiting link if would be better*/ - else if((mega[1][mega[2][mega[1][liste[i]].linkdecis[j]].prev].finaldecis - > mega[1][liste[i]].decis[j]) - && (mega[2][mega[1][liste[i]].linkdecis[j]].prio >= 1)) - { - /*current link is better and reset other link*/ - mega[1][mega[2][mega[1][liste[i]].linkdecis[j]].prev].next = -2; - - mega[1][liste[i]].next = mega[1][liste[i]].linkdecis[j]; - mega[2][mega[1][liste[i]].linkdecis[j]].prev = liste[i]; - mega[2][mega[1][liste[i]].linkdecis[j]].prio = 1; - mega[1][liste[i]].finaldecis = mega[1][liste[i]].decis[j]; - flag = 0; - finish = 1; - } - - else { - j++; /* if first choice is not possible then try next */ - } + int i, ii, j, k, l; + float trad[3]; + int liste[M], inliste, n1liste[POSI], inn1liste; + int n2liste[POSI], inn2liste, n3liste[POSI], inn3liste; + float seekx[3], esti, acc, vel[3]; + int finish, flag, nvel; - }while((j < mega[1][liste[i]].inlist) && flag); - }/*if(mega[1]....)*/ - else { - mega[1][liste[i]].next=-1; /*No link could be established*/ - }/*else*/ - }/*if(mega[1] .next<0)*/ - }/*for(i=0....)*/ + /* Define some varibles. This is done every time tracking is called + (my be not necessary but is not a big problem*/ + trad[0] = (float) tpar.dvxmax; + trad[1] = (float) tpar.dvymax; + trad[2] = (float) tpar.dvzmax; - }while(finish); + /* BEGIN TRACKING*/ + /* Secondly start with second priority this is if particle has already no link to + previous timstep but in Particles in neigbourhoud have. Current timstep is t[1]*/ - }/*if(inlist >0)*/ + /*first search for tracks with no previous links ancd no next link*/ + /*links named -1 or -2 are no links*/ - /*END OF second TRAIL*/ + for(inliste=0, i=0; i 1) { + for (vel[0]=0.0, vel[1]=0.0, vel[2]=0.0, nvel=0, j=0; j -1) { + for(l=0; l<3; l++) + vel[l] += mega[1][n1liste[j]].x[l]- mega[0][mega[1][n1liste[j]].prev].x[l]; + nvel++; + } + } + if(nvel > 0) { + /*intermediate storage of center of position in next frame */ + for(l=0; l<3; l++) + mega[1][i].decis[l] = vel[l]/(float)nvel; + liste[inliste]=i; + inliste++; + } + } + } + + /*calculate possible tracks for t2 and t3 and calculate decision criteria*/ + if (inliste > 0) { + for(i=0; i < inliste; i++) { + for(j=0; j < 3; j++) + seekx[j] = mega[1][liste[i]].x[j] + mega[1][liste[i]].decis[j]; + + /*find neighbours in next timestep t = 2*/ + inn2liste = 0; + neighbours(seekx, trad, n2liste, &inn2liste, 2); + /* if no neighour is found no link will be established*/ + + /*calculate decision criteria*/ + if(inn2liste > 0) { + for(k=0; k acc) + esti = acc; + } /*for(l....)*/ + mega[1][liste[i]].decis[k] = esti; + mega[1][liste[i]].linkdecis[k] = n2liste[k]; + } /*else(inn2liste >0*/ + } /*for (k...)*/ + mega[1][liste[i]].inlist = inn2liste; + if (inn2liste > 1) + sort(inn2liste, mega[1][liste[i]].decis, mega[1][liste[i]].linkdecis); + + }/*if(inn1liste > 0)*/ + }/*for(i=0....)*/ + /*establish links by streaming completly through the data*/ + do { + finish = 0; + for(i=0; i 0) { + /*in the following is a sorted list of decis assumed*/ + flag = 1; + j = 0; + do { + if(mega[2][mega[1][liste[i]].linkdecis[j]].prev < 0) { + /*found possible link*/ + mega[1][liste[i]].next = mega[1][liste[i]].linkdecis[j]; + mega[2][mega[1][liste[i]].linkdecis[j]].prev = liste[i]; + mega[2][mega[1][liste[i]].linkdecis[j]].prio = 1; + mega[1][liste[i]].finaldecis = mega[1][liste[i]].decis[j]; + flag = 0; + } /*if(p2 == -1)*/ + + /*test exiting link if would be better*/ + else if((mega[1][mega[2][mega[1][liste[i]].linkdecis[j]].prev].finaldecis + > mega[1][liste[i]].decis[j]) + && (mega[2][mega[1][liste[i]].linkdecis[j]].prio >= 1)) + { + /*current link is better and reset other link*/ + mega[1][mega[2][mega[1][liste[i]].linkdecis[j]].prev].next = -2; + + mega[1][liste[i]].next = mega[1][liste[i]].linkdecis[j]; + mega[2][mega[1][liste[i]].linkdecis[j]].prev = liste[i]; + mega[2][mega[1][liste[i]].linkdecis[j]].prio = 1; + mega[1][liste[i]].finaldecis = mega[1][liste[i]].decis[j]; + flag = 0; + finish = 1; + } + + else { + j++; /* if first choice is not possible then try next */ + } + } while((j < mega[1][liste[i]].inlist) && flag); + }/*if(mega[1]....)*/ + else { + mega[1][liste[i]].next=-1; /*No link could be established*/ + }/*else*/ + }/*if(mega[1] .next<0)*/ + }/*for(i=0....)*/ + } while(finish); + } /*if(inlist >0)*/ + /*END OF second TRAIL*/ } @@ -837,137 +849,126 @@ void level2(void) void level3(void) { - int i, ii, j, k, l; - float trad[3]; - int liste[M], inliste, n2liste[POSI], inn2liste, n3liste[POSI], inn3liste; - float seekx[3], esti, acc; - int finish, flag; - - - /* Define some varibles. This is done every time tracking is called - (my be not necessary but is not a big problem*/ - - trad[0]= tpar.dvxmax; - trad[1]= tpar.dvymax; - trad[2]= tpar.dvzmax; - - - /* BEGIN TRACKING*/ - - - /* Thirdly start with third priority this is if particle has no link to previous - timstep and in Particles in neigbourhoud have not. Current timstep is t[1]*/ - - /*first search for tracks with no previous links and no next link*/ - /*links named -1 or -2 are no links*/ - - for(inliste=0, i=0; i 0) { - for(i=0; i < inliste; i++) { - for(j=0; j < 3; j++) - seekx[j] = mega[1][liste[i]].x[j]; - - /*find neighbours in next timestep t = 2*/ - inn2liste = 0; - neighbours(seekx, trad, n2liste, &inn2liste, 2); - /* if no neighour is found no link will be established*/ - - /*calculate decision criteria*/ - if(inn2liste > 0) { - for(k=0; k acc) - esti = acc; - }/*for(l....)*/ - mega[1][liste[i]].decis[k] = esti; - mega[1][liste[i]].linkdecis[k] = n2liste[k]; - } /*else(inn2liste >0*/ - }/*for (k...)*/ - mega[1][liste[i]].inlist = inn2liste; - if(inn2liste > 1) - sort(inn2liste, mega[1][liste[i]].decis, mega[1][liste[i]].linkdecis); - - }/*if(inn1liste > 0)*/ - - }/*for(i=0....)*/ - - /*establish links by streaming completly through the data*/ - - do { - finish = 0; - for(i=0; i 0) { - /*in the following is a sorted list of decis assumed*/ - flag = 1; - j = 0; - do { - - if(mega[2][mega[1][liste[i]].linkdecis[j]].prev < 0) { - /*found possible link*/ - mega[1][liste[i]].next = mega[1][liste[i]].linkdecis[j]; - mega[2][mega[1][liste[i]].linkdecis[j]].prev = liste[i]; - mega[2][mega[1][liste[i]].linkdecis[j]].prio = 2; - mega[1][liste[i]].finaldecis = mega[1][liste[i]].decis[j]; - flag = 0; - }/*if(p2 == -1)*/ + int i, ii, j, k, l; + float trad[3]; + int liste[M], inliste, n2liste[POSI], inn2liste, n3liste[POSI], inn3liste; + float seekx[3], esti, acc; + int finish, flag; - /*test exiting link if would be better*/ - else if((mega[1][mega[2][mega[1][liste[i]].linkdecis[j]].prev].finaldecis - > mega[1][liste[i]].decis[j]) - && (mega[2][mega[1][liste[i]].linkdecis[j]].prio >= 2)) - { - /*current link is better and reset other link*/ - mega[1][mega[2][mega[1][liste[i]].linkdecis[j]].prev].next = -2; - - mega[1][liste[i]].next = mega[1][liste[i]].linkdecis[j]; - mega[2][mega[1][liste[i]].linkdecis[j]].prev = liste[i]; - mega[2][mega[1][liste[i]].linkdecis[j]].prio = 2; - mega[1][liste[i]].finaldecis = mega[1][liste[i]].decis[j]; - flag = 0; - finish = 1; + + /* Define some varibles. This is done every time tracking is called + (my be not necessary but is not a big problem*/ + + trad[0] = (float) tpar.dvxmax; + trad[1] = (float) tpar.dvymax; + trad[2] = (float) tpar.dvzmax; + + /* BEGIN TRACKING*/ + + + /* Thirdly start with third priority this is if particle has no link to previous + timstep and in Particles in neigbourhoud have not. Current timstep is t[1]*/ + + /*first search for tracks with no previous links and no next link*/ + /*links named -1 or -2 are no links*/ + + for(inliste=0, i=0; i0)*/ - /*END OF THIRD TRAIL*/ + /*calculate possible tracks for t2 and t3 and calculate decision criteria*/ + if (inliste > 0) { + for(i=0; i < inliste; i++) { + for(j=0; j < 3; j++) + seekx[j] = mega[1][liste[i]].x[j]; + + /*find neighbours in next timestep t = 2*/ + inn2liste = 0; + neighbours(seekx, trad, n2liste, &inn2liste, 2); + /* if no neighour is found no link will be established*/ + + /*calculate decision criteria*/ + if(inn2liste > 0) { + for(k=0; k acc) + esti = acc; + } /*for(l....)*/ + mega[1][liste[i]].decis[k] = esti; + mega[1][liste[i]].linkdecis[k] = n2liste[k]; + } /*else(inn2liste >0*/ + } /*for (k...)*/ + mega[1][liste[i]].inlist = inn2liste; + if (inn2liste > 1) + sort(inn2liste, mega[1][liste[i]].decis, mega[1][liste[i]].linkdecis); + } /*if(inn1liste > 0)*/ + }/*for(i=0....)*/ + + /*establish links by streaming completly through the data*/ + + do { + finish = 0; + for(i=0; i 0) { + /*in the following is a sorted list of decis assumed*/ + flag = 1; + j = 0; + do { + if(mega[2][mega[1][liste[i]].linkdecis[j]].prev < 0) { + /*found possible link*/ + mega[1][liste[i]].next = mega[1][liste[i]].linkdecis[j]; + mega[2][mega[1][liste[i]].linkdecis[j]].prev = liste[i]; + mega[2][mega[1][liste[i]].linkdecis[j]].prio = 2; + mega[1][liste[i]].finaldecis = mega[1][liste[i]].decis[j]; + flag = 0; + } /*if(p2 == -1)*/ + + /*test exiting link if would be better*/ + else if((mega[1][mega[2][mega[1][liste[i]].linkdecis[j]].prev].finaldecis + > mega[1][liste[i]].decis[j]) + && (mega[2][mega[1][liste[i]].linkdecis[j]].prio >= 2)) + { + /*current link is better and reset other link*/ + mega[1][mega[2][mega[1][liste[i]].linkdecis[j]].prev].next = -2; + mega[1][liste[i]].next = mega[1][liste[i]].linkdecis[j]; + mega[2][mega[1][liste[i]].linkdecis[j]].prev = liste[i]; + mega[2][mega[1][liste[i]].linkdecis[j]].prio = 2; + mega[1][liste[i]].finaldecis = mega[1][liste[i]].decis[j]; + flag = 0; + finish = 1; + } + else { + j++; /* if first choice is not possible then try next */ + } + } while((j < mega[1][liste[i]].inlist) && flag); + } /*if(mega[1]....)*/ + else { + mega[1][liste[i]].next = -1; /*No link could be established*/ + } /*else*/ + } /*if(mega[1] < 0)*/ + } /*for(i=0....)*/ + } while(finish); + }/*if(inlist >0)*/ + /*END OF THIRD TRAIL*/ } @@ -976,78 +977,76 @@ void level3(void) void sort(int n, float a[], int b[]) { - int flag = 0, i, itemp; - float ftemp; - - do { - flag = 0; - for(i=0; i<(n-1); i++) - if(a[i] > a[i+1]) { - ftemp = a[i]; - itemp = b[i]; - a[i] = a[i+1]; - b[i] = b[i+1]; - a[i+1] = ftemp; - b[i+1] = itemp; - flag = 1; - } - }while(flag); + int flag = 0, i, itemp; + float ftemp; + + do { + flag = 0; + for(i=0; i<(n-1); i++) + if(a[i] > a[i+1]) { + ftemp = a[i]; + itemp = b[i]; + a[i] = a[i+1]; + b[i] = b[i+1]; + a[i+1] = ftemp; + b[i+1] = itemp; + flag = 1; + } + } while(flag); } void rotate_dataset(void) { - void * tmp; - void * tmp2; - int i; - - /*rotate dataset by changeing pointer*/ - tmp = mega[0]; - mega[0] = mega[1]; - mega[1] = mega[2]; - mega[2] = mega[3]; - mega[3] =tmp; - - /*rotate counter*/ - m[0] = m[1]; - m[1] = m[2]; - m[2] = m[3]; - - tmp = c4[0]; - c4[0] = c4[1]; - c4[1] = c4[2]; - c4[2] = c4[3]; - c4[3] =tmp; - - for(i=0; i<4; i++) { - tmp2 = t4[0][i]; - t4[0][i] = t4[1][i]; - t4[1][i] = t4[2][i]; - t4[2][i] = t4[3][i]; - t4[3][i] =tmp2; - - nt4[0][i] = nt4[1][i]; - nt4[1][i] = nt4[2][i]; - nt4[2][i] = nt4[3][i]; - } + void * tmp; + void * tmp2; + int i; + + /*rotate dataset by changeing pointer*/ + tmp = mega[0]; + mega[0] = mega[1]; + mega[1] = mega[2]; + mega[2] = mega[3]; + mega[3] =tmp; + + /*rotate counter*/ + m[0] = m[1]; + m[1] = m[2]; + m[2] = m[3]; + + tmp = c4[0]; + c4[0] = c4[1]; + c4[1] = c4[2]; + c4[2] = c4[3]; + c4[3] =tmp; + + for(i=0; i<4; i++) { + tmp2 = t4[0][i]; + t4[0][i] = t4[1][i]; + t4[1][i] = t4[2][i]; + t4[2][i] = t4[3][i]; + t4[3][i] =tmp2; + + nt4[0][i] = nt4[1][i]; + nt4[1][i] = nt4[2][i]; + nt4[2][i] = nt4[3][i]; + } } - - void neighbours(float seekx[], float radi[], int nliste[], int *innliste, int set) { - int i; - /*search for points in srearch radius. No sorted list is supported, - although sorted in z would increase speed*/ - - for(i=0; i< m[set]; i++) { - if(fabs(seekx[0] - mega[set][i].x[0]) < radi[0]) - if(fabs(seekx[1] - mega[set][i].x[1]) < radi[1]) - if(fabs(seekx[2] - mega[set][i].x[2]) < radi[2]){ - nliste[*innliste]=i; - (*innliste)++; - if(*innliste > POSI) - printf("More Points found than can be supported! Reduce search area or increase POSI\n"); - } - } + int i; + /*search for points in srearch radius. No sorted list is supported, + although sorted in z would increase speed*/ + + for(i=0; i< m[set]; i++) { + if(fabs(seekx[0] - mega[set][i].x[0]) < radi[0]) + if(fabs(seekx[1] - mega[set][i].x[1]) < radi[1]) + if(fabs(seekx[2] - mega[set][i].x[2]) < radi[2]) { + nliste[*innliste]=i; + (*innliste)++; + if(*innliste > POSI) + printf("More Points found than can be supported! Reduce search area or increase POSI\n"); + } + } } diff --git a/PTV/src_c/ptv.h b/PTV/src_c/ptv.h index e672d7d..166d51a 100644 Binary files a/PTV/src_c/ptv.h and b/PTV/src_c/ptv.h differ diff --git a/PTV/src_c/ray_tracing.c b/PTV/src_c/ray_tracing.c index 662c265..70f09a5 100644 --- a/PTV/src_c/ray_tracing.c +++ b/PTV/src_c/ray_tracing.c @@ -1,68 +1,62 @@ /**************************************************************************** -Routine: ray_tracing +Routine: ray_tracing -Author/Copyright: Hans-Gerd Maas +Author/Copyright: Hans-Gerd Maas -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: 21.4.88 +Creation Date: 21.4.88 -Description: traces one ray, given by image coordinates, - exterior and interior orientation - through dufferent media - (see Hoehle, Manual of photogrammetry) +Description: traces one ray, given by image coordinates, + exterior and interior orientation + through dufferent media + (see Hoehle, Manual of photogrammetry) -Routines contained: - +Routines contained: - ****************************************************************************/ #include "ptv.h" +void norm_cross(double a[3], double b[3], double *n1, double *n2, double *n3); +void dot(double a[3], double b[3], double *d); +void modu(double a[3], double *m); - -void ray_tracing (x,y,Ex,I,mm,Xb2,Yb2,Zb2,a3,b3,c3) - -double x, y; -Exterior Ex; -Interior I; -mm_np mm; -double *Xb2, *Yb2, *Zb2, *a3, *b3, *c3; +#ifdef EVER_CALLED // added, ad holten 12-2012 +void ray_tracing (double x, double y, Exterior Ex, Interior I, mm_np mm, + double *Xb2, double *Yb2, double *Zb2, double *a3, double *b3, double *c3) /* ray-tracing, see HOEHLE and Manual of Photogrammetry */ - { - double a1, b1, c1, a2, b2, c2, Xb1, Yb1, Zb1, d1, d2, cosi1, cosi2, + double a1, b1, c1, a2, b2, c2, Xb1, Yb1, Zb1, d1, d2, cosi1, cosi2, vect1[3], vect2[3], factor, s2; s2 = sqrt (x*x + y*y + I.cc*I.cc); /* direction cosines in image coordinate system */ - vect1[0] = x/s2; vect1[1] = y/s2; vect1[2] = -I.cc/s2; + vect1[0] = x/s2; vect1[1] = y/s2; vect1[2] = -I.cc/s2; matmul (vect2, Ex.dm, vect1, 3,3,1); - + /* direction cosines in space coordinate system , medium n1 */ - a1 = vect2[0]; b1 = vect2[1]; c1 = vect2[2]; - - d1 = -(Ex.z0 - mm.d[0]) / c1; + a1 = vect2[0]; b1 = vect2[1]; c1 = vect2[2]; d1 = -(Ex.z0 - mm.d[0]) / c1; /* point on the horizontal plane between n1,n2 */ - Xb1 = Ex.x0 + d1*a1; Yb1 = Ex.y0 + d1*b1; Zb1 = Ex.z0 + d1*c1; + Xb1 = Ex.x0 + d1*a1; Yb1 = Ex.y0 + d1*b1; Zb1 = Ex.z0 + d1*c1; cosi1 = c1; factor = cosi1 * mm.n1/mm.n2[0] - + sqrt (1 - (mm.n1*mm.n1)/(mm.n2[0]*mm.n2[0]) - + (cosi1*cosi1)*(mm.n1*mm.n1)/(mm.n2[0]*mm.n2[0])); + + sqrt (1 - (mm.n1*mm.n1)/(mm.n2[0]*mm.n2[0]) + + (cosi1*cosi1)*(mm.n1*mm.n1)/(mm.n2[0]*mm.n2[0])); /* direction cosines in space coordinate system , medium n2 */ a2 = a1 * mm.n1/mm.n2[0]; b2 = b1 * mm.n1/mm.n2[0]; c2 = c1 * mm.n1/mm.n2[0] - factor; - d2 = -mm.d[0]/c2; /* point on the horizontal plane between n2,n3 */ @@ -70,38 +64,30 @@ double *Xb2, *Yb2, *Zb2, *a3, *b3, *c3; cosi2 = c2; factor = cosi2 * mm.n2[0]/mm.n3 - + sqrt (1 - (mm.n2[0]*mm.n2[0])/(mm.n3*mm.n3) - + (cosi2*cosi2)*(mm.n2[0]*mm.n2[0])/(mm.n3*mm.n3)); + + sqrt (1 - (mm.n2[0]*mm.n2[0])/(mm.n3*mm.n3) + + (cosi2*cosi2)*(mm.n2[0]*mm.n2[0])/(mm.n3*mm.n3)); /* direction cosines in space coordinate system , medium mm.n3 */ *a3 = a2 * mm.n2[0]/mm.n3; *b3 = b2 * mm.n2[0]/mm.n3; *c3 = c2 * mm.n2[0]/mm.n3 - factor; } - -void point_line_line(Ex0, I0, G0, mm, gX0, gY0, gZ0, a0, b0, c0, - Ex1, I1, G1, gX1, gY1, gZ1, a1, b1, c1, x,y,z) - - -Exterior Ex0; -Interior I0; -Glass G0; -Exterior Ex1; -Interior I1; -Glass G1; -mm_np mm; -double gX0, gY0, gZ0, a0, b0, c0, gX1, gY1, gZ1, a1, b1, c1; -double *x,*y,*z; - -//Beat Lüthi Nov 2008 - +#endif + +void point_line_line(Exterior Ex0, Interior I0, Glass G0, mm_np mm, + double gX0, double gY0, double gZ0, double a0, double b0, double c0, + Exterior Ex1, Interior I1, Glass G1, + double gX1, double gY1, double gZ1, double a1, double b1, double c1, + double *x, double *y, double *z) +//Beat L¸thi Nov 2008 { - double a[3],b[3],A[3],B[3],n[3],AB[3],dist,Bp[3],c,ABp[3],mABp,d,Ae1[3],e1[3],mb,nb[3],cosb,f,Ap[3],App[3],BpAe1[3],mBpAe1; - double dummy; + double a[3], b[3], A[3], B[3], n[3], AB[3], dist, Bp[3], c,ABp[3], mABp, d,Ae1[3], + e1[3], mb,nb[3], cosb, f, Ap[3], App[3], BpAe1[3], mBpAe1; + double dummy; A[0]=gX0;A[1]=gY0;A[2]=gZ0; B[0]=gX1;B[1]=gY1;B[2]=gZ1; - a[0]=a0;a[1]=b0;a[2]=c0; + a[0]=a0;a[1]=b0;a[2]=c0; b[0]=a1;b[1]=b1;b[2]=c1; norm_cross(b,a,&n[0],&n[1],&n[2]); AB[0]=B[0]-A[0]; @@ -132,8 +118,8 @@ double *x,*y,*z; nb[1]=b[1]/mb; nb[2]=b[2]/mb; dot(e1,nb,&cosb); - f=d/cosb; - Ap[0]=Bp[0]+f*nb[0]; + f=d/cosb; + Ap[0]=Bp[0]+f*nb[0]; Ap[1]=Bp[1]+f*nb[1]; Ap[2]=Bp[2]+f*nb[2]; App[0]=B[0]+f*nb[0]; @@ -147,13 +133,10 @@ double *x,*y,*z; *z=0.5*(Ap[2]+App[2]); } -void norm_cross(a,b,n1,n2,n3) - -double a[3],b[3],*n1,*n2,*n3; -//Beat Lüthi Nov 2008 - +void norm_cross(double a[3], double b[3], double *n1, double *n2, double *n3) +//Beat L¸thi Nov 2008 { - double res[3],dummy; + double res[3],dummy; res[0]=a[1]*b[2]-a[2]*b[1]; res[1]=a[2]*b[0]-a[0]*b[2]; @@ -165,70 +148,56 @@ double a[3],b[3],*n1,*n2,*n3; *n3=res[2]/dummy; } -void dot(a,b,d) - -double a[3],b[3],*d; -//Beat Lüthi Nov 2008 - +void dot(double a[3], double b[3], double *d) +//Beat L¸thi Nov 2008 { *d=a[0]*b[0]+a[1]*b[1]+a[2]*b[2]; } -void modu(a,m) - -double a[3],*m; -//Beat Lüthi Nov 2008 - +void modu(double a[3], double *m) +//Beat L¸thi Nov 2008 { *m=sqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]); } -void ray_tracing_v2 (x,y,Ex,I,G,mm,Xb2,Yb2,Zb2,a3,b3,c3) - -double x, y; -Exterior Ex; -Interior I; -Glass G; -mm_np mm; -double *Xb2, *Yb2, *Zb2, *a3, *b3, *c3; - +void ray_tracing_v2 (double x, double y, Exterior Ex, Interior I, Glass G, mm_np mm, + double *Xb2, double *Yb2, double* Zb2, + double *a3, double *b3, double *c3) /* ray-tracing, see HOEHLE and Manual of Photogrammetry */ - { - double a1, b1, c1, a2, b2, c2, Xb1, Yb1, Zb1, d1, d2, cosi1, cosi2, - vect1[3], vect2[3], factor, s2; + double a1, b1, c1, a2, b2, c2, Xb1, Yb1, Zb1, d1, d2, + vect1[3], vect2[3], s2; double a[3],b[3],base2[3],c,dummy,bn[3],bp[3],n,p; s2 = sqrt (x*x + y*y + I.cc*I.cc); /* direction cosines in image coordinate system */ - vect1[0] = x/s2; vect1[1] = y/s2; vect1[2] = -I.cc/s2; + vect1[0] = x/s2; vect1[1] = y/s2; vect1[2] = -I.cc/s2; matmul (vect2, Ex.dm, vect1, 3,3,1); - + /* direction cosines in space coordinate system , medium n1 */ - a1 = vect2[0]; b1 = vect2[1]; c1 = vect2[2]; + a1 = vect2[0]; b1 = vect2[1]; c1 = vect2[2]; + + //old d1 = -(Ex.z0 - mm.d[0]) / c1; + //find dist to outer interface + //... from Jakob Mann vector3 XLinePlane(vector3 a, vector3 b, struct plane pl) + //... a + b*((pl.c - dot(pl.base[2],a))/dot(pl.base[2],b)); - //old d1 = -(Ex.z0 - mm.d[0]) / c1; - //find dist to outer interface - //... from Jakob Mann vector3 XLinePlane(vector3 a, vector3 b, struct plane pl) - //... a + b*((pl.c - dot(pl.base[2],a))/dot(pl.base[2],b)); - /*Ex.x0=0.; - Ex.y0=20.; - Ex.z0=10.; - Ex.omega=-0.7853981; - Ex.phi=0.; - Ex.kappa=0.; - G.vec_x=0.; - G.vec_y=10.; - G.vec_z=0.; + Ex.y0=20.; + Ex.z0=10.; + Ex.omega=-0.7853981; + Ex.phi=0.; + Ex.kappa=0.; + G.vec_x=0.; + G.vec_y=10.; + G.vec_z=0.; vect2[0]=0.; vect2[1]=-1./sqrt(2.); vect2[2]=-1./sqrt(2.);*/ - a[0]=Ex.x0;a[1]=Ex.y0;a[2]=Ex.z0; b[0]=vect2[0];b[1]=vect2[1];b[2]=vect2[2]; c=sqrt(G.vec_x*G.vec_x+G.vec_y*G.vec_y+G.vec_z*G.vec_z); @@ -241,7 +210,7 @@ double *Xb2, *Yb2, *Zb2, *a3, *b3, *c3; /* point on the horizontal plane between n1,n2 */ - //old Xb1 = Ex.x0 + d1*a1; Yb1 = Ex.y0 + d1*b1; Zb1 = Ex.z0 + d1*c1; + //old Xb1 = Ex.x0 + d1*a1; Yb1 = Ex.y0 + d1*b1; Zb1 = Ex.z0 + d1*c1; Xb1=a[0]+b[0]*d1; Yb1=a[1]+b[1]*d1; Zb1=a[2]+b[2]*d1; @@ -249,8 +218,8 @@ double *Xb2, *Yb2, *Zb2, *a3, *b3, *c3; //old cosi1 = c1; //cosi1=base2[0]*b[0]+base2[1]*b[1]+base2[2]*b[2]; //factor = cosi1 * mm.n1/mm.n2[0] - // + sqrt (1 - (mm.n1*mm.n1)/(mm.n2[0]*mm.n2[0]) - // + (cosi1*cosi1)*(mm.n1*mm.n1)/(mm.n2[0]*mm.n2[0])); + // + sqrt (1 - (mm.n1*mm.n1)/(mm.n2[0]*mm.n2[0]) + // + (cosi1*cosi1)*(mm.n1*mm.n1)/(mm.n2[0]*mm.n2[0])); /* direction cosines in space coordinate system , medium n2 */ //old a2 = a1 * mm.n1/mm.n2[0]; @@ -259,7 +228,7 @@ double *Xb2, *Yb2, *Zb2, *a3, *b3, *c3; //old d2 = -mm.d[0]/c2; - bn[0]=base2[0];bn[1]=base2[1];bn[2]=base2[2]; + bn[0]=base2[0];bn[1]=base2[1];bn[2]=base2[2]; n=(b[0]*bn[0]+b[1]*bn[1]+b[2]*bn[2]); bp[0]=b[0]-bn[0]*n;bp[1]=b[1]-bn[1]*n;bp[2]=b[2]-bn[2]*n; dummy=sqrt(bp[0]*bp[0]+bp[1]*bp[1]+bp[2]*bp[2]); @@ -272,7 +241,7 @@ double *Xb2, *Yb2, *Zb2, *a3, *b3, *c3; a2=p*bp[0]+n*bn[0]; b2=p*bp[1]+n*bn[1]; c2=p*bp[2]+n*bn[2]; - d2=mm.d[0]/fabs((base2[0]*a2+base2[1]*b2+base2[2]*c2)); + d2=mm.d[0]/fabs((base2[0]*a2+base2[1]*b2+base2[2]*c2)); /* point on the horizontal plane between n2,n3 */ @@ -281,8 +250,8 @@ double *Xb2, *Yb2, *Zb2, *a3, *b3, *c3; //old cosi2 = c2; //cosi2=base2[0]*a2+base2[1]*b2+base2[2]*c2; //factor = cosi2 * mm.n2[0]/mm.n3 - // + sqrt (1 - (mm.n2[0]*mm.n2[0])/(mm.n3*mm.n3) - // + (cosi2*cosi2)*(mm.n2[0]*mm.n2[0])/(mm.n3*mm.n3)); + // + sqrt (1 - (mm.n2[0]*mm.n2[0])/(mm.n3*mm.n3) + // + (cosi2*cosi2)*(mm.n2[0]*mm.n2[0])/(mm.n3*mm.n3)); /* direction cosines in space coordinate system , medium mm.n3 */ //old *a3 = a2 * mm.n2[0]/mm.n3; diff --git a/PTV/src_c/readme.txt b/PTV/src_c/readme.txt index 4d4667b..1864cc2 100644 Binary files a/PTV/src_c/readme.txt and b/PTV/src_c/readme.txt differ diff --git a/PTV/src_c/resource.h b/PTV/src_c/resource.h index d22f6cd..b68ddcd 100644 Binary files a/PTV/src_c/resource.h and b/PTV/src_c/resource.h differ diff --git a/PTV/src_c/rotation.c b/PTV/src_c/rotation.c index 06e243b..713dabd 100644 --- a/PTV/src_c/rotation.c +++ b/PTV/src_c/rotation.c @@ -1,43 +1,39 @@ /**************************************************************************** -Routine: rotation.c +Routine: rotation.c -Author/Copyright: Hans-Gerd Maas +Author/Copyright: Hans-Gerd Maas -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: 21.4.88 +Creation Date: 21.4.88 -Description: computes the rotation matrix for given three - angles omega, phi, kappa of exterior orientation - (see: Kraus) +Description: computes the rotation matrix for given three + angles omega, phi, kappa of exterior orientation + (see: Kraus) -Routines contained: - +Routines contained: - ****************************************************************************/ #include "ptv.h" -void rotation_matrix (Ex, dm) - -Exterior Ex; -Dmatrix dm; - +void rotation_matrix (Exterior Ex, Dmatrix dm) { - dm[0][0] = cos(Ex.phi) * cos(Ex.kappa); - dm[0][1] = (-1) * cos(Ex.phi) * sin(Ex.kappa); - dm[0][2] = sin(Ex.phi); - dm[1][0] = cos(Ex.omega) * sin(Ex.kappa) - + sin(Ex.omega) * sin(Ex.phi) * cos(Ex.kappa); - dm[1][1] = cos(Ex.omega) * cos(Ex.kappa) - - sin(Ex.omega) * sin(Ex.phi) * sin(Ex.kappa); - dm[1][2] = (-1) * sin(Ex.omega) * cos(Ex.phi); - dm[2][0] = sin(Ex.omega) * sin(Ex.kappa) - - cos(Ex.omega) * sin(Ex.phi) * cos(Ex.kappa); - dm[2][1] = sin(Ex.omega) * cos(Ex.kappa) - + cos(Ex.omega) * sin(Ex.phi) * sin(Ex.kappa); - dm[2][2] = cos(Ex.omega) * cos(Ex.phi); + dm[0][0] = cos(Ex.phi) * cos(Ex.kappa); + dm[0][1] = -cos(Ex.phi) * sin(Ex.kappa); + dm[0][2] = sin(Ex.phi); + dm[1][0] = cos(Ex.omega) * sin(Ex.kappa) + + sin(Ex.omega) * sin(Ex.phi) * cos(Ex.kappa); + dm[1][1] = cos(Ex.omega) * cos(Ex.kappa) + - sin(Ex.omega) * sin(Ex.phi) * sin(Ex.kappa); + dm[1][2] = -sin(Ex.omega) * cos(Ex.phi); + dm[2][0] = sin(Ex.omega) * sin(Ex.kappa) + - cos(Ex.omega) * sin(Ex.phi) * cos(Ex.kappa); + dm[2][1] = sin(Ex.omega) * cos(Ex.kappa) + + cos(Ex.omega) * sin(Ex.phi) * sin(Ex.kappa); + dm[2][2] = cos(Ex.omega) * cos(Ex.phi); } diff --git a/PTV/src_c/segmentation.c b/PTV/src_c/segmentation.c index f6c7bdb..a00a9fa 100644 --- a/PTV/src_c/segmentation.c +++ b/PTV/src_c/segmentation.c @@ -1,363 +1,363 @@ /**************************************************************************** -Routine: segmentation.c +Routine: segmentation.c -Author/Copyright: Hans-Gerd Maas +Author/Copyright: Hans-Gerd Maas -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: 1988/89 +Creation Date: 1988/89 -Description: target recognition with adaptive threshold - based on high pass filter and connectivity analysis - with peak fitting technique +Description: target recognition with adaptive threshold + based on high pass filter and connectivity analysis + with peak fitting technique -Routines contained: unsharp_mask () ... +Routines contained: unsharp_mask () ****************************************************************************/ #include "ptv.h" -void highpass (pic_name, img, img_hp, dim_lp, filter_hp, field, nr) -char pic_name[256]; /* image name */ -unsigned char *img; -unsigned char *img_hp; /* highpass filtered image */ -int dim_lp; /* dimension of subtracted lowpass image */ -int filter_hp; /* flag for additional filtering of _hp */ -int field; /* field to be used */ -int nr; /* image number for display */ - +void highpass (char* pic_name, unsigned char *img, unsigned char *img_hp, int dim_lp, int filter_hp, int field, int nr) +/* + char *pic_name image name + unsigned char *img + unsigned char *img_hp highpass filtered image + int dim_lp dimension of subtracted lowpass image + int filter_hp flag for additional filtering of _hp + int field; field to be used + int nr; image number for display +*/ { - register int i; - FILE *fp; - unsigned char *img_lp; - char lp_name[256], hp_name[256]; - register unsigned char *ptr1, *ptr2, *ptr3; - void unsharp_mask (); - - sprintf (lp_name, "%s_lp", pic_name); - sprintf (hp_name, "%s_hp", pic_name); - - /* allocate memory */ - - img_lp = (unsigned char *) calloc (imgsize, 1); - if ( ! img_lp) - { - puts ("calloc for img_lp --> error"); - exit (1); + register int i; + FILE *fp; + unsigned char *img_lp; + char lp_name[256], hp_name[256]; + register unsigned char *ptr1, *ptr2, *ptr3; + + // void unsharp_mask(int n, unsigned char *img0, unsigned char *img_lp); + + sprintf (lp_name, "%s_lp", pic_name); + sprintf (hp_name, "%s_hp", pic_name); + + /* allocate memory */ + img_lp = (unsigned char *) calloc (imgsize, 1); + if (! img_lp) { + puts ("calloc for img_lp --> error"); + exit (1); } - unsharp_mask (dim_lp, img, img_lp); + unsharp_mask (dim_lp, img, img_lp); - if (examine == 3) - { - fp = fopen (lp_name, "w"); /* save lowpass image */ - if (tiff_flag) { write_tiff (lp_name, img_lp, imx, imy); } - else { fwrite (img_lp, 1, imgsize, fp); } - printf("low pass: %s will be deleted when quitting ptv\n", lp_name); - fclose (fp); + if (examine == 3) { + fp = fopen (lp_name, "w"); /* save lowpass image */ + if (tiff_flag) { write_tiff (lp_name, img_lp, imx, imy); } + else { fwrite (img_lp, 1, imgsize, fp); } + printf("low pass: %s will be deleted when quitting ptv\n", lp_name); + fclose (fp); } - /* subtract lowpass from original (=> highpass) */ - for (ptr1=img, ptr2=img_lp, ptr3=img_hp, i=0; i highpass) */ + for (ptr1=img, ptr2=img_lp, ptr3=img_hp, i=0; i *ptr2) *ptr3 = *ptr1 - *ptr2; - else *ptr3 = 0; + if (*ptr1 > *ptr2) *ptr3 = *ptr1 - *ptr2; + else *ptr3 = 0; } + /* consider field mode */ + if (field == 1 || field == 2) split (img_hp, field); - /* consider field mode */ - if (field == 1 || field == 2) split (img_hp, field); - - /* filter highpass image, if wanted */ - switch (filter_hp) + /* filter highpass image, if wanted */ + switch (filter_hp) { - case 0: break; - case 1: lowpass_3 (img_hp, img_hp); break; - case 2: filter_3 (img_hp, img_hp); break; + case 0: break; + case 1: lowpass_3 (img_hp, img_hp); break; + case 2: filter_3 (img_hp, img_hp); break; } - /* save highpass image for later use */ - if (examine == 3) - { fp = fopen (hp_name, "w"); - if (tiff_flag) { write_tiff (hp_name, img_hp, imx, imy); } - else { fwrite (img_hp, 1, imgsize, fp);} - fclose (fp); - } - - free (img_lp); + /* save highpass image for later use */ + if (examine == 3) { + fp = fopen (hp_name, "w"); + if (tiff_flag) { write_tiff (hp_name, img_hp, imx, imy); } + else { fwrite (img_hp, 1, imgsize, fp); } + fclose (fp); + } + free (img_lp); } -void targ_rec (interp, img0, img, par_file, xmin,xmax,ymin,ymax, pix, nr, num) - -Tcl_Interp* interp; -unsigned char *img, *img0; /* image data, image to be set to zero */ -char par_file[]; /* name of parameter file */ -int xmin,xmax,ymin,ymax; /* search area */ -target pix[]; /* pixel coord array, global */ -int nr; /* image number for display */ -int *num; /* number of detections */ - -/* thresholding and center of gravity with a peak fitting technique */ -/* uses 4 neighbours for connectivity and 8 to find local maxima */ - +void targ_rec (Tcl_Interp* interp, unsigned char *img0, unsigned char *img, char par_file[], + int xmin, int xmax, int ymin, int ymax, + target pix[], int nr, int *num) +/* + unsigned char *img, *img0 image data, image to be set to zero + char par_file[] name of parameter file + int xmin,xmax,ymin,ymax search area + target pix[] pixel coord array, global + int nr image number for display + int *num number of detections + + thresholding and center of gravity with a peak fitting technique + uses 4 neighbours for connectivity and 8 to find local maxima +*/ { - register int i, j, m; - int n=0, n_wait=0, n_targets=0, sumg, sumg_min; - int numpix; - int thres, gvthres[4], disco; - int cr_sz; /* size of crosses to be drawn */ - int nnmin,nnmax, nx,ny, nxmin,nxmax, nymin,nymax; - int xa,ya,xb,yb, x4[4],y4[4], xn,yn; - double x, y; - FILE *fpp; - - register unsigned char gv, gvref; - - targpix waitlist[2048]; - - /* read image name, filter dimension and threshold from parameter file */ - fpp = fopen_r (par_file); - fscanf (fpp, "%d", &gvthres[0]); /* threshold for binarization 1.image */ - fscanf (fpp, "%d", &gvthres[1]); /* threshold for binarization 2.image */ - fscanf (fpp, "%d", &gvthres[2]); /* threshold for binarization 3.image */ - fscanf (fpp, "%d", &gvthres[3]); /* threshold for binarization 4.image */ - fscanf (fpp, "%d", &disco); /* threshold value for discontinuity */ - fscanf (fpp, "%d %d", &nnmin, &nnmax); /* min. and max. number of */ - fscanf (fpp, "%d %d", &nxmin, &nxmax); /* pixels per target, */ - fscanf (fpp, "%d %d", &nymin, &nymax); /* abs, in x, in y */ - fscanf (fpp, "%d", &sumg_min); /* min. sumg */ - fscanf (fpp, "%d", &cr_sz); /* size of crosses */ - fclose (fpp); - - /* give thres value refering to image number */ - thres=gvthres[nr]; - - /* thresholding and connectivity analysis in image */ - - for (i=ymin; i thres) - if ( gv >= *(img + i*imx + j-1) - && gv >= *(img + i*imx + j+1) - && gv >= *(img + (i-1)*imx + j) - && gv >= *(img + (i+1)*imx + j) - && gv >= *(img + (i-1)*imx + j-1) - && gv >= *(img + (i+1)*imx + j-1) - && gv >= *(img + (i-1)*imx + j+1) - && gv >= *(img + (i+1)*imx + j+1) ) - - /* => local maximum, 'peak' */ - { - yn=i; xn=j; - xn = xn; - sumg = gv; *(img + i*imx + j) = 0; - xa = xn; xb = xn; ya = yn; yb = yn; - gv -= thres; - x = (xn) * gv; - y = yn * gv; - numpix = 1; - waitlist[0].x = j; waitlist[0].y = i; n_wait = 1; - - while (n_wait > 0) - { - gvref = *(img0 + imx*(waitlist[0].y) + (waitlist[0].x)); - - x4[0] = waitlist[0].x - 1; y4[0] = waitlist[0].y; - x4[1] = waitlist[0].x + 1; y4[1] = waitlist[0].y; - x4[2] = waitlist[0].x; y4[2] = waitlist[0].y - 1; - x4[3] = waitlist[0].x; y4[3] = waitlist[0].y + 1; - - for (n=0; n<4; n++) - { - xn = x4[n]; yn = y4[n]; - xn = xn; - gv = *(img + imx*yn + xn); - - /* conditions for threshold, discontinuity, image borders */ - /* and peak fitting */ - if ( (gv > thres) - && (xn>=xmin)&&(xn=ymin)&&(yn= *(img0 + imx*(yn-1) + xn)) - && (gvref + disco >= *(img0 + imx*(yn+1) + xn)) - && (gvref + disco >= *(img0 + imx*yn + (xn-1))) - && (gvref + disco >= *(img0 + imx*yn + (xn+1))) ) - { - sumg += gv; *(img + imx*yn + xn) = 0; - if (xn < xa) xa = xn; if (xn > xb) xb = xn; - if (yn < ya) ya = yn; if (yn > yb) yb = yn; - waitlist[n_wait].x = xn; waitlist[n_wait].y = yn; - x = x + (xn) * (gv - thres); - y = y + yn * (gv - thres); - numpix++; n_wait++; - } - } - - n_wait--; - for (m=0; m= nnmin) && (numpix <= nnmax) - && (nx >= nxmin) && (nx <= nxmax) - && (ny >= nymin) && (ny <= nymax) - && (sumg > sumg_min) ) - { - pix[n_targets].n = numpix; - pix[n_targets].nx = nx; - pix[n_targets].ny = ny; - pix[n_targets].sumg = sumg; - sumg -= (numpix*thres); - x /= sumg; x += 0.5; y /= sumg; y += 0.5; - pix[n_targets].x = x; - pix[n_targets].y = y; - pix[n_targets].pnr = n_targets++; - - xn = x; yn = y; - drawcross (interp, (int) xn, (int) yn, cr_sz, nr, "Blue"); - - } - } /* end of if-loop */ + gv = *(img + i*imx + j); + if ( gv > thres + && gv >= *(img + i*imx + j-1) + && gv >= *(img + i*imx + j+1) + && gv >= *(img + (i-1)*imx + j) + && gv >= *(img + (i+1)*imx + j) + && gv >= *(img + (i-1)*imx + j-1) + && gv >= *(img + (i+1)*imx + j-1) + && gv >= *(img + (i-1)*imx + j+1) + && gv >= *(img + (i+1)*imx + j+1) ) + /* => local maximum, 'peak' */ + { + yn = i; + xn = j; + sumg = gv; + *(img + i*imx + j) = 0; + xa = xn; xb = xn; + ya = yn; yb = yn; + gv -= thres; + x = xn * gv; + y = yn * gv; + numpix = 1; + waitlist[0].x = j; waitlist[0].y = i; n_wait = 1; + + while (n_wait > 0) + { + gvref = *(img0 + imx*(waitlist[0].y) + (waitlist[0].x)); + + x4[0] = waitlist[0].x - 1; y4[0] = waitlist[0].y; + x4[1] = waitlist[0].x + 1; y4[1] = waitlist[0].y; + x4[2] = waitlist[0].x; y4[2] = waitlist[0].y - 1; + x4[3] = waitlist[0].x; y4[3] = waitlist[0].y + 1; + + for (n=0; n<4; n++) + { + xn = x4[n]; yn = y4[n]; + gv = *(img + imx*yn + xn); + + /* conditions for threshold, discontinuity, image borders */ + /* and peak fitting */ + if ( gv > thres + && xn>=xmin && xn=ymin && yn= *(img0 + imx*(yn-1) + xn) + && gvref + disco >= *(img0 + imx*(yn+1) + xn) + && gvref + disco >= *(img0 + imx*yn + xn-1) + && gvref + disco >= *(img0 + imx*yn + xn+1) ) + { + sumg += gv; *(img + imx*yn + xn) = 0; + if (xn < xa) xa = xn; if (xn > xb) xb = xn; + if (yn < ya) ya = yn; if (yn > yb) yb = yn; + waitlist[n_wait].x = xn; waitlist[n_wait].y = yn; + x = x + xn * (gv - thres); + y = y + yn * (gv - thres); + numpix++; n_wait++; + } + } + + n_wait--; + for (m=0; m= nnmin && numpix <= nnmax + && nx >= nxmin && nx <= nxmax + && ny >= nymin && ny <= nymax + && sumg > sumg_min ) + { + pix[n_targets].n = numpix; + pix[n_targets].nx = nx; + pix[n_targets].ny = ny; + pix[n_targets].sumg = sumg; + sumg -= (numpix*thres); + x /= sumg; x += 0.5; + y /= sumg; y += 0.5; + pix[n_targets].x = x; + pix[n_targets].y = y; + pix[n_targets].pnr = n_targets++; + + xn = (int)x; yn = (int)y; + drawcross (interp, xn, yn, cr_sz, nr, "Blue"); + } + } } - *num = n_targets; + *num = n_targets; } - - -void simple_connectivity (interp, img0, img, par_file, xmin,xmax,ymin,ymax, pix, nr, num) -Tcl_Interp* interp; -unsigned char *img, *img0; /* image data, image to be set to zero */ -char par_file[]; /* name of parameter file */ -int xmin,xmax,ymin,ymax; /* search area */ -target pix[]; /* pixel coord array, global */ -int nr; /* image number for display */ -int *num; /* number of detections */ - -/* thresholding and center of gravity with a peak fitting technique */ -/* uses 4 neighbours for connectivity and 8 to find local maxima */ - +void simple_connectivity (Tcl_Interp* interp, unsigned char *img0, unsigned char *img, + char par_file[], int xmin, int xmax, int ymin, int ymax, + target pix[], int nr, int *num) +/* + unsigned char *img, *img0 image data, image to be set to zero + char par_file[] name of parameter file + int xmin,xmax,ymin,ymax search area + target pix[] pixel coord array, global + int nr image number for display + int *num number of detections +*/ +/* thresholding and center of gravity with a peak fitting technique */ +/* uses 4 neighbours for connectivity and 8 to find local maxima */ { - register int i, j, m; - int n=0, n_wait=0, n_targets=0, sumg, sumg_min; - int numpix; - int thres; - int cr_sz; /* size of crosses to be drawn */ - int nnmin,nnmax, nx,ny, nxmin,nxmax, nymin,nymax; - int xa,ya,xb,yb, x4[4],y4[4], xn,yn; - double x, y; - FILE *fpp; - - register unsigned char gv, gvref; - - targpix waitlist[2048]; - - /* read image name, threshold and shape limits from parameter file */ - fpp = fopen_r (par_file); - fscanf (fpp, "%d", &thres); /* threshold for binarization */ - fscanf (fpp, "%d", &n); /* threshold value for discontinuity */ - fscanf (fpp, "%d %d", &nnmin, &nnmax); /* min. and max. number of */ - fscanf (fpp, "%d %d", &nxmin, &nxmax); /* pixels per target, */ - fscanf (fpp, "%d %d", &nymin, &nymax); /* abs, in x, in y */ - fscanf (fpp, "%d", &sumg_min); /* min. sumg */ - fscanf (fpp, "%d", &cr_sz); /* size of crosses */ - fclose (fpp); - - - /* thresholding and connectivity analysis in image */ - - for (i=ymin; i 2*thres) + register unsigned char gv, gvref; + register int i, j, m; + int n=0, n_wait=0, n_targets=0, sumg, sumg_min, numpix, thres, cr_sz; + int nnmin, nnmax, nx, ny, nxmin, nxmax, nymin, nymax; + int xa, ya, xb, yb, x4[4], y4[4], xn, yn; + double x, y; + FILE *fpp; + + targpix waitlist[2048]; + + /* read image name, threshold and shape limits from parameter file */ + fpp = fopen_rp (par_file); // replaced fopen_r(), ad holten 12-2012 + if (!fpp) return; + fscanf (fpp, "%d", &thres); /* threshold for binarization */ + fscanf (fpp, "%*d"); // added, ad holten 12-2012 + fscanf (fpp, "%*d"); // + fscanf (fpp, "%*d"); // + fscanf (fpp, "%d", &n); /* threshold value for discontinuity */ + fscanf (fpp, "%d%d", &nnmin, &nnmax); /* min. and max. number of */ + fscanf (fpp, "%d%d", &nxmin, &nxmax); /* pixels per target, */ + fscanf (fpp, "%d%d", &nymin, &nymax); /* abs, in x, in y */ + fscanf (fpp, "%d", &sumg_min); /* min. sumg */ + fscanf (fpp, "%d", &cr_sz); /* size of crosses */ + fclose (fpp); + + + /* thresholding and connectivity analysis in image */ + // for (i=ymin; i 0) - { - gvref = *(img0 + imx*(waitlist[0].y) + (waitlist[0].x)); - - x4[0] = waitlist[0].x - 1; y4[0] = waitlist[0].y; - x4[1] = waitlist[0].x + 1; y4[1] = waitlist[0].y; - x4[2] = waitlist[0].x; y4[2] = waitlist[0].y - 1; - x4[3] = waitlist[0].x; y4[3] = waitlist[0].y + 1; - - for (n=0; n<4; n++) + gv = *(img + i*imx + j); + if ( gv > 2*thres) { - xn = x4[n]; yn = y4[n]; - xn = xn; - gv = *(img + imx*yn + xn); - - /* conditions for threshold, discontinuity, image borders */ - /* and peak fitting */ - if ( (gv > thres) - && (xn>=xmin)&&(xn=ymin)&&(yn xb) xb = xn; - if (yn < ya) ya = yn; if (yn > yb) yb = yn; - waitlist[n_wait].x = xn; waitlist[n_wait].y = yn; - x = x + (xn ) * (gv - thres); - y = y + yn * (gv - thres); - numpix++; n_wait++; - } + yn = i; + xn = j; + sumg = gv; + *(img + i*imx + j) = 0; + xa = xn; xb = xn; + ya = yn; yb = yn; + gv -= thres; + x = xn * gv; + y = yn * gv; + numpix = 1; + waitlist[0].x = j; waitlist[0].y = i; n_wait = 1; + + while (n_wait > 0) + { + gvref = *(img0 + imx*(waitlist[0].y) + (waitlist[0].x)); + + x4[0] = waitlist[0].x - 1; y4[0] = waitlist[0].y; + x4[1] = waitlist[0].x + 1; y4[1] = waitlist[0].y; + x4[2] = waitlist[0].x; y4[2] = waitlist[0].y - 1; + x4[3] = waitlist[0].x; y4[3] = waitlist[0].y + 1; + + for (n=0; n<4; n++) + { + xn = x4[n]; yn = y4[n]; + gv = *(img + imx*yn + xn); + + /* conditions for threshold, discontinuity, image borders */ + /* and peak fitting */ + if (gv > thres + && xn>=xmin && xn=ymin && yn xb) xb = xn; + if (yn < ya) ya = yn; if (yn > yb) yb = yn; + waitlist[n_wait].x = xn; waitlist[n_wait].y = yn; + x = x + xn * (gv - thres); + y = y + yn * (gv - thres); + numpix++; n_wait++; + } + } + + n_wait--; + for (m=0; m= nnmin && numpix <= nnmax + && nx >= nxmin && nx <= nxmax + && ny >= nymin && ny <= nymax + && sumg > sumg_min ) + { + pix[n_targets].n = numpix; + pix[n_targets].nx = nx; + pix[n_targets].ny = ny; + pix[n_targets].sumg = sumg; + sumg -= (numpix*thres); + x /= sumg; x += 0.5; + y /= sumg; y += 0.5; + pix[n_targets].x = x; + pix[n_targets].y = y; + pix[n_targets].tnr = -1; + pix[n_targets].pnr = n_targets++; + + xn = (int)x; yn = (int)y; + // drawcross (xn, yn, cr_sz, 8); // bug, ad holten, 12-2012 + drawcross (interp, xn, yn, cr_sz, nr, "Blue"); + } } - - n_wait--; - for (m=0; m= nnmin) && (numpix <= nnmax) - && (nx >= nxmin) && (nx <= nxmax) - && (ny >= nymin) && (ny <= nymax) - && (sumg > sumg_min) ) - { - pix[n_targets].n = numpix; - pix[n_targets].nx = nx; - pix[n_targets].ny = ny; - pix[n_targets].sumg = sumg; - sumg -= (numpix*thres); - x /= sumg; x += 0.5; y /= sumg; y += 0.5; - pix[n_targets].x = x; - pix[n_targets].y = y; - pix[n_targets].tnr = -1; - pix[n_targets].pnr = n_targets++; - - xn = x; yn = y; - drawcross (xn, yn, cr_sz, 8); - } - } /* end of if-loop */ - } - - *num = n_targets; + } + *num = n_targets; } diff --git a/PTV/src_c/sortgrid.c b/PTV/src_c/sortgrid.c index 342567a..ac17cff 100644 --- a/PTV/src_c/sortgrid.c +++ b/PTV/src_c/sortgrid.c @@ -1,21 +1,21 @@ /**************************************************************************** -Routine: sortgrid.c +Routine: sortgrid.c -Author/Copyright: Hans-Gerd Maas +Author/Copyright: Hans-Gerd Maas -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: 22.6.88 +Creation Date: 22.6.88 -Description: reads objects, detected by detection etc., - sorts them with respect to the 13 wires of the grid, - detects missing points - and writes the result to a new file +Description: reads objects, detected by detection etc., + sorts them with respect to the 13 wires of the grid, + detects missing points + and writes the result to a new file - does not work in each imaginable case ! + does not work in each imaginable case ! ****************************************************************************/ /* @@ -24,191 +24,127 @@ Copyright (c) 1990-2011 ETH Zurich See the file license.txt for copying permission. */ - #include "ptv.h" -void just_plot (interp, Ex, I, G, ap, mm, imx,imy, pix_x,pix_y, - nfix,fix, field, n_img) +void just_plot (Tcl_Interp* interp, Exterior Ex, Interior I, Glass G, ap_52 ap, + mm_np mm, int imx, int imy, double pix_x, double pix_y, + int nfix, coord_3d fix[], int field, int n_img) +{ + int i, intx, inty; + double xp, yp, eps=10.0; -Tcl_Interp* interp; -Exterior Ex; -Interior I; -Glass G; -ap_52 ap; -mm_np mm; -int nfix, imx, imy, field, n_img; -double pix_x, pix_y; -coord_3d fix[]; + /* reproject all calibration plate points into pixel space + and search a detected target nearby */ + for (i=0; i -eps && yp > -eps && xp < imx+eps && yp < imy+eps) - { - j = nearest_neighbour_pix (old, num, xp, yp, eps); + for (i=0; i -eps && yp > -eps && xp < imx+eps && yp < imy+eps) { + j = nearest_neighbour_pix (old, num, xp, yp, eps); + if (j != -999) { + pix[i] = old[j]; + pix[i].pnr = fix[i].pnr; + } + } } - } } -void sortgrid_file (interp, Ex, I, G, ap, mm, imx,imy, pix_x,pix_y, - nfix,fix, num,pix, field, n_img) - - -///Beat and Debashish January 2011 -Tcl_Interp* interp; -Exterior Ex; -Interior I; -Glass G; -ap_52 ap; -mm_np mm; -int nfix, num, imx, imy, field, n_img; -double pix_x, pix_y; -coord_3d fix[]; -target pix[]; +// Beat and Debashish January 2011 +void sortgrid_file (Tcl_Interp* interp, Exterior Ex, Interior I, Glass G, ap_52 ap, + mm_np mm, int imx, int imy, double pix_x, double pix_y, + int nfix, coord_3d fix[], int num, target pix[], int field, int n_img) +{ + int i, k; + double eps=10.0; + target old[1024]; + char file_sort[256]; + int dummy, detection_pnr[1000]; + + sprintf (file_sort, "for_sortgrid.%1d", n_img); + + fpp = fopen_rp (file_sort); // replaced fopen_r(), ad holten, 12-2012 + if (!fpp) return; + k = 0; + while (fscanf (fpp, "%d %lf %lf %lf %d", &fix[k].pnr, + &fix[k].x, &fix[k].y, &fix[k].z, &dummy) != EOF) { + detection_pnr[k]=dummy; + k++; + } + fclose (fpp); + for (i=0; i=0) - return((int)(x+.5)); - else - return((int)(x-.5)); + if (x>=0) + return((int)(x+.5)); + else + return((int)(x-.5)); } +#endif - -void write_ori (Ex, I, G, filename) /* write exterior and interior orientation */ -Exterior Ex; -Interior I; -Glass G; -char filename[64]; +void write_ori (Exterior Ex, Interior I, Glass G, char filename[64]) +/* write exterior and interior orientation */ { - FILE *fp; - int i; - - fp = fopen (filename, "w"); - fprintf (fp, "%11.4f %11.4f %11.4f\n %10.7f %10.7f %10.7f\n\n", - Ex.x0, Ex.y0, Ex.z0, Ex.omega, Ex.phi, Ex.kappa); - for (i=0; i<3; i++) fprintf (fp, " %10.7f %10.7f %10.7f\n", - Ex.dm[i][0], Ex.dm[i][1], Ex.dm[i][2]); - fprintf (fp,"\n %8.4f %8.4f\n %8.4f\n", I.xh, I.yh, I.cc); - fprintf (fp,"\n %20.15f %20.15f %20.15f\n", G.vec_x, G.vec_y, G.vec_z); - fclose (fp); + FILE *fp; + int i; + + fp = fopen (filename, "w"); + fprintf (fp, "%11.4f %11.4f %11.4f\n %10.7f %10.7f %10.7f\n\n", + Ex.x0, Ex.y0, Ex.z0, Ex.omega, Ex.phi, Ex.kappa); + for (i=0; i<3; i++) fprintf (fp, " %10.7f %10.7f %10.7f\n", + Ex.dm[i][0], Ex.dm[i][1], Ex.dm[i][2]); + fprintf (fp,"\n %8.4f %8.4f\n %8.4f\n", I.xh, I.yh, I.cc); + fprintf (fp,"\n %20.15f %20.15f %20.15f\n", G.vec_x, G.vec_y, G.vec_z); + fclose (fp); } -void read_ori (Ex, I, G, filename) /* read exterior and interior orientation */ -Exterior *Ex; -Interior *I; -Glass *G; -char filename[64]; +int read_ori (Exterior *Ex, Interior *I, Glass *G, char filename[64]) +/* read exterior and interior orientation */ { - FILE *fp; - int i; - - fp = fopen_r (filename); - fscanf (fp, "%lf %lf %lf %lf %lf %lf", - &(Ex->x0), &(Ex->y0), &(Ex->z0), - &(Ex->omega), &(Ex->phi), &(Ex->kappa)); - for (i=0; i<3; i++) fscanf (fp, " %lf %lf %lf", - &(Ex->dm[i][0]), &(Ex->dm[i][1]), &(Ex->dm[i][2])); - fscanf (fp, "%lf %lf %lf", &(I->xh), &(I->yh), &(I->cc)); - fscanf (fp, "%lf %lf %lf", &(G->vec_x), &(G->vec_y), &(G->vec_z)); - fclose (fp); + FILE *fp; + int i; + + fp = fopen_rp (filename); // replaced fopen_r(), ad holten, 12-2012 + if (!fp) return 0; + + fscanf (fp, "%lf %lf %lf %lf %lf %lf", + &(Ex->x0), &(Ex->y0), &(Ex->z0), &(Ex->omega), &(Ex->phi), &(Ex->kappa)); + for (i=0; i<3; i++) + fscanf (fp, " %lf %lf %lf", &(Ex->dm[i][0]), &(Ex->dm[i][1]), &(Ex->dm[i][2])); + fscanf (fp, "%lf %lf %lf", &(I->xh), &(I->yh), &(I->cc)); + fscanf (fp, "%lf %lf %lf", &(G->vec_x), &(G->vec_y), &(G->vec_z)); + fclose (fp); + return 1; } +FILE *fopen_rp (char *filename) +/* tries to open 'filename' for reading; + prints a message, if the file can't be opened. */ +{ + FILE *fp = fopen(filename, "r"); + if (! fp) + printf ("Could not open this file:\n %s\n", filename); + + return (fp); +} -FILE *fopen_r (filename) -char filename[256]; +FILE *fopen_r (CHAR filename[256]) /* tries to open a file; gives a message, if it cannot open it - and waits until it has been created */ + and waits until it has been created */ { - FILE *fpr; - int count; - - fpr = fopen (filename, "r"); - if ( ! fpr) - { - printf ("could not open %s, please create this file\n", filename); + FILE *fpr; + int count; - /* wait until file can be opened */ - while ( ! fpr) fpr = fopen (filename, "r"); + fpr = fopen (filename, "r"); + if (! fpr) { + printf ("could not open %s, please create this file\n", filename); - /* wait until file really created */ - for (count=0; count<100000; count++); - } + /* wait until file can be opened */ + while ( ! fpr) fpr = fopen (filename, "r"); - return (fpr); + /* wait until file really created */ + for (count=0; count<100000; count++); + } + return (fpr); } -void read_image (Tcl_Interp* interp, char path[128], unsigned char *img) +int read_image (Tcl_Interp* interp, char path[128], unsigned char *img) { - int i, j; - Tk_PhotoHandle img_handle; - Tk_PhotoImageBlock img_block; - - if (tiff_flag) { - sprintf(val, "temp read %s", path); - Tcl_Eval(interp, val); - - img_handle = Tk_FindPhoto( interp, "temp"); - Tk_PhotoGetImage (img_handle, &img_block); - for ( j=0;j 1) - { - sprintf (buf, "cannot delete point from zoomed image"); - Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); - Tcl_Eval(interp, ".text delete 3"); - Tcl_Eval(interp, ".text insert 3 $tbuf"); - return (0); - } - - for (i=0; i 1) { - dmin = d; imin = i; + sprintf (buf, "cannot delete point from zoomed image"); + Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); + Tcl_Eval(interp, ".text delete 3"); + Tcl_Eval(interp, ".text insert 3 $tbuf"); + return (0); } - } - if (dmin > 10) return (-1); /* limit: 10 pixel */ - intx = (int) pix[nr][imin].x; - inty = (int) pix[nr][imin].y; - drawcross (interp, intx, inty, cr_sz+1, nr, "magenta"); + for (i=0; i 10) return (-1); /* limit: 10 pixel */ + intx = (int) pix[nr][imin].x; + inty = (int) pix[nr][imin].y; - for (i=imin; i1; dj/=2) - { - if (crd[j0].x < xmin) j0 += dj; - else j0 -= dj; - } - j0 -= 12; if (j0 < 0) j0 = 0; /* due to trunc */ + xmin = x - eps; xmax = x + eps; ymin = y - eps; ymax = y + eps; - for (j=j0; j xmax) break; /* finish search */ + /* binarized search for start point of candidate search */ + for (j0=num/2, dj=num/4; dj>1; dj/=2) + { + if (crd[j0].x < xmin) j0 += dj; + else j0 -= dj; + } + j0 -= 12; if (j0 < 0) j0 = 0; /* due to trunc */ - if (crd[j].y > ymin && crd[j].y < ymax) + for (j=j0; j xmax) break; /* finish search */ + + if (crd[j].y > ymin && crd[j].y < ymax) + { + d = sqrt ((x-crd[j].x)*(x-crd[j].x) + (y-crd[j].y)*(y-crd[j].y)); + if (d < dmin) + { + dmin = d; pnr = j; + } + } } - } - return (pnr); + return (pnr); } -int nearest_neighbour_pix (pix, num, x, y, eps) -target pix[]; -int num; -double x, y, eps; +int nearest_neighbour_pix (target pix[], int num, double x, double y, double eps) { - register int j; - int pnr = -999; - double d, dmin=1e20, xmin, xmax, ymin, ymax; + register int j; + int pnr = -999; + double d, dmin=1e20, xmin, xmax, ymin, ymax; - xmin = x - eps; xmax = x + eps; ymin = y - eps; ymax = y + eps; + xmin = x - eps; xmax = x + eps; ymin = y - eps; ymax = y + eps; - for (j=0; jymin && pix[j].yxmin && pix[j].xymin && pix[j].yxmin && pix[j].x=i; --j) @@ -334,14 +316,10 @@ int count; } } - - -void bubble_conlist (item, count) -correspond *item; -int count; +void bubble_conlist (correspond *item, int count) { - int i,j; - correspond temp; + int i,j; + correspond temp; for (i=1; i=i; --j) { @@ -352,54 +330,47 @@ int count; } } -void bubble_foundpix1 ( foundpix *item) +void bubble_foundpix1 (foundpix *item) { - int i,j; - foundpix temp; + int i,j; + foundpix temp; - for (i=1; i=i; --j) - { - if (item[j-1].freq < item[j].freq) + for (i=1; i=i; --j) { - temp = *(item+j-1); *(item+j-1) = *(item+j); *(item+j) = temp; + if (item[j-1].freq < item[j].freq) + { + temp = *(item+j-1); + *(item+j-1) = *(item+j); + *(item+j) = temp; + } } - } } /***********************************************************************/ -/***********************************************************************/ - - /* quicksort algorithms for several issues */ - /***********************************************************************/ /* quicksort of 2d coordinates in x-order */ -void quicksort_coord2d_x (crd, num) -coord_2d *crd; -int num; +void quicksort_coord2d_x (coord_2d *crd, int num) { qs_coord2d_x (crd, 0, num-1); } - -void qs_coord2d_x (crd, left, right) -coord_2d *crd; -int left, right; +void qs_coord2d_x (coord_2d *crd, int left, int right) { register int i, j; - double xm; + double xm; coord_2d temp; - i = left; j = right; xm = crd[(left+right)/2].x; + i = left; j = right; xm = crd[(left+right)/2].x; do { - while (crd[i].x < xm && ileft) j--; + while (crd[i].x < xm && ileft) j--; if (i <= j) { @@ -411,8 +382,8 @@ int left, right; } while (i <= j); - if (left < j) qs_coord2d_x (crd, left, j); - if (i < right) qs_coord2d_x (crd, i, right); + if (left < j) qs_coord2d_x (crd, left, j); + if (i < right) qs_coord2d_x (crd, i, right); } @@ -420,156 +391,124 @@ int left, right; /* quicksort of 2d coordinates in pnr-order */ -void quicksort_coord2d_pnr (crd, num) -coord_2d *crd; -int num; +void quicksort_coord2d_pnr (coord_2d *crd, int num) { - qs_coord2d_pnr (crd, 0, num-1); + qs_coord2d_pnr (crd, 0, num-1); } - - -void qs_coord2d_pnr (crd, left, right) -coord_2d *crd; -int left, right; +void qs_coord2d_pnr (coord_2d *crd, int left, int right) { - register int i, j; - double pnrm; - coord_2d temp; + register int i, j; + double pnrm; + coord_2d temp; - i = left; j = right; pnrm = crd[(left+right)/2].pnr; + i = left; j = right; pnrm = crd[(left+right)/2].pnr; - do - { - while (crd[i].pnr < pnrm && ileft) j--; + do { + while (crd[i].pnr < pnrm && ileft) j--; - if (i <= j) - { - temp = crd[i]; - crd[i] = crd[j]; - crd[j] = temp; - i++; j--; - } - } - while (i <= j); + if (i <= j) + { + temp = crd[i]; + crd[i] = crd[j]; + crd[j] = temp; + i++; j--; + } + } while (i <= j); - if (left < j) qs_coord2d_pnr (crd, left, j); - if (i < right) qs_coord2d_pnr (crd, i, right); + if (left < j) qs_coord2d_pnr (crd, left, j); + if (i < right) qs_coord2d_pnr (crd, i, right); } - - - /***********************************************************************/ - /* quicksort of targets in y-order */ -void quicksort_target_y (pix, num) -target *pix; -int num; +void quicksort_target_y (target *pix, int num) { - qs_target_y (pix, 0, num-1); + qs_target_y (pix, 0, num-1); } - - - -void qs_target_y (pix, left, right) -target *pix; -int left, right; +void qs_target_y (target *pix, int left, int right) { - register int i, j; - double ym; - target temp; + register int i, j; + double ym; + target temp; - i = left; j = right; ym = pix[(left+right)/2].y; + i = left; j = right; ym = pix[(left+right)/2].y; - do - { - while (pix[i].y < ym && ileft) j--; + do { + while (pix[i].y < ym && ileft) j--; - if (i <= j) - { - temp = pix[i]; - pix[i] = pix[j]; - pix[j] = temp; - i++; j--; - } - } - while (i <= j); + if (i <= j) + { + temp = pix[i]; + pix[i] = pix[j]; + pix[j] = temp; + i++; j--; + } + } while (i <= j); - if (left < j) qs_target_y (pix, left, j); - if (i < right) qs_target_y (pix, i, right); + if (left < j) qs_target_y (pix, left, j); + if (i < right) qs_target_y (pix, i, right); } - /***********************************************************************/ /* quicksort for list of correspondences in order of match quality */ /* 4 camera version */ -void quicksort_con (con, num) -n_tupel *con; -int num; +void quicksort_con (n_tupel *con, int num) { - qs_con (con, 0, num-1); + qs_con (con, 0, num-1); } -void qs_con (con, left, right) -n_tupel *con; -int left, right; +void qs_con (n_tupel *con, int left, int right) { - register int i, j; - double xm; - n_tupel temp; + register int i, j; + double xm; + n_tupel temp; - i = left; j = right; xm = con[(left+right)/2].corr; + i = left; j = right; xm = con[(left+right)/2].corr; - do - { - while (con[i].corr > xm && i con[j].corr && j>left) j--; + do { + while (con[i].corr > xm && i con[j].corr && j>left) j--; - if (i <= j) - { - temp = con[i]; - con[i] = con[j]; - con[j] = temp; - i++; j--; - } - } - while (i <= j); + if (i <= j) + { + temp = con[i]; + con[i] = con[j]; + con[j] = temp; + i++; j--; + } + } while (i <= j); - if (left < j) qs_con (con, left, j); - if (i < right) qs_con (con, i, right); + if (left < j) qs_con (con, left, j); + if (i < right) qs_con (con, i, right); } /***********************************************************************/ - -void tclimg2cimg (interp, c_img, tcl_img) -Tcl_Interp* interp; -unsigned char *c_img; -Tk_PhotoImageBlock *tcl_img; +void tclimg2cimg (Tcl_Interp* interp, unsigned char *c_img, Tk_PhotoImageBlock *tcl_img) { - int i, j; - i=0; - for ( j=0;jpixelPtr +i) = *(c_img +j); - *(tcl_img->pixelPtr +i+1) = *(c_img +j); - *(tcl_img->pixelPtr +i+2) = *(c_img +j); - /* value for alpha canal 42, answer to all questions*/ - *(tcl_img->pixelPtr +i+3) = 42; - } + int i, j; + i=0; + for ( j=0;jpixelPtr +i) = *(c_img +j); + *(tcl_img->pixelPtr +i+1) = *(c_img +j); + *(tcl_img->pixelPtr +i+2) = *(c_img +j); + /* value for alpha canal 42, answer to all questions*/ + *(tcl_img->pixelPtr +i+3) = 42; + } } diff --git a/PTV/src_c/track.c b/PTV/src_c/track.c index 17fea61..a7b6a3e 100644 --- a/PTV/src_c/track.c +++ b/PTV/src_c/track.c @@ -1,19 +1,19 @@ /******************************************************************* -Routine: track.c +Routine: track.c -Author/Copyright: Jochen Willneff +Author/Copyright: Jochen Willneff -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich Creation Date: Beginning: February '98 - End: far away + End: far away -Description: Tracking of particles in image- and objectspace +Description: Tracking of particles in image- and objectspace -Routines contained: trackcorr_c +Routines contained: trackcorr_c *******************************************************************/ #include "ptv.h" @@ -23,1051 +23,1089 @@ void write_addedback(); int trackcorr_c (ClientData clientData, Tcl_Interp* interp, int argc, const char** argv) { - char val[256], buf[256]; - int i, j, h, k, mm, kk, step, okay=0, invol=0; - int zaehler1, zaehler2,philf[4][4]; - int count1=0, count2=0, count3=0, lost =0, zusatz=0; - int intx0, intx1, inty0, inty1; - int intx2, inty2,intx3, inty3; - int quali=0; - double x1[4], y1[4], x2[4], y2[4], angle, acc, angle0, acc0, lmax, dl; - double xr[4], xl[4], yd[4], yu[4], angle1, acc1; - double X1, Y1, Z1, X0, Y0, Z0, X2, Y2, Z2; - double X3, Y3, Z3, X4, Y4, Z4, X5, Y5, Z5, X6, Y6, Z6; - double xp[4], yp[4], xc[4], yc[4], xn[4], yn[4]; - double rr, Ymin=0, Ymax=0; - double npart=0, nlinks=0; - - foundpix *w, *wn, p16[16]; - - display = atoi(argv[1]); - - Tcl_Eval(interp, ".text delete 2"); - Tcl_Eval(interp, ".text insert 2 \"Track established correspondences\""); - - /* read data */ - readseqtrackcrit (); - - /*Alloc space, if checkflag for mega, c4, t4 is zero */ - if (!trackallocflag) - { - for (i=0; i<4; i++) + char val[256], buf[256]; + int i, j, h, k, mm, kk, step, okay=0, invol=0; + int zaehler1, zaehler2,philf[4][4]; + int count1=0, count2=0, count3=0, lost =0, zusatz=0; + int intx0, intx1, inty0, inty1; + int intx2, inty2; + int quali=0; + double x1[4], y1[4], x2[4], y2[4], angle, acc, angle0, acc0, lmax, dl; + double xr[4], xl[4], yd[4], yu[4], angle1, acc1; + double X1, Y1, Z1, X0, Y0, Z0, X2, Y2, Z2; + double X3, Y3, Z3, X4, Y4, Z4, X5, Y5, Z5, X6, Y6, Z6; + double xp[4], yp[4], xc[4], yc[4], xn[4], yn[4]; + double rr, Ymin=0, Ymax=0; + double npart=0, nlinks=0; + + foundpix *w, *wn, p16[16]; + + display = atoi(argv[1]); + + Tcl_Eval(interp, ".text delete 2"); + Tcl_Eval(interp, ".text insert 2 \"Track established correspondences\""); + + /* read data */ + readseqtrackcrit (); + + /*Alloc space, if checkflag for mega, c4, t4 is zero */ + if (!trackallocflag) { - mega[i]=(P *) calloc(sizeof(P),M); - c4[i]=(corres *) calloc(sizeof(corres),M); - for (k=0; k<4; k++) { t4[i][k]=(target *) calloc(sizeof (target),M);} + for (i=0; i<4; i++) + { + mega[i]=(P *) calloc(sizeof(P),M); + c4[i]=(corres *) calloc(sizeof(corres),M); + for (k=0; k<4; k++) { + t4[i][k]=(target *) calloc(sizeof (target),M); + } + } + trackallocflag=1; } - trackallocflag=1; - } - - /*load again first data sets*/ - step = seq_first; - read_ascii_data(step); - rotate_dataset(); - read_ascii_data(step+1); - rotate_dataset(); - read_ascii_data(step+2); - lmax=sqrt((tpar.dvxmin-tpar.dvxmax)*(tpar.dvxmin-tpar.dvxmax) - +(tpar.dvymin-tpar.dvymax)*(tpar.dvymin-tpar.dvymax) - +(tpar.dvzmin-tpar.dvzmax)*(tpar.dvzmin-tpar.dvzmax)); + /*load again first data sets*/ + step = seq_first; + read_ascii_data(step); + rotate_dataset(); + read_ascii_data(step+1); + rotate_dataset(); + read_ascii_data(step+2); - volumedimension (&X_lay[1], &X_lay[0], &Ymax, &Ymin, &Zmax_lay[1], &Zmin_lay[0]); + lmax = sqrt((tpar.dvxmin-tpar.dvxmax)*(tpar.dvxmin-tpar.dvxmax) + +(tpar.dvymin-tpar.dvymax)*(tpar.dvymin-tpar.dvymax) + +(tpar.dvzmin-tpar.dvzmax)*(tpar.dvzmin-tpar.dvzmax)); + volumedimension (&X_lay[1], &X_lay[0], &Ymax, &Ymin, &Zmax_lay[1], &Zmin_lay[0]); - /* sequence loop */ - for (step = seq_first; step < seq_last; step=step++) - { - sprintf (buf, "Time step: %d, seqnr: %d, Particle info:", step- seq_first, step); - Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); - Tcl_Eval(interp, ".text delete 2"); - Tcl_Eval(interp, ".text insert 2 $tbuf"); + /* sequence loop */ + for (step = seq_first; step < seq_last; step=step++) + { + sprintf (buf, "Time step: %d, seqnr: %d, Particle info:", step- seq_first, step); + Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); + Tcl_Eval(interp, ".text delete 2"); + Tcl_Eval(interp, ".text insert 2 $tbuf"); - count1=0; lost =0; zusatz=0; + count1=0; lost =0; zusatz=0; - /* try to track correspondences from previous 0 - corp, variable h */ - for (h=0; h=0) { - X0=mega[0][mega[1][h].prev].x[0]; - Y0=mega[0][mega[1][h].prev].x[1]; - Z0=mega[0][mega[1][h].prev].x[2]; - X2=2*X1-X0; - Y2=2*Y1-Y0; - Z2=2*Z1-Z0; - - for (j=0; j 0) count2++; - for (i=0; i=0) { - X5=0.5*(5.0*X3-4.0*X1+X0); - Y5=0.5*(5.0*Y3-4.0*Y1+Y0); - Z5=0.5*(5.0*Z3-4.0*Z1+Z0); - } else { - X5=2*X3-X1; - Y5=2*Y3-Y1; - Z5=2*Z3-Z1; } - - searchquader(X5, Y5, Z5, &xr, &xl, &yd, &yu); - - for (j=0;j=0) { + X0=mega[0][mega[1][h].prev].x[0]; + Y0=mega[0][mega[1][h].prev].x[1]; + Z0=mega[0][mega[1][h].prev].x[2]; + X2=2*X1-X0; + Y2=2*Y1-Y0; + Z2=2*Z1-Z0; + + for (j=0; j 0) count2++; + for (i=0; i 0) count3++; + /* found 3D-position */ + X3=mega[2][w[mm].ftnr].x[0]; + Y3=mega[2][w[mm].ftnr].x[1]; + Z3=mega[2][w[mm].ftnr].x[2]; - for (i=0; i=0) { + X5=0.5*(5.0*X3-4.0*X1+X0); + Y5=0.5*(5.0*Y3-4.0*Y1+Y0); + Z5=0.5*(5.0*Z3-4.0*Z1+Z0); + } + else { + X5=2*X3-X1; + Y5=2*Y3-Y1; + Z5=2*Z3-Z1; + } - /*end of candidate struct */ - /* ************************************************ */ - for (kk=0; kk < zaehler2; kk++) { /* zaehler2-loop */ - - X4=mega[3][wn[kk].ftnr].x[0]; - Y4=mega[3][wn[kk].ftnr].x[1]; - Z4=mega[3][wn[kk].ftnr].x[2]; - - okay=0; rr=1000000; quali=0; dl=0; - acc=2*tpar.dacc;angle=2*tpar.dangle; - acc0=2*tpar.dacc;angle0=2*tpar.dangle; - acc1=2*tpar.dacc;angle1=2*tpar.dangle; - - /* displacement check */ - if ( tpar.dvxmin < (X4-X3) && (X4-X3) < tpar.dvxmax && - tpar.dvymin < (Y4-Y3) && (Y4-Y3) < tpar.dvymax && - tpar.dvzmin < (Z4-Z3) && (Z4-Z3) < tpar.dvzmax ) { okay=1; - /* end displacement check */ - - if ( okay ==1 ) { - dl=(sqrt((X1-X3)*(X1-X3)+(Y1-Y3)*(Y1-Y3)+(Z1-Z3)*(Z1-Z3)) - +sqrt((X4-X3)*(X4-X3)+(Y4-Y3)*(Y4-Y3)+(Z4-Z3)*(Z4-Z3)))/2; - - angle_acc(X3, Y3, Z3, X4, Y4, Z4, X5, Y5, Z5, &angle1, &acc1); - - if (mega[1][h].prev>=0) { - angle_acc(X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, &angle0, &acc0); - } else { acc0=acc1; angle0=angle1;} - - acc=(acc0+acc1)/2; angle=(angle0+angle1)/2; - quali=wn[kk].freq+w[mm].freq; - rr=1000000; - - if ( (acc 0) count3++; - /* search for unused candidates in next time step */ - for (j=0;j0 ) { - x2[j]=t4[3][j][philf[j][0]].x; y2[j]= t4[3][j][philf[j][0]].y; - } - } - quali=0; + for (i=0; i=0) { + angle_acc(X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, &angle0, &acc0); + } + else { + acc0=acc1; + angle0=angle1; + } + + acc=(acc0+acc1)/2; angle=(angle0+angle1)/2; + quali=wn[kk].freq+w[mm].freq; + rr=1000000; + + if ( (acc= 2) { - - X4 = X5; Y4 =Y5; Z4 = Z5; - invol=0; okay=0; - - det_lsq_3d (Ex, I, G, ap, mmp, - x2[0], y2[0], x2[1], y2[1], x2[2], y2[2], x2[3], y2[3], &X4, &Y4, &Z4); - - /* volume check */ - if ( X_lay[0] < X4 && X4 < X_lay[1] && - Ymin < Y4 && Y4 < Ymax && - Zmin_lay[0] < Z4 && Z4 < Zmax_lay[1]) {invol=1;} - - /* displacement check */ - if ( invol==1 && - tpar.dvxmin < (X3-X4) && (X3-X4) < tpar.dvxmax && - tpar.dvymin < (Y3-Y4) && (Y3-Y4) < tpar.dvymax && - tpar.dvzmin < (Z3-Z4) && (Z3-Z4) < tpar.dvzmax ) { okay=1; - /* end displacement check */ - if (okay == 1) { - rr=1000000; dl=0; - acc=2*tpar.dacc;angle=2*tpar.dangle; - angle_acc(X3, Y3, Z3, X4, Y4, Z4, X5, Y5, Z5, &angle, &acc); - dl=(sqrt((X1-X3)*(X1-X3)+(Y1-Y3)*(Y1-Y3)+(Z1-Z3)*(Z1-Z3)) - +sqrt((X4-X3)*(X4-X3)+(Y4-Y3)*(Y4-Y3)+(Z4-Z3)*(Z4-Z3)))/2; - - if ( (acc0 ) { + x2[j]=t4[3][j][philf[j][0]].x; + y2[j]= t4[3][j][philf[j][0]].y; + } + } + quali=0; -} - } - } okay=0; } - invol=0; - } - - quali=0; - - /* end of creating new particle position */ - /* *************************************************************** */ - - - /* try to link if kk is not found/good enough and prev exist */ - if ( mega[1][h].inlist == 0 && mega[1][h].prev>=0 ) { - acc=2*tpar.dacc;angle=2*tpar.dangle; - if ( tpar.dvxmin < (X3-X1) && (X3-X1) < tpar.dvxmax && - tpar.dvymin < (Y3-Y1) && (Y3-Y1) < tpar.dvymax && - tpar.dvzmin < (Z3-Z1) && (Z3-Z1) < tpar.dvzmax ) { okay=1; - - if ( okay ==1 ) - { - rr=1000000; quali=0; - quali=w[mm].freq; - angle_acc(X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, &angle, &acc); - dl=(sqrt((X1-X0)*(X1-X0)+(Y1-Y0)*(Y1-Y0)+(Z1-Z0)*(Z1-Z0)) - +sqrt((X1-X3)*(X1-X3)+(Y1-Y3)*(Y1-Y3)+(Z1-Z3)*(Z1-Z3)))/2; - - if ( (acc= 2) { + + X4 = X5; Y4 =Y5; Z4 = Z5; + invol=0; okay=0; + + det_lsq_3d (Ex, I, G, ap, mmp, + x2[0], y2[0], x2[1], y2[1], x2[2], y2[2], x2[3], y2[3], &X4, &Y4, &Z4); + + /* volume check */ + if ( X_lay[0] < X4 && X4 < X_lay[1] && + Ymin < Y4 && Y4 < Ymax && + Zmin_lay[0] < Z4 && Z4 < Zmax_lay[1]) {invol=1;} + + /* displacement check */ + if ( invol==1 && + tpar.dvxmin < (X3-X4) && (X3-X4) < tpar.dvxmax && + tpar.dvymin < (Y3-Y4) && (Y3-Y4) < tpar.dvymax && + tpar.dvzmin < (Z3-Z4) && (Z3-Z4) < tpar.dvzmax ) + { + okay=1; + if (okay == 1) { + rr=1000000; dl=0; + acc=2*tpar.dacc;angle=2*tpar.dangle; + angle_acc(X3, Y3, Z3, X4, Y4, Z4, X5, Y5, Z5, &angle, &acc); + dl=(sqrt((X1-X3)*(X1-X3)+(Y1-Y3)*(Y1-Y3)+(Z1-Z3)*(Z1-Z3)) + +sqrt((X4-X3)*(X4-X3)+(Y4-Y3)*(Y4-Y3)+(Z4-Z3)*(Z4-Z3)))/2; + + if ( (acc=0 ) { + acc=2*tpar.dacc;angle=2*tpar.dangle; + if (tpar.dvxmin < (X3-X1) && (X3-X1) < tpar.dvxmax && + tpar.dvymin < (Y3-Y1) && (Y3-Y1) < tpar.dvymax && + tpar.dvzmin < (Z3-Z1) && (Z3-Z1) < tpar.dvzmax ) + { + okay=1; + + if ( okay ==1 ) + { + rr=1000000; quali=0; + quali=w[mm].freq; + angle_acc(X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, &angle, &acc); + dl = (sqrt((X1-X0)*(X1-X0)+(Y1-Y0)*(Y1-Y0)+(Z1-Z0)*(Z1-Z0)) + +sqrt((X1-X3)*(X1-X3)+(Y1-Y3)*(Y1-Y3)+(Z1-Z3)*(Z1-Z3)))/2; + + if ( (acc=0 ) + { + /* + printf("h: %d\n", h); + printf("X0: %6.3f %6.3f %6.3f, prev: %d\n", X0, Y0, Z0, mega[1][h].prev); + printf("X1: %6.3f %6.3f %6.3f, h: %d\n", X1, Y1, Z1, h); + printf("X2: %6.3f %6.3f %6.3f\n", X2, Y2, Z2); + printf("X3: %6.3f %6.3f %6.3f, ftnr: %d\n", X3, Y3, Z3, w[mm].ftnr); + */ + for (j=0;j=0 ) - { - /* - printf("h: %d\n", h); - printf("X0: %6.3f %6.3f %6.3f, prev: %d\n", X0, Y0, Z0, mega[1][h].prev); - printf("X1: %6.3f %6.3f %6.3f, h: %d\n", X1, Y1, Z1, h); - printf("X2: %6.3f %6.3f %6.3f\n", X2, Y2, Z2); - printf("X3: %6.3f %6.3f %6.3f, ftnr: %d\n", X3, Y3, Z3, w[mm].ftnr); - */ - for (j=0;j0 ) { + x2[j]=t4[2][j][philf[j][0]].x; y2[j]= t4[2][j][philf[j][0]].y; + } + } + quali=0; - /* reset img coord because of n_img smaller 4 */ - for (j=0;j<4;j++) { x2[j]=-1e10; y2[j]=-1e10; } - - /* search for unused candidates in next time step */ - for (j=0;j0 ) { - x2[j]=t4[2][j][philf[j][0]].x; y2[j]= t4[2][j][philf[j][0]].y; - } - } - quali=0; - - for (j=0;j=2) { - X3 = X2; Y3 =Y2; Z3 = Z2; - invol=0; okay=0; - - det_lsq_3d (Ex, I, G, ap, mmp, - x2[0], y2[0], x2[1], y2[1], x2[2], y2[2], x2[3], y2[3], &X3, &Y3, &Z3); - - /* in volume check */ - if ( X_lay[0] < X3 && X3 < X_lay[1] && - Ymin < Y3 && Y3 < Ymax && - Zmin_lay[0] < Z3 && Z3 < Zmax_lay[1]) {invol=1;} - - /* displacement check */ - if ( invol==1 && - tpar.dvxmin < (X2-X3) && (X2-X3) < tpar.dvxmax && - tpar.dvymin < (Y2-Y3) && (Y2-Y3) < tpar.dvymax && - tpar.dvzmin < (Z2-Z3) && (Z2-Z3) < tpar.dvzmax ) { okay=1; - /* end displacement check */ - if (okay == 1) { - rr=1000000; dl=0; - acc=2*tpar.dacc;angle=2*tpar.dangle; - angle_acc(X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, &angle, &acc); - dl=(sqrt((X1-X0)*(X1-X0)+(Y1-Y0)*(Y1-Y0)+(Z1-Z0)*(Z1-Z0)) - +sqrt((X1-X3)*(X1-X3)+(Y1-Y3)*(Y1-Y3)+(Z1-Z3)*(Z1-Z3)))/2; - - if ( (acc=2) { + X3 = X2; Y3 =Y2; Z3 = Z2; + invol=0; okay=0; + + det_lsq_3d (Ex, I, G, ap, mmp, + x2[0], y2[0], x2[1], y2[1], x2[2], y2[2], x2[3], y2[3], &X3, &Y3, &Z3); + + /* in volume check */ + if ( X_lay[0] < X3 && X3 < X_lay[1] && + Ymin < Y3 && Y3 < Ymax && Zmin_lay[0] < Z3 && Z3 < Zmax_lay[1]) + { + invol=1; + } + + /* displacement check */ + if ( invol==1 && + tpar.dvxmin < (X2-X3) && (X2-X3) < tpar.dvxmax && + tpar.dvymin < (Y2-Y3) && (Y2-Y3) < tpar.dvymax && + tpar.dvzmin < (Z2-Z3) && (Z2-Z3) < tpar.dvzmax ) + { + okay=1; + if (okay == 1) { + rr=1000000; dl=0; + acc=2*tpar.dacc;angle=2*tpar.dangle; + angle_acc(X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, &angle, &acc); + dl = (sqrt((X1-X0)*(X1-X0)+(Y1-Y0)*(Y1-Y0)+(Z1-Z0)*(Z1-Z0)) + + sqrt((X1-X3)*(X1-X3)+(Y1-Y3)*(Y1-Y3)+(Z1-Z3)*(Z1-Z3)))/2; + + if ( (acc 0 ) { - sort(mega[1][h].inlist, &mega[1][h].decis, &mega[1][h].linkdecis); - mega[1][h].finaldecis=mega[1][h].decis[0]; - mega[1][h].next=mega[1][h].linkdecis[0]; - } - } + free(w); + } /* end of h-loop */ - /* create links with decision check */ - for (h=0;h 0 ) { - /* best choice wasn't used yet, so link is created */ - if ( mega[2][mega[1][h].next].prev == -1) { mega[2][mega[1][h].next].prev=h; } - - /* best choice was already used by mega[2][mega[1][h].next].prev */ - else { - /* check which is the better choice */ - if ( mega[1][mega[2][mega[1][h].next].prev].finaldecis > mega[1][h].finaldecis) + /* sort decis and give preliminary "finaldecis" */ + for (h=0;h1) { - /* - printf("zweite Wahl fuer %d waere: %5.3f\n", - mega[1][mega[2][mega[1][h].next].prev].linkdecis[1], - mega[1][mega[2][mega[1][h].next].prev].decis[1]); */ } - - /* remove link with prev */ - mega[1][mega[2][mega[1][h].next].prev].next= -2; - mega[2][mega[1][h].next].prev=h; + if(mega[1][h].inlist > 0 ) { + // stripped & from last 2 args, ad holten 12-2012 + sort(mega[1][h].inlist, mega[1][h].decis, mega[1][h].linkdecis); + mega[1][h].finaldecis=mega[1][h].decis[0]; + mega[1][h].next=mega[1][h].linkdecis[0]; + } } - else { - /* - printf("h ist schlechter, h: %4d jetzt: %5.3f vorher: %d %5.3f\n", - h, mega[1][h].finaldecis, mega[1][mega[2][mega[1][h].next].prev].next, - mega[1][mega[2][mega[1][h].next].prev].finaldecis); */ - if (mega[1][h].inlist>1) { - /* - printf("zweite Wahl fuer %d waere: %5.3f\n", - mega[1][h].linkdecis[1], - mega[1][h].decis[1]); */ } - - mega[1][h].next=-2;} - } - } - if (mega[1][h].next != -2 ) count1++; - } /* end of creation of links with decision check */ - - /* ******** Draw links now ******** */ - if (display) - for (h=0;h0 && c4[2][mega[1][h].next].p[j]>0) { - xp[j]=t4[1][j][c4[1][h].p[j]].x; - yp[j]=t4[1][j][c4[1][h].p[j]].y; - xc[j]=t4[2][j][c4[2][mega[1][h].next].p[j]].x; - yc[j]=t4[2][j][c4[2][mega[1][h].next].p[j]].y; - predict (xp[j], yp[j], xc[j], yc[j], &xn[j], &yn[j]); - - if ( ( fabs(xp[j]-zoom_x[j]) < imx/(2*zoom_f[j])) - && (fabs(yp[j]-zoom_y[j]) < imy/(2*zoom_f[j]))) - { - strcpy(val,""); - sprintf(val ,"orange"); - - intx0 = (int)(imx/2+zoom_f[j]*(xp[j]-zoom_x[j])); - inty0 = (int)(imy/2+zoom_f[j]*(yp[j]-zoom_y[j])); - intx1 = (int)(imx/2+zoom_f[j]*(xc[j]-zoom_x[j])); - inty1 = (int)(imy/2+zoom_f[j]*(yc[j]-zoom_y[j])); - intx2 = (int)(imx/2+zoom_f[j]*(xn[j]-zoom_x[j])); - inty2 = (int)(imy/2+zoom_f[j]*(yn[j]-zoom_y[j])); - - drawcross(interp,intx0,inty0,cr_sz,j,"green"); - drawcross(interp,intx1,inty1,cr_sz+1,j,"yellow"); - drawcross(interp,intx2,inty2,cr_sz+1,j,"white"); - drawvector (interp, intx0, inty0, intx1, inty1, 2, j, buf); - drawvector (interp, intx1, inty1, intx2, inty2, 1, j, "white"); + if(mega[1][h].inlist > 0 ) { + /* best choice wasn't used yet, so link is created */ + if ( mega[2][mega[1][h].next].prev == -1) { mega[2][mega[1][h].next].prev=h; } + + + /* best choice was already used by mega[2][mega[1][h].next].prev */ + else { + /* check which is the better choice */ + if ( mega[1][mega[2][mega[1][h].next].prev].finaldecis > mega[1][h].finaldecis) + { + /* + printf("h ist besser, h: %4d jetzt: %5.3f vorher: %d %5.3f\n", + h, mega[1][h].finaldecis, mega[1][mega[2][mega[1][h].next].prev].next, + mega[1][mega[2][mega[1][h].next].prev].finaldecis); + */ + + if (mega[1][mega[2][mega[1][h].next].prev].inlist>1) { + /* + printf("zweite Wahl fuer %d waere: %5.3f\n", + mega[1][mega[2][mega[1][h].next].prev].linkdecis[1], + mega[1][mega[2][mega[1][h].next].prev].decis[1]); + */ + } - if (mega[1][h].finaldecis> 0.2) { - draw_pnr ( interp, intx0, inty0, h, j, "white"); - draw_pnr ( interp, intx0, inty0+10, mega[1][h].next, j, val); - draw_value (interp, intx0, inty0 + 20,mega[1][h].finaldecis, j, val); } + /* remove link with prev */ + mega[1][mega[2][mega[1][h].next].prev].next= -2; + mega[2][mega[1][h].next].prev=h; + } + else { + /* + printf("h ist schlechter, h: %4d jetzt: %5.3f vorher: %d %5.3f\n", + h, mega[1][h].finaldecis, mega[1][mega[2][mega[1][h].next].prev].next, + mega[1][mega[2][mega[1][h].next].prev].finaldecis); + */ + if (mega[1][h].inlist>1) { + /* + printf("zweite Wahl fuer %d waere: %5.3f\n", + mega[1][h].linkdecis[1], + mega[1][h].decis[1]); + */ + } + mega[1][h].next=-2;} + } + } + if (mega[1][h].next != -2 ) count1++; + } /* end of creation of links with decision check */ - } - } + /* ******** Draw links now ******** */ + if (display) + for (h=0;h0 && c4[2][mega[1][h].next].p[j]>0) { + xp[j]=t4[1][j][c4[1][h].p[j]].x; + yp[j]=t4[1][j][c4[1][h].p[j]].y; + xc[j]=t4[2][j][c4[2][mega[1][h].next].p[j]].x; + yc[j]=t4[2][j][c4[2][mega[1][h].next].p[j]].y; + predict (xp[j], yp[j], xc[j], yc[j], &xn[j], &yn[j]); + + if ( ( fabs(xp[j]-zoom_x[j]) < imx/(2*zoom_f[j])) + && (fabs(yp[j]-zoom_y[j]) < imy/(2*zoom_f[j]))) + { + strcpy(val,""); + sprintf(val ,"orange"); + + intx0 = (int)(imx/2+zoom_f[j]*(xp[j]-zoom_x[j])); + inty0 = (int)(imy/2+zoom_f[j]*(yp[j]-zoom_y[j])); + intx1 = (int)(imx/2+zoom_f[j]*(xc[j]-zoom_x[j])); + inty1 = (int)(imy/2+zoom_f[j]*(yc[j]-zoom_y[j])); + intx2 = (int)(imx/2+zoom_f[j]*(xn[j]-zoom_x[j])); + inty2 = (int)(imy/2+zoom_f[j]*(yn[j]-zoom_y[j])); + + drawcross(interp,intx0,inty0,cr_sz,j,"green"); + drawcross(interp,intx1,inty1,cr_sz+1,j,"yellow"); + drawcross(interp,intx2,inty2,cr_sz+1,j,"white"); + drawvector (interp, intx0, inty0, intx1, inty1, 2, j, buf); + drawvector (interp, intx1, inty1, intx2, inty2, 1, j, "white"); + + if (mega[1][h].finaldecis> 0.2) { + draw_pnr ( interp, intx0, inty0, h, j, "white"); + draw_pnr ( interp, intx0, inty0+10, mega[1][h].next, j, val); + draw_value (interp, intx0, inty0 + 20,mega[1][h].finaldecis, j, val); + } + } + } + } + } } - } - } - - /* ******** End of Draw links now ******** */ - sprintf (buf, "step: %d, curr: %d, next: %d, links: %d, lost: %d, add: %d", - step, m[1], m[2], count1, m[1]-count1, zusatz); - /* for the average of particles and links */ - npart = npart + m[1]; - nlinks = nlinks + count1; - /* - printf("%s\n", buf); - */ + /* ******** End of Draw links now ******** */ + sprintf (buf, "step: %d, curr: %d, next: %d, links: %d, lost: %d, add: %d", + step, m[1], m[2], count1, m[1]-count1, zusatz); - Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); - Tcl_Eval(interp, ".text delete 3"); - Tcl_Eval(interp, ".text insert 3 $tbuf"); + /* for the average of particles and links */ + npart = npart + m[1]; + nlinks = nlinks + count1; + /* + printf("%s\n", buf); + */ - Tcl_Eval(interp, "update idletasks"); + Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); + Tcl_Eval(interp, ".text delete 3"); + Tcl_Eval(interp, ".text insert 3 $tbuf"); - rotate_dataset(); - write_ascii_data(step); - write_added(step); - if(step< seq_last-2) { read_ascii_data(step+3); } + Tcl_Eval(interp, "update idletasks"); + rotate_dataset(); + write_ascii_data(step); + write_added(step); + if(step< seq_last-2) { read_ascii_data(step+3); } - } /* end of sequence loop */ + } /* end of sequence loop */ - /* average of all steps */ - npart /= (seq_last-seq_first); - nlinks /= (seq_last-seq_first); - printf ("Average over sequence, particles: %5.1f, links: %5.1f, lost: %5.1f\n", - npart, nlinks, npart-nlinks); + /* average of all steps */ + npart /= (seq_last-seq_first); + nlinks /= (seq_last-seq_first); + printf ("Average over sequence, particles: %5.1f, links: %5.1f, lost: %5.1f\n", + npart, nlinks, npart-nlinks); - rotate_dataset(); - write_ascii_data(step); - write_added(step); + rotate_dataset(); + write_ascii_data(step); + write_added(step); - /* reset of display flag */ - display = 1; + /* reset of display flag */ + display = 1; - return TCL_OK; + return TCL_OK; } - - - - - - -/* track backwards */ +/* track backwards */ int trackback_c (ClientData clientData, Tcl_Interp* interp, int argc, const char** argv) { - char buf[256]; - int i, j, h, k, step, okay=0, invol=0; - int zaehler1, philf[4][4]; - 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]; - double X1, Y1, Z1, X0, Y0, Z0, X2, Y2, Z2, X5, Y5, Z5; - double X3, Y3, Z3, X4, Y4, Z4, X6, Y6, Z6; - double xn[4], yn[4]; - double rr, Ymin=0, Ymax=0; - double npart=0, nlinks=0; - foundpix *w, p16[16]; - - display = atoi(argv[1]); - - Tcl_Eval(interp, ".text delete 2"); - Tcl_Eval(interp, ".text insert 2 \"Track established correspondences\""); - - /* read data */ - readseqtrackcrit (); - - /*Alloc space, if checkflag for mega, c4, t4 is zero */ - if (!trackallocflag) - { - for (i=0; i<4; i++) + char buf[256]; + int i, j, h, k, step, okay=0, invol=0; + int zaehler1, philf[4][4]; + 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]; + double X1, Y1, Z1, X0, Y0, Z0, X2, Y2, Z2, X5, Y5, Z5; + double X3, Y3, Z3, X4, Y4, Z4, X6, Y6, Z6; + double xn[4], yn[4]; + double rr, Ymin=0, Ymax=0; + double npart=0, nlinks=0; + foundpix *w, p16[16]; + + display = atoi(argv[1]); + + Tcl_Eval(interp, ".text delete 2"); + Tcl_Eval(interp, ".text insert 2 \"Track established correspondences\""); + + /* read data */ + readseqtrackcrit (); + + /*Alloc space, if checkflag for mega, c4, t4 is zero */ + if (!trackallocflag) { - mega[i]=(P *) calloc(sizeof(P),M); - c4[i]=(corres *) calloc(sizeof(corres),M); - for (k=0; k<4; k++) { t4[i][k]=(target *) calloc(sizeof (target),M);} + for (i=0; i<4; i++) + { + mega[i]=(P *) calloc(sizeof(P),M); + c4[i]=(corres *) calloc(sizeof(corres),M); + for (k=0; k<4; k++) { t4[i][k]=(target *) calloc(sizeof (target),M);} + } + trackallocflag=1; } - trackallocflag=1; - } - - /*load again first data sets*/ - step = seq_last; - read_ascii_datanew(step); - rotate_dataset(); - read_ascii_datanew(step-1); - rotate_dataset(); - read_ascii_datanew(step-2); - rotate_dataset(); - read_ascii_datanew(step-3); - - lmax=sqrt((tpar.dvxmin-tpar.dvxmax)*(tpar.dvxmin-tpar.dvxmax) - +(tpar.dvymin-tpar.dvymax)*(tpar.dvymin-tpar.dvymax) - +(tpar.dvzmin-tpar.dvzmax)*(tpar.dvzmin-tpar.dvzmax)); - - volumedimension (&X_lay[1], &X_lay[0], &Ymax, &Ymin, &Zmax_lay[1], &Zmin_lay[0]); - - /* sequence loop */ - for (step = seq_last-1; step > seq_first; step--) - { - sprintf (buf, "Time step: %d, seqnr: %d, Particle info:", step- seq_first, step); - Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); - Tcl_Eval(interp, ".text delete 2"); - Tcl_Eval(interp, ".text insert 2 $tbuf"); - - for (h=0; h seq_first; step--) { - if (mega[1][h].next>=0 && mega[1][h].prev==-1) { - X1=Y1=Z1=X0=Y0=Z0=X2=Y2=Z2=X5=Y5=Z5=X3=Y3=Z3=X4=Y4=Z4=X6=Y6=Z6=-999; - - mega[1][h].inlist=0; - for (i=0; i<16;i++) - { - p16[i].ftnr=-1; - p16[i].freq=0; - for(j=0;j0) { - if (philf[j][k] == -999){ - p16[j*4+k].ftnr=-1; - }else{ - p16[j*4+k].ftnr=t4[3][j][philf[j][k]].tnr; - p16[j*4+k].whichcam[j]=1; + sprintf (buf, "Time step: %d, seqnr: %d, Particle info:", step- seq_first, step); + Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); + Tcl_Eval(interp, ".text delete 2"); + Tcl_Eval(interp, ".text insert 2 $tbuf"); + + for (h=0; h=0 && mega[1][h].prev==-1) { + X1=Y1=Z1=X0=Y0=Z0=X2=Y2=Z2=X5=Y5=Z5=X3=Y3=Z3=X4=Y4=Z4=X6=Y6=Z6=-999; + + mega[1][h].inlist=0; + for (i=0; i<16;i++) + { + p16[i].ftnr=-1; + p16[i].freq=0; + for(j=0;j0) { + if (philf[j][k] == -999){ + p16[j*4+k].ftnr=-1; + } + else { + p16[j*4+k].ftnr=t4[3][j][philf[j][k]].tnr; + p16[j*4+k].whichcam[j]=1; + } + //p16[j*4+k].ftnr=t4[3][j][philf[j][k]].tnr; //Beat 090325 + //if(philf[j][k] != -999) p16[j*4+k].whichcam[j]=1; + //if(philf[j][k] == -999) p16[j*4+k].ftnr=-1; + } + } } - //p16[j*4+k].ftnr=t4[3][j][philf[j][k]].tnr; //Beat 090325 - //if(philf[j][k] != -999) p16[j*4+k].whichcam[j]=1; - //if(philf[j][k] == -999) p16[j*4+k].ftnr=-1; - } - } - } - - /* fill and sort candidate struct */ - sortwhatfound(&p16, &zaehler1); - w = (foundpix *) calloc (zaehler1, sizeof (foundpix)); - - /*end of candidate struct */ - if (zaehler1 > 0) count2++; - for (i=0; i 0) for (i=0; i 0) count2++; + for (i=0; i 0) for (i=0; i0 ) { + x2[j]=t4[2][j][philf[j][0]].x; + y2[j]=t4[2][j][philf[j][0]].y; + } + } + + for (j=0;j=2) { + X3 = X2; Y3 =Y2; Z3 = Z2; + invol=0; okay=0; + + det_lsq_3d (Ex, I, G, ap, mmp, + x2[0], y2[0], x2[1], y2[1], x2[2], y2[2], x2[3], y2[3], &X3, &Y3, &Z3); + + /* volume check */ + if ( X_lay[0] < X3 && X3 < X_lay[1] && + Ymin < Y3 && Y3 < Ymax && + Zmin_lay[0] < Z3 && Z3 < Zmax_lay[1]) { invol=1; } + + okay=0; acc=2*tpar.dacc;angle=2*tpar.dangle;rr=1000000; dl=0; + + /* displacement check */ + if ( invol==1 && + tpar.dvxmin < (X1-X3) && (X1-X3) < tpar.dvxmax && + tpar.dvymin < (Y1-Y3) && (Y1-Y3) < tpar.dvymax && + tpar.dvzmin < (Z1-Z3) && (Z1-Z3) < tpar.dvzmax ) + { + okay=1; + if ( okay ==1 ) { + dl = (sqrt((X1-X0)*(X1-X0)+(Y1-Y0)*(Y1-Y0)+(Z1-Z0)*(Z1-Z0)) + +sqrt((X1-X3)*(X1-X3)+(Y1-Y3)*(Y1-Y3)+(Z1-Z3)*(Z1-Z3)))/2; + + angle_acc(X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, &angle, &acc); + + if ( (acc0 ) { x2[j]=t4[2][j][philf[j][0]].x; y2[j]=t4[2][j][philf[j][0]].y; } - } - - for (j=0;j=2) { - X3 = X2; Y3 =Y2; Z3 = Z2; - invol=0; okay=0; - - det_lsq_3d (Ex, I, G, ap, mmp, - x2[0], y2[0], x2[1], y2[1], x2[2], y2[2], x2[3], y2[3], &X3, &Y3, &Z3); - - /* volume check */ - if ( X_lay[0] < X3 && X3 < X_lay[1] && - Ymin < Y3 && Y3 < Ymax && - Zmin_lay[0] < Z3 && Z3 < Zmax_lay[1]) {invol=1;} - - okay=0; acc=2*tpar.dacc;angle=2*tpar.dangle;rr=1000000; dl=0; - - /* displacement check */ - if ( invol==1 && - tpar.dvxmin < (X1-X3) && (X1-X3) < tpar.dvxmax && - tpar.dvymin < (Y1-Y3) && (Y1-Y3) < tpar.dvymax && - tpar.dvzmin < (Z1-Z3) && (Z1-Z3) < tpar.dvzmax ) { okay=1; - /* end displacement check */ - - if ( okay ==1 ) { - dl=(sqrt((X1-X0)*(X1-X0)+(Y1-Y0)*(Y1-Y0)+(Z1-Z0)*(Z1-Z0)) - +sqrt((X1-X3)*(X1-X3)+(Y1-Y3)*(Y1-Y3)+(Z1-Z3)*(Z1-Z3)))/2; - - angle_acc(X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, &angle, &acc); - - if ( (acc 0 ) { + // // stripped & from last 2 args, ad holten 12-2012 + sort(mega[1][h].inlist, mega[1][h].decis, mega[1][h].linkdecis); + } } - } /* end of if old wasn't found try to create new particle position from rest */ - } - } /* end of h-loop */ - /* sort decis */ - for (h=0;h 0 ) { sort(mega[1][h].inlist, &mega[1][h].decis, &mega[1][h].linkdecis); } - } + /* create links with decision check */ + count1=0; zusatz=0; + for (h=0;h 0 ) { + /* if old/new and unused prev == -1 and next == -2 link is created */ + if ( mega[2][mega[1][h].linkdecis[0]].prev == -1 && mega[2][mega[1][h].linkdecis[0]].next == -2 ) + { + mega[1][h].finaldecis=mega[1][h].decis[0]; + mega[1][h].prev=mega[1][h].linkdecis[0]; + mega[2][mega[1][h].prev].next=h; + zusatz++; + } - /* create links with decision check */ - count1=0; zusatz=0; - for (h=0;h 0 ) { - /* if old/new and unused prev == -1 and next == -2 link is created */ - if ( mega[2][mega[1][h].linkdecis[0]].prev == -1 && mega[2][mega[1][h].linkdecis[0]].next == -2 ) - { - mega[1][h].finaldecis=mega[1][h].decis[0]; - mega[1][h].prev=mega[1][h].linkdecis[0]; - mega[2][mega[1][h].prev].next=h; - zusatz++; - } - - /* old which link to prev has to be checked */ - if ( mega[2][mega[1][h].linkdecis[0]].prev != -1 && mega[2][mega[1][h].linkdecis[0]].next == -2 ) - { - X0=mega[0][mega[1][h].next].x[0]; - Y0=mega[0][mega[1][h].next].x[1]; - Z0=mega[0][mega[1][h].next].x[2]; - - X1=mega[1][h].x[0]; - Y1=mega[1][h].x[1]; - Z1=mega[1][h].x[2]; - - X3=mega[2][mega[1][h].linkdecis[0]].x[0]; - Y3=mega[2][mega[1][h].linkdecis[0]].x[1]; - Z3=mega[2][mega[1][h].linkdecis[0]].x[2]; - - X4=mega[3][mega[2][mega[1][h].linkdecis[0]].prev].x[0]; - Y4=mega[3][mega[2][mega[1][h].linkdecis[0]].prev].x[1]; - Z4=mega[3][mega[2][mega[1][h].linkdecis[0]].prev].x[2]; - - X5=0.5*(5.0*X3-4.0*X1+X0); - Y5=0.5*(5.0*Y3-4.0*Y1+Y0); - Z5=0.5*(5.0*Z3-4.0*Z1+Z0); - - acc=2*tpar.dacc;angle=2*tpar.dangle; - angle_acc(X3, Y3, Z3, X4, Y4, Z4, X5, Y5, Z5, &angle, &acc); + /* old which link to prev has to be checked */ + if ( mega[2][mega[1][h].linkdecis[0]].prev != -1 && mega[2][mega[1][h].linkdecis[0]].next == -2 ) + { + X0=mega[0][mega[1][h].next].x[0]; + Y0=mega[0][mega[1][h].next].x[1]; + Z0=mega[0][mega[1][h].next].x[2]; + + X1=mega[1][h].x[0]; + Y1=mega[1][h].x[1]; + Z1=mega[1][h].x[2]; + + X3=mega[2][mega[1][h].linkdecis[0]].x[0]; + Y3=mega[2][mega[1][h].linkdecis[0]].x[1]; + Z3=mega[2][mega[1][h].linkdecis[0]].x[2]; + + X4=mega[3][mega[2][mega[1][h].linkdecis[0]].prev].x[0]; + Y4=mega[3][mega[2][mega[1][h].linkdecis[0]].prev].x[1]; + Z4=mega[3][mega[2][mega[1][h].linkdecis[0]].prev].x[2]; + + X5=0.5*(5.0*X3-4.0*X1+X0); + Y5=0.5*(5.0*Y3-4.0*Y1+Y0); + Z5=0.5*(5.0*Z3-4.0*Z1+Z0); + + acc=2*tpar.dacc;angle=2*tpar.dangle; + angle_acc(X3, Y3, Z3, X4, Y4, Z4, X5, Y5, Z5, &angle, &acc); + /* + printf("check X0: %6.3f %6.3f %6.3f\n", X0, Y0, Z0); + printf("check X1: %6.3f %6.3f %6.3f\n", X1, Y1, Z1); + printf("check X3: %6.3f %6.3f %6.3f\n", X3, Y3, Z3); + printf("check X4: %6.3f %6.3f %6.3f\n", X4, Y4, Z4); + printf("check X5: %6.3f %6.3f %6.3f\n", X5, Y5, Z5); + printf("check old, new: X3: %6.3f, Y3: %6.3f, Z3: %6.3f, angle: %6.3f, acc: %6.3f\n", X3, Y3, Z3, angle, acc); + */ + if ( (acc seq_first+2) { read_ascii_datanew(step-3); } - - } /* end of sequence loop */ - - /* average of all steps */ - npart /= (seq_last-seq_first-1); - nlinks /= (seq_last-seq_first-1); - printf ("Average over sequence, particles: %5.1f, links: %5.1f, lost: %5.1f\n", - npart, nlinks, npart-nlinks); - - rotate_dataset(); - write_ascii_datanew(step); -write_addedback(step); - - /* reset of display flag */ - display = 1; - return TCL_OK; + printf("%s\n", buf); + */ + + Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY); + Tcl_Eval(interp, ".text delete 3"); + Tcl_Eval(interp, ".text insert 3 $tbuf"); + Tcl_Eval(interp, "update idletasks"); + + rotate_dataset(); + write_ascii_datanew(step); + write_addedback(step); + + if (step> seq_first+2) { + read_ascii_datanew(step-3); + } + } /* end of sequence loop */ + + /* average of all steps */ + npart /= (seq_last-seq_first-1); + nlinks /= (seq_last-seq_first-1); + printf ("Average over sequence, particles: %5.1f, links: %5.1f, lost: %5.1f\n", + npart, nlinks, npart-nlinks); + + rotate_dataset(); + write_ascii_datanew(step); + write_addedback(step); + + /* reset of display flag */ + display = 1; + return TCL_OK; } diff --git a/PTV/src_c/trafo.c b/PTV/src_c/trafo.c index 2f3594b..2cb8b96 100644 --- a/PTV/src_c/trafo.c +++ b/PTV/src_c/trafo.c @@ -1,131 +1,110 @@ /**************************************************************************** -Routine: trafo.c +Routine: trafo.c -Author/Copyright: Hans-Gerd Maas +Author/Copyright: Hans-Gerd Maas -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: 25.5.88 +Creation Date: 25.5.88 -Description: diverse transformations +Description: diverse transformations -Routines contained: pix_crd, crd_pix, affin_trafo, affin_retour +Routines contained: pix_crd, crd_pix, affin_trafo, affin_retour ****************************************************************************/ #include "ptv.h" - -void pixel_to_metric (xp,yp, imx,imy, pix_x,pix_y, xc,yc, field) - -double xp, yp; /* input: pixel coordinates */ -double *xc, *yc; /* output: metric coordinates */ -int imx, imy; /* image format */ -double pix_x, pix_y; /* pixel size */ -int field; - -/* transformation detection pixel coordinates -> geometric coordinates */ -/* single point */ - +void pixel_to_metric(double xp, double yp, int imx, int imy, + double pix_x, double pix_y, double *xc, double *yc, int field) +/* transformation detection pixel coordinates -> geometric coordinates + single point + + double xp, yp : input: pixel coordinates + double *xc, *yc : output: metric coordinates + int imx, imy : image format + double pix_x, pix_y : pixel size + int field +*/ { - switch (field) - { - case 1: yp = 2 * yp + 1; break; - case 2: yp *= 2; break; - } - - *xc = (xp - imx/2.) * pix_x; - *yc = (imy/2. - yp) * pix_y; + switch (field) { + case 1: yp = 2 * yp + 1; break; + case 2: yp *= 2; break; + } + *xc = (xp - imx/2.) * pix_x; + *yc = (imy/2. - yp) * pix_y; } +void metric_to_pixel(double xc, double yc, int imx, int imy, + double pix_x, double pix_y, double *xp, double *yp, int field) - - -void metric_to_pixel (xc,yc, imx,imy, pix_x,pix_y, xp,yp, field) - -double xc, yc; /* input: metric coordinates */ -double *xp, *yp; /* output: pixel coordinates */ -int imx, imy; /* image format */ -double pix_x, pix_y; /* pixel size */ -int field; - -/* transformation detection pixel coordinates -> geometric coordinates */ -/* without read and write */ - +/* transformation detection pixel coordinates -> geometric coordinates + without read and write + + double xc, yc : input: metric coordinates + double *xp, *yp : output: pixel coordinates + int imx, imy : image format + double pix_x, pix_y : pixel size + int field +*/ { - *xp = (xc/pix_x) + imx/2; - *yp = imy/2 - (yc/pix_y); - - switch (field) - { - case 1: *yp = (*yp-1)/2; break; - case 2: *yp /= 2; break; - } -} - + *xp = (xc/pix_x) + imx/2; + *yp = imy/2 - (yc/pix_y); -void distort_brown_affin (x, y, ap, x1, y1) + switch (field) { + case 1: *yp = (*yp-1)/2; break; + case 2: *yp /= 2; break; + } +} -double x, y, *x1, *y1; -ap_52 ap; -/* transformation with Brown + affine */ - +void distort_brown_affin (double x, double y, ap_52 ap, double *x1, double *y1) +/* transformation with Brown + affine */ { - double r; - - - r = sqrt (x*x + y*y); - if (r != 0) - { - x += x * (ap.k1*r*r + ap.k2*r*r*r*r + ap.k3*r*r*r*r*r*r) - + ap.p1 * (r*r + 2*x*x) + 2*ap.p2*x*y; - - y += y * (ap.k1*r*r + ap.k2*r*r*r*r + ap.k3*r*r*r*r*r*r) - + ap.p2 * (r*r + 2*y*y) + 2*ap.p1*x*y; - - *x1 = ap.scx * x - sin(ap.she) * y; *y1 = cos(ap.she) * y; - } + double r; + + r = sqrt (x*x + y*y); + if (r != 0) { + x += x * (ap.k1*r*r + ap.k2*r*r*r*r + ap.k3*r*r*r*r*r*r) + + ap.p1 * (r*r + 2*x*x) + 2*ap.p2*x*y; + + y += y * (ap.k1*r*r + ap.k2*r*r*r*r + ap.k3*r*r*r*r*r*r) + + ap.p2 * (r*r + 2*y*y) + 2*ap.p1*x*y; + + *x1 = ap.scx * x - sin(ap.she) * y; + *y1 = cos(ap.she) * y; + } } - - -void correct_brown_affin (x, y, ap, x1, y1) - -double x, y, *x1, *y1; -ap_52 ap; -/* correct crd to geo with Brown + affine */ - +void correct_brown_affin (double x, double y, ap_52 ap, double *x1, double *y1) +/* correct crd to geo with Brown + affine */ { - double r, xq, yq; - - - r = sqrt (x*x + y*y); - if (r != 0) - { - xq = (x + y*sin(ap.she))/ap.scx - - x * (ap.k1*r*r + ap.k2*r*r*r*r + ap.k3*r*r*r*r*r*r) - - ap.p1 * (r*r + 2*x*x) - 2*ap.p2*x*y; - yq = y/cos(ap.she) - - y * (ap.k1*r*r + ap.k2*r*r*r*r + ap.k3*r*r*r*r*r*r) - - ap.p2 * (r*r + 2*y*y) - 2*ap.p1*x*y; - } - r = sqrt (xq*xq + yq*yq); /* one iteration */ - if (r != 0) - { - *x1 = (x + yq*sin(ap.she))/ap.scx - - xq * (ap.k1*r*r + ap.k2*r*r*r*r + ap.k3*r*r*r*r*r*r) - - ap.p1 * (r*r + 2*xq*xq) - 2*ap.p2*xq*yq; - *y1 = y/cos(ap.she) - - yq * (ap.k1*r*r + ap.k2*r*r*r*r + ap.k3*r*r*r*r*r*r) - - ap.p2 * (r*r + 2*yq*yq) - 2*ap.p1*xq*yq; - } + double r, xq, yq; + + r = sqrt (x*x + y*y); + if (r != 0) { + xq = (x + y*sin(ap.she))/ap.scx + - x * (ap.k1*r*r + ap.k2*r*r*r*r + ap.k3*r*r*r*r*r*r) + - ap.p1 * (r*r + 2*x*x) - 2*ap.p2*x*y; + yq = y/cos(ap.she) + - y * (ap.k1*r*r + ap.k2*r*r*r*r + ap.k3*r*r*r*r*r*r) + - ap.p2 * (r*r + 2*y*y) - 2*ap.p1*x*y; + } + r = sqrt (xq*xq + yq*yq); /* one iteration */ + if (r != 0) { + *x1 = (x + yq*sin(ap.she))/ap.scx + - xq * (ap.k1*r*r + ap.k2*r*r*r*r + ap.k3*r*r*r*r*r*r) + - ap.p1 * (r*r + 2*xq*xq) - 2*ap.p2*xq*yq; + *y1 = y/cos(ap.she) + - yq * (ap.k1*r*r + ap.k2*r*r*r*r + ap.k3*r*r*r*r*r*r) + - ap.p2 * (r*r + 2*yq*yq) - 2*ap.p1*xq*yq; + } } diff --git a/PTV/src_c/ttools.c b/PTV/src_c/ttools.c index 79f094c..0204c8e 100644 --- a/PTV/src_c/ttools.c +++ b/PTV/src_c/ttools.c @@ -1,384 +1,379 @@ /**************************************************************************** -Author/Copyright: Jochen Willneff +Author/Copyright: Jochen Willneff -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: end of 99 ...going on +Creation Date: end of 99 ...going on -Description: different search routines and track tools +Description: different search routines and track tools -Routines contained: pix_in_next, candsearch_in_pix, searchposition, - predict, readseqtrackcrit, readtrackdata, - searchquader +Routines contained: pix_in_next, candsearch_in_pix, searchposition, + predict, readseqtrackcrit, readtrackdata, + searchquader ****************************************************************************/ #include "ptv.h" -int pix_in_next (next, num, x, y, dl, dr, du, dd, found) -target next[]; -int num; -double x, y, dl, dr, du, dd; -int found[POSI]; +#ifdef EVER_CALLED // Unused function, ad holten 12-2012 +int pix_in_next (target next[], int num, double x, double y, + double dl, double dr, double du, double dd, int found[POSI]) { - /* search of POSI near candidates in targetlist */ - - int j, j0, dj; - int zaehler=0; - double d, xmin, xmax, ymin, ymax; - double dcand[POSI]; - int cand[POSI]; - xmin = x - dl; xmax = x + dr; ymin = y - du; ymax = y + dd; - - - /* binarized search for start point of candidate search */ - for (j0=num/2, dj=num/4; dj>1; dj/=2) - { - if (next[j0].y < ymin) j0 += dj; - else j0 -= dj; - } - - j0 -= 12; if (j0 < 0) j0 = 0; /* due to trunc */ - for (j=j0; j ymax) break; /* finish search */ - if (next[j].x > xmin && next[j].x < xmax && next[j].y > ymin && next[j].y < ymax && next[j].tnr>0) + /* search of POSI near candidates in targetlist */ + + int j, j0, dj; + int zaehler=0; + double d, xmin, xmax, ymin, ymax; + double dcand[POSI]; + int cand[POSI]; + xmin = x - dl; xmax = x + dr; ymin = y - du; ymax = y + dd; + + + /* binarized search for start point of candidate search */ + for (j0=num/2, dj=num/4; dj>1; dj/=2) { - d = sqrt ((x-next[j].x)*(x-next[j].x) + (y-next[j].y)*(y-next[j].y)); - cand[zaehler]=next[j].tnr; dcand[zaehler]=d; - zaehler++; + if (next[j0].y < ymin) j0 += dj; + else j0 -= dj; } - } - return (zaehler); + j0 -= 12; if (j0 < 0) j0 = 0; /* due to trunc */ + for (j=j0; j ymax) break; /* finish search */ + if (next[j].x > xmin && next[j].x < xmax && next[j].y > ymin && next[j].y < ymax && next[j].tnr>0) + { + d = sqrt ((x-next[j].x)*(x-next[j].x) + (y-next[j].y)*(y-next[j].y)); + cand[zaehler]=next[j].tnr; dcand[zaehler]=d; + zaehler++; + } + } + return (zaehler); } +#endif - -int candsearch_in_pix (next, num, x, y, dl, dr, du, dd, p) -target next[]; -int num; -double x, y, dl, dr, du, dd; -int p[4]; +int candsearch_in_pix (target next[], int num, double x, double y, + double dl, double dr, double du, double dd, int p[4]) { - /* search of four near candidates in targetlist */ - - int j, j0, dj, pnr = -999; - int zaehler=0, p1, p2, p3, p4; - double d, dmin=1e20, xmin, xmax, ymin, ymax; - double d1, d2, d3, d4; - xmin = x - dl; xmax = x + dr; ymin = y - du; ymax = y + dd; - - if(xmin<0.0) xmin=0.0; - if(xmax>imx) xmax=imx; - if(ymin<0.0) ymin=0.0; - if(ymax>imy) ymax=imy; - - if(x<0.0) x=0.0; - if(x>imx) x=imx; - if(y<0.0) y=0.0; - if(y>imy) y=imy; - - p1 = p2 = p3 = p4 = -999; - d1 = d2 = d3 = d4 = dmin; - - if (x>=0.0 && x<=imx ) { if (y>=0.0 && y<=imy ) { - - /* binarized search for start point of candidate search */ - for (j0=num/2, dj=num/4; dj>1; dj/=2) - { - if (next[j0].y < ymin) j0 += dj; - else j0 -= dj; - } - - j0 -= 12; if (j0 < 0) j0 = 0; /* due to trunc */ - for (j=j0; j ymax ) break; /* finish search */ - if (next[j].x > xmin && next[j].x < xmax && next[j].y > ymin && next[j].y < ymax ) - { - d = sqrt ((x-next[j].x)*(x-next[j].x) + (y-next[j].y)*(y-next[j].y)); - - if (d < dmin) { dmin = d; pnr = j; - } - if ( d < d1 ) - { - p4=p3; p3=p2; p2=p1; p1=j; - d4=d3; d3=d2; d2=d1; d1=d; - } - else if ( d1 < d && d < d2 ) - { - p4=p3; p3=p2; p2=j; - d4=d3; d3=d2; d2=d; - } - else if ( d2 < d && d < d3 ) - { - p4=p3; p3=j; - d4=d3; d3=d; - } - else if ( d3 < d && d < d4 ) - { - p4=j; - d4=d; - } - } - } - } - - p[0]=p1; - p[1]=p2; - p[2]=p3; - p[3]=p4; - for (j=0; j<4; j++) if ( p[j] != -999 ) {zaehler++; } - } } - return (zaehler); + /* search of four near candidates in targetlist */ + + int j, j0, dj, pnr = -999; + int zaehler=0, p1, p2, p3, p4; + double d, dmin=1e20, xmin, xmax, ymin, ymax; + double d1, d2, d3, d4; + xmin = x - dl; xmax = x + dr; ymin = y - du; ymax = y + dd; + + if(xmin<0.0) xmin=0.0; + if(xmax>imx) xmax=imx; + if(ymin<0.0) ymin=0.0; + if(ymax>imy) ymax=imy; + + if(x<0.0) x=0.0; + if(x>imx) x=imx; + if(y<0.0) y=0.0; + if(y>imy) y=imy; + + p1 = p2 = p3 = p4 = -999; + d1 = d2 = d3 = d4 = dmin; + + if (x>=0.0 && x<=imx ) { + if (y>=0.0 && y<=imy ) { + + /* binarized search for start point of candidate search */ + for (j0=num/2, dj=num/4; dj>1; dj/=2) + { + if (next[j0].y < ymin) j0 += dj; + else j0 -= dj; + } + + j0 -= 12; if (j0 < 0) j0 = 0; /* due to trunc */ + for (j=j0; j ymax ) break; /* finish search */ + if (next[j].x > xmin && next[j].x < xmax && next[j].y > ymin && next[j].y < ymax ) + { + d = sqrt ((x-next[j].x)*(x-next[j].x) + (y-next[j].y)*(y-next[j].y)); + + if (d < dmin) { + dmin = d; pnr = j; + } + if ( d < d1 ) + { + p4=p3; p3=p2; p2=p1; p1=j; + d4=d3; d3=d2; d2=d1; d1=d; + } + else if ( d1 < d && d < d2 ) + { + p4=p3; p3=p2; p2=j; + d4=d3; d3=d2; d2=d; + } + else if ( d2 < d && d < d3 ) + { + p4=p3; p3=j; + d4=d3; d3=d; + } + else if ( d3 < d && d < d4 ) + { + p4=j; + d4=d; + } + } + } + } + p[0]=p1; + p[1]=p2; + p[2]=p3; + p[3]=p4; + for (j=0; j<4; j++) if ( p[j] != -999 ) {zaehler++; } + } + } + return (zaehler); } -int candsearch_in_pixrest (next, num, x, y, dl, dr, du, dd, p) -target next[]; -int num; -double x, y, dl, dr, du, dd; -int p[4]; +int candsearch_in_pixrest (target next[], int num, double x, double y, + double dl, double dr, double du, double dd, int p[4]) { - /* search of four near candidates in targetlist */ - - int j, j0, dj; - int zaehler=0, p1, p2, p3, p4; - double d, dmin=1e20, xmin, xmax, ymin, ymax; - xmin = x - dl; xmax = x + dr; ymin = y - du; ymax = y + dd; - - if(xmin<0.0) xmin=0.0; - if(xmax>imx) xmax=imx; - if(ymin<0.0) ymin=0.0; - if(ymax>imy) ymax=imy; - - if(x<0.0) x=0.0; - if(x>imx) x=imx; - if(y<0.0) y=0.0; - if(y>imy) y=imy; - - p1 = p2 = p3 = p4 = -999; - - /* binarized search for start point of candidate search */ - for (j0=num/2, dj=num/4; dj>1; dj/=2) - { - if (next[j0].y < ymin) j0 += dj; - else j0 -= dj; - } - - j0 -= 12; if (j0 < 0) j0 = 0; /* due to trunc */ - for (j=j0; j ymax ) break; /* finish search */ - if (next[j].x > xmin && next[j].x < xmax && next[j].y > ymin && next[j].y < ymax ) - { - d = sqrt ((x-next[j].x)*(x-next[j].x) + (y-next[j].y)*(y-next[j].y)); - if (d < dmin) { dmin = d; p1 = j; } - } - } - } - - p[0]=p1; - p[1]=p2; - p[2]=p3; - p[3]=p4; - for (j=0; j<4; j++) if ( p[j] != -999 ) {zaehler++; } - return (zaehler); -} + /* search of four near candidates in targetlist */ + + int j, j0, dj; + int zaehler=0, p1, p2, p3, p4; + double d, dmin=1e20, xmin, xmax, ymin, ymax; + xmin = x - dl; xmax = x + dr; ymin = y - du; ymax = y + dd; + if(xmin<0.0) xmin=0.0; + if(xmax>imx) xmax=imx; + if(ymin<0.0) ymin=0.0; + if(ymax>imy) ymax=imy; + if(x<0.0) x=0.0; + if(x>imx) x=imx; + if(y<0.0) y=0.0; + if(y>imy) y=imy; -void predict (x1, y1, x2, y2, x3, y3) -double x1, y1, x2, y2, *x3, *y3; + p1 = p2 = p3 = p4 = -999; + + /* binarized search for start point of candidate search */ + for (j0=num/2, dj=num/4; dj>1; dj/=2) + { + if (next[j0].y < ymin) j0 += dj; + else j0 -= dj; + } + + j0 -= 12; if (j0 < 0) j0 = 0; /* due to trunc */ + for (j=j0; j ymax ) break; /* finish search */ + if (next[j].x > xmin && next[j].x < xmax && next[j].y > ymin && next[j].y < ymax ) + { + d = sqrt ((x-next[j].x)*(x-next[j].x) + (y-next[j].y)*(y-next[j].y)); + if (d < dmin) { dmin = d; p1 = j; } + } + } + } + + p[0]=p1; + p[1]=p2; + p[2]=p3; + p[3]=p4; + for (j=0; j<4; j++) if ( p[j] != -999 ) {zaehler++; } + return (zaehler); +} + + +void predict (double x1, double y1, double x2, double y2, double *x3, double *y3) { - *x3 = 2*x2 - x1; - *y3 = 2*y2 - y1; - return; + *x3 = 2*x2 - x1; + *y3 = 2*y2 - y1; + return; } void readseqtrackcrit () { - int i_img; - /* reads pixfiles and try to track particles in imagespace - over the sequence */ - 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); - - fpp = fopen_r ("parameters/track.par"); - fscanf (fpp, "%lf\n", &tpar.dvxmin); - fscanf (fpp, "%lf\n", &tpar.dvxmax); - fscanf (fpp, "%lf\n", &tpar.dvymin); - fscanf (fpp, "%lf\n", &tpar.dvymax); - fscanf (fpp, "%lf\n", &tpar.dvzmin); - fscanf (fpp, "%lf\n", &tpar.dvzmax); - fscanf (fpp, "%lf\n", &tpar.dangle); - fscanf (fpp, "%lf\n", &tpar.dacc); - fscanf (fpp,"%d\n", &tpar.add); - /* - fscanf (fpp,"%d\n", &tpar.dsumg); - fscanf (fpp,"%d\n", &tpar.dn); - fscanf (fpp,"%d\n", &tpar.dnx); - fscanf (fpp,"%d\n", &tpar.dny); - */ - fclose (fpp); - - /* read illuminated layer data */ - fpp = fopen_r ("parameters/criteria.par"); - fscanf (fpp, "%lf\n", &X_lay[0]); - fscanf (fpp, "%lf\n", &Zmin_lay[0]); - fscanf (fpp, "%lf\n", &Zmax_lay[0]); - fscanf (fpp, "%lf\n", &X_lay[1]); - fscanf (fpp, "%lf\n", &Zmin_lay[1]); - fscanf (fpp, "%lf\n", &Zmax_lay[1]); - fclose (fpp); + int i_img; + /* reads pixfiles and try to track particles in imagespace + over the sequence */ + fpp = fopen_rp ("parameters/sequence.par"); // replaced fopen_r, ad holten 12-2012 + if (!fpp) return; + 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); + + fpp = fopen_rp ("parameters/track.par"); // replaced fopen_r, ad holten 12-2012 + if (!fpp) return; + fscanf (fpp, "%lf\n", &tpar.dvxmin); + fscanf (fpp, "%lf\n", &tpar.dvxmax); + fscanf (fpp, "%lf\n", &tpar.dvymin); + fscanf (fpp, "%lf\n", &tpar.dvymax); + fscanf (fpp, "%lf\n", &tpar.dvzmin); + fscanf (fpp, "%lf\n", &tpar.dvzmax); + fscanf (fpp, "%lf\n", &tpar.dangle); + fscanf (fpp, "%lf\n", &tpar.dacc); + fscanf (fpp,"%d\n", &tpar.add); + /* + fscanf (fpp,"%d\n", &tpar.dsumg); + fscanf (fpp,"%d\n", &tpar.dn); + fscanf (fpp,"%d\n", &tpar.dnx); + fscanf (fpp,"%d\n", &tpar.dny); + */ + fclose (fpp); + + /* read illuminated layer data */ + fpp = fopen_rp ("parameters/criteria.par"); // replaced fopen_r, ad holten 12-2012 + if (!fpp) return; + fscanf (fpp, "%lf\n", &X_lay[0]); + fscanf (fpp, "%lf\n", &Zmin_lay[0]); + fscanf (fpp, "%lf\n", &Zmax_lay[0]); + fscanf (fpp, "%lf\n", &X_lay[1]); + fscanf (fpp, "%lf\n", &Zmin_lay[1]); + fscanf (fpp, "%lf\n", &Zmax_lay[1]); + fclose (fpp); } -void searchquader(X, Y, Z, xr, xl, yd, yu) -double X, Y, Z, xr[4], xl[4], yd[4], yu[4]; +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; ixr[i] ) xr[i]=x; - if (y >yd[i] ) yd[i]=y; + /* project quader in image space to define search area */ + for (k=0; k<8; k++) + { + quader[k].pnr=k; } - 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; - + /* 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; ixr[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; } void sortwhatfound (foundpix item[16], int *zaehler) { - int i,j,m, different; - foundpix temp; - - different=0; - - /* where what was found */ - for (i=0; i<16; i++) - for (j=0; j<4; j++) - for (m=0; m<4; m++) - if(item[i].ftnr == item[4*j+m].ftnr) - { - item[i].whichcam[j]=1; - } - - /* how often was ftnr found */ - for (i=0; i<16; i++) - for (j=0; j=i; --j) - { - if ( item[j-1].freq < item[j].freq ) + int i,j,m, different; + foundpix temp; + + different=0; + + /* where what was found */ + for (i=0; i<16; i++) + for (j=0; j<4; j++) + for (m=0; m<4; m++) + if(item[i].ftnr == item[4*j+m].ftnr) + { + item[i].whichcam[j]=1; + } + + /* how often was ftnr found */ + for (i=0; i<16; i++) + for (j=0; j=i; --j) { - temp = *(item+j-1); *(item+j-1) = *(item+j); *(item+j) = temp; + if ( item[j-1].freq < item[j].freq ) + { + temp = *(item+j-1); *(item+j-1) = *(item+j); *(item+j) = temp; + } } - } - - for (i=0; i<16; i++) - for (j=i+1; j<16; j++) - { - if (item[i].ftnr == item[j].ftnr || item[j].freq <2) - { - item[j].freq=0; - item[j].ftnr=-1; - } - } - - /* sort freq */ - for (i=1; i<16; ++i) for (j=16-1; j>=i; --j) - { - if ( item[j-1].freq < item[j].freq ) + + for (i=0; i<16; i++) + for (j=i+1; j<16; j++) { - temp = *(item+j-1); *(item+j-1) = *(item+j); *(item+j) = temp; + if (item[i].ftnr == item[j].ftnr || item[j].freq <2) + { + item[j].freq=0; + item[j].ftnr=-1; + } } - } - for (i=0; i<16; ++i) if(item[i].freq != 0) different++; - *zaehler=different; + /* sort freq */ + for (i=1; i<16; ++i) for (j=16-1; j>=i; --j) + { + if ( item[j-1].freq < item[j].freq ) + { + temp = *(item+j-1); *(item+j-1) = *(item+j); *(item+j) = temp; + } + } + for (i=0; i<16; ++i) if(item[i].freq != 0) different++; + *zaehler=different; } -void angle_acc( X0, Y0, Z0,X1, Y1, Z1, X2, Y2, Z2, angle, acc ) -double X0, Y0, Z0,X1, Y1, Z1, X2, Y2, Z2, *angle, *acc; +void angle_acc(double X0, double Y0, double Z0, double X1, double Y1, double Z1, + double X2, double Y2, double Z2, double *angle, double *acc) { - double ds, da; - coord_3d v0, v1, v2; - - /* calculation of angle and acceleration */ - v0.x=X1-X0; v0.y=Y1-Y0; v0.z=Z1-Z0; - v1.x=X2-X0; v1.y=Y2-Y0; v1.z=Z2-Z0; - v2.x=v1.x-v0.x; v2.y=v1.y-v0.y; v2.z=v1.z-v0.z; - - ds= sqrt( v2.x*v2.x+v2.y*v2.y+v2.z*v2.z); - /* special case 200 gon */ - if (v1.x ==-v0.x && v1.y ==-v0.y && v1.z ==-v0.z) - { *angle =200; } else { - da=acos ((v0.x*v1.x+v0.y*v1.y+v0.z*v1.z)/ - (sqrt( v1.x*v1.x+v1.y*v1.y+v1.z*v1.z)* - sqrt( v0.x*v0.x+v0.y*v0.y+v0.z*v0.z))); - *angle=da*ro; } - *acc=ds; - - return; + double ds, da; + coord_3d v0, v1, v2; + + /* calculation of angle and acceleration */ + v0.x=X1-X0; v0.y=Y1-Y0; v0.z=Z1-Z0; + v1.x=X2-X0; v1.y=Y2-Y0; v1.z=Z2-Z0; + v2.x=v1.x-v0.x; v2.y=v1.y-v0.y; v2.z=v1.z-v0.z; + + ds= sqrt( v2.x*v2.x+v2.y*v2.y+v2.z*v2.z); + /* special case 200 gon */ + if (v1.x ==-v0.x && v1.y ==-v0.y && v1.z ==-v0.z) + { + *angle =200; + } + else { + da=acos ((v0.x*v1.x+v0.y*v1.y+v0.z*v1.z)/ + (sqrt( v1.x*v1.x+v1.y*v1.y+v1.z*v1.z)* + sqrt( v0.x*v0.x+v0.y*v0.y+v0.z*v0.z))); + *angle=da*ro; + } + *acc=ds; + return; } diff --git a/PTV/src_c/vrml.c b/PTV/src_c/vrml.c index b7a1d41..0f74dba 100644 --- a/PTV/src_c/vrml.c +++ b/PTV/src_c/vrml.c @@ -1,668 +1,694 @@ /**************************************************************************** -Author/Copyright: Jochen Willneff +Author/Copyright: Jochen Willneff -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: end of 99 ... - -Description: creates VRML files from tracks and - detected particles - -Routines contained: vrmltracks_c, vrmldetections_c, vrmldettracks_c +Creation Date: end of 99 ... + +Description: creates VRML files from tracks and + detected particles + +Routines contained: vrmltracks_c, vrmldetections_c, vrmldettracks_c ****************************************************************************/ #include "ptv.h" int vrmltracks_c(ClientData clientData, Tcl_Interp* interp, int argc, const char** argv) { - int i, anz1, anz2, m, j, k; - FILE *fp1, *fp2; - char val[256]; - vector *line1, *line2; - double color, ymin=0, ymax=0, cubes; - double mx, my, mz, dx, dy, dz, du, dl, rotz, rotx; - - - /* open file for line elements */ - fp2 = fopen ("tracks.wrl", "w"); - - /* create header and coordsys for vrml-file */ - fprintf(fp2, "#VRML V1.0 ascii\n\n"); + int i, anz1, anz2, m, j; + FILE *fp1, *fp2; + char val[256]; + vector *line1, *line2; + double color, ymin=0, ymax=0, cubes; + double mx, my, mz, dx, dy, dz, du, dl, rotz, rotx; + + + /* open file for line elements */ + fp2 = fopen ("tracks.wrl", "w"); + + /* create header and coordsys for vrml-file */ + fprintf(fp2, "#VRML V1.0 ascii\n\n"); + + /* create boundaries from object volume */ + volumedimension(&X_lay[1], &X_lay[0], &ymax, &ymin, &Zmax_lay[1], &Zmin_lay[0]); + cubes=(Zmax_lay[1]-Zmin_lay[0])/600; + + /* create viewpoint */ + fprintf(fp2, " PerspectiveCamera {\n"); + fprintf(fp2, " position %7.3f %7.3f %7.3f\n", + (X_lay[0]+X_lay[1])/2,(ymax+ymin)/2,Zmax_lay[1]); + fprintf(fp2, " orientation 1 0 0 0\n"); + fprintf(fp2, " focalDistance 5\n"); + fprintf(fp2, " heightAngle 0.785398 }\n\n\n"); - /* create boundaries from object volume */ - volumedimension(&X_lay[1], &X_lay[0], &ymax, &ymin, &Zmax_lay[1], &Zmin_lay[0]); - cubes=(Zmax_lay[1]-Zmin_lay[0])/600; - - /* create viewpoint */ - fprintf(fp2, " PerspectiveCamera {\n"); - fprintf(fp2, " position %7.3f %7.3f %7.3f\n", - (X_lay[0]+X_lay[1])/2,(ymax+ymin)/2,Zmax_lay[1]); - fprintf(fp2, " orientation 1 0 0 0\n"); - fprintf(fp2, " focalDistance 5\n"); - fprintf(fp2, " heightAngle 0.785398 }\n\n\n"); - - /* create cameras */ - /* - fprintf(fp2, "#create cameras\n\n"); - - for (i=0; i= 0) { + + fprintf(fp2, " Separator {\n"); + fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f}\n", + line1[j].x1, line1[j].y1, line1[j].z1); + + fprintf(fp2, " Cube { width %3.2f height %3.2f depth %3.2f } }\n", + cubes, cubes, cubes ); + /* + fprintf(fp2, " Sphere { radius %3.2f } }\n", cubes ); + */ + /* create cubes of last time step */ + if (i ==seq_last-1) { + + fprintf(fp2, " Separator {\n"); + fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f}\n", + line2[m].x1, line2[m].y1, line2[m].z1); + + fprintf(fp2, " Cube { width %3.2f height %3.2f depth %3.2f } }\n", + cubes, cubes, cubes ); + /* + fprintf(fp2, " Sphere { radius %3.2f } }\n", cubes ); + */ + } + + /* create last cubes of ending trajectories */ + + if (line2[m].n <= 0) { + + fprintf(fp2, " Separator {\n"); + fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f}\n", + line2[m].x1, line2[m].y1, line2[m].z1); + + fprintf(fp2, " Cube { width %3.2f height %3.2f depth %3.2f } }\n", + cubes, cubes, cubes ); + /* + fprintf(fp2, " Sphere { radius %3.2f } }\n", cubes ); + */ + } + + mx=(line1[j].x1+line2[m].x1)/2; + my=(line1[j].y1+line2[m].y1)/2; + mz=(line1[j].z1+line2[m].z1)/2; + dx=line1[j].x1-line2[m].x1; + dy=line1[j].y1-line2[m].y1; + dz=line1[j].z1-line2[m].z1; + du=sqrt(dx*dx+dy*dy); + dl=sqrt(dx*dx+dy*dy+dz*dz); + + rotz=0; + if(dy == 0.0) {rotz=-M_PI/2;} else {rotz = -atan(dx/dy);} + + rotx=0; + if(du == 0.0) {rotx=M_PI/2;} + + if(du != 0.0) { + if(dx>=0.0 && dy>=0.0 && dz> 0.0) {rotx = atan(dz/du);} + if(dx>=0.0 && dy< 0.0 && dz> 0.0) {rotx = -atan(dz/du);} + if(dx< 0.0 && dy> 0.0 && dz> 0.0) {rotx = atan(dz/du);} + if(dx< 0.0 && dy<=0.0 && dz> 0.0) {rotx = -atan(dz/du);} + if(dx>=0.0 && dy>=0.0 && dz< 0.0) {rotx = atan(dz/du);} + if(dx>=0.0 && dy< 0.0 && dz< 0.0) {rotx = -atan(dz/du);} + if(dx< 0.0 && dy> 0.0 && dz< 0.0) {rotx = atan(dz/du);} + if(dx< 0.0 && dy<=0.0 && dz< 0.0) {rotx = -atan(dz/du);} + } + + /* + fprintf(fp2, " Separator {\n"); + fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f}\n",mx, my, mz); + fprintf(fp2, " Transform {rotation 0 0 1 %7.5f}\n",rotz); + fprintf(fp2, " Transform {rotation 1 0 0 %7.5f}\n",rotx); + + fprintf(fp2, " Cylinder { radius %3.2f height %3.2f } }\n\n",cubes/4, dl); + + fprintf(fp2, " Cube {width %3.2f height %3.2f depth %3.2f } }\n\n",cubes/2, dl,cubes/2); + */ + /* end of cylinder */ + + + fprintf(fp2, " Coordinate3 { point [\n"); + fprintf(fp2, " %7.3f %7.3f %7.3f,\n",line1[j].x1, line1[j].y1, line1[j].z1); + fprintf(fp2, " %7.3f %7.3f %7.3f, ] }\n", line2[m].x1, line2[m].y1, line2[m].z1); + fprintf(fp2, " IndexedLineSet { coordIndex [ 0, 1, -1] } }\n\n"); + + } + } + fprintf(fp2, " }\n\n"); + fprintf(fp2, "# end of time step %d\n\n", i); + strcpy(val, ""); + free(line1); free(line2); + line1 = line2 = NULL; // added, ad holten 2012 + } /* end of sequence loop */ + + if (line1) free(line1); + if (line2) free(line2); - for (i=seq_first; i= 0) { - - fprintf(fp2, " Separator {\n"); - fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f}\n", - line1[j].x1, line1[j].y1, line1[j].z1); - - fprintf(fp2, " Cube { width %3.2f height %3.2f depth %3.2f } }\n", - cubes, cubes, cubes ); - /* - fprintf(fp2, " Sphere { radius %3.2f } }\n", cubes ); - */ - /* create cubes of last time step */ - if(i ==seq_last-1) { - - fprintf(fp2, " Separator {\n"); - fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f}\n", - line2[m].x1, line2[m].y1, line2[m].z1); - - fprintf(fp2, " Cube { width %3.2f height %3.2f depth %3.2f } }\n", - cubes, cubes, cubes ); - /* - fprintf(fp2, " Sphere { radius %3.2f } }\n", cubes ); - */ - } - - /* create last cubes of ending trajectories */ - - if(line2[m].n <= 0) { - - fprintf(fp2, " Separator {\n"); - fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f}\n", - line2[m].x1, line2[m].y1, line2[m].z1); - - fprintf(fp2, " Cube { width %3.2f height %3.2f depth %3.2f } }\n", - cubes, cubes, cubes ); - /* - fprintf(fp2, " Sphere { radius %3.2f } }\n", cubes ); - */ - } - - mx=(line1[j].x1+line2[m].x1)/2; - my=(line1[j].y1+line2[m].y1)/2; - mz=(line1[j].z1+line2[m].z1)/2; - dx=line1[j].x1-line2[m].x1; - dy=line1[j].y1-line2[m].y1; - dz=line1[j].z1-line2[m].z1; - du=sqrt(dx*dx+dy*dy); - dl=sqrt(dx*dx+dy*dy+dz*dz); - - rotz=0; - if(dy == 0.0) {rotz=-M_PI/2;} else {rotz = -atan(dx/dy);} - - rotx=0; - if(du == 0.0) {rotx=M_PI/2;} - - if(du != 0.0) { - if(dx>=0.0 && dy>=0.0 && dz> 0.0) {rotx = atan(dz/du);} - if(dx>=0.0 && dy< 0.0 && dz> 0.0) {rotx = -atan(dz/du);} - if(dx< 0.0 && dy> 0.0 && dz> 0.0) {rotx = atan(dz/du);} - if(dx< 0.0 && dy<=0.0 && dz> 0.0) {rotx = -atan(dz/du);} - if(dx>=0.0 && dy>=0.0 && dz< 0.0) {rotx = atan(dz/du);} - if(dx>=0.0 && dy< 0.0 && dz< 0.0) {rotx = -atan(dz/du);} - if(dx< 0.0 && dy> 0.0 && dz< 0.0) {rotx = atan(dz/du);} - if(dx< 0.0 && dy<=0.0 && dz< 0.0) {rotx = -atan(dz/du);} - } - - /* - fprintf(fp2, " Separator {\n"); - fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f}\n",mx, my, mz); - fprintf(fp2, " Transform {rotation 0 0 1 %7.5f}\n",rotz); - fprintf(fp2, " Transform {rotation 1 0 0 %7.5f}\n",rotx); - - fprintf(fp2, " Cylinder { radius %3.2f height %3.2f } }\n\n",cubes/4, dl); - - fprintf(fp2, " Cube {width %3.2f height %3.2f depth %3.2f } }\n\n",cubes/2, dl,cubes/2); - */ - /* end of cylinder */ - - - fprintf(fp2, " Separator {\n"); - fprintf(fp2, " Coordinate3 { point [\n"); - fprintf(fp2, " %7.3f %7.3f %7.3f,\n",line1[j].x1, line1[j].y1, line1[j].z1); - fprintf(fp2, " %7.3f %7.3f %7.3f, ] }\n", line2[m].x1, line2[m].y1, line2[m].z1); - fprintf(fp2, " IndexedLineSet { coordIndex [ 0, 1, -1] } }\n\n"); - - } - } - fprintf(fp2, " }\n\n"); - fprintf(fp2, "# end of time step %d\n\n", i); - strcpy(val, ""); - free(line1); free(line2); - } /* end of sequence loop */ - - fprintf(fp2, "# trajectories finished\n"); - - fclose(fp2); - Tcl_Eval(interp, ".text delete 2"); - Tcl_Eval(interp, ".text insert 2 \"Tracks written to VRML-File: tracks.wrl\""); - 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; + 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 vrmldetections_c(ClientData clientData, Tcl_Interp* interp, int argc, const char** argv) { - int i, anz1, j, k, dumy; - FILE *fp1, *fp2; - char val[256]; - vector *line1; - double color, ymin=0, ymax=0, cubes; + int i, anz1, j, dumy; + FILE *fp1, *fp2; + char val[256]; + vector *line1; + double color, ymin=0, ymax=0, cubes; - /* open file for line elements */ - fp2 = fopen ("detections.wrl", "w"); + /* open file for line elements */ + fp2 = fopen ("detections.wrl", "w"); - fprintf(fp2, "#VRML V1.0 ascii\n\n"); - /* create header and coordsys for vrml-file */ - - /* create boundaries from object volume */ - volumedimension(&X_lay[1], &X_lay[0], &ymax, &ymin, &Zmax_lay[1], &Zmin_lay[0]); - cubes=(Zmax_lay[1]-Zmin_lay[0])/500; + fprintf(fp2, "#VRML V1.0 ascii\n\n"); + /* create header and coordsys for vrml-file */ - /* create viewpoint */ - fprintf(fp2, " PerspectiveCamera {\n"); - fprintf(fp2, " position %7.3f %7.3f %7.3f\n", - (X_lay[0]+X_lay[1])/2,(ymax+ymin)/2, Zmax_lay[1]); - fprintf(fp2, " orientation 1 0 0 0\n"); - fprintf(fp2, " focalDistance 5\n"); - fprintf(fp2, " heightAngle 0.785398 }\n\n\n"); + /* create boundaries from object volume */ + volumedimension(&X_lay[1], &X_lay[0], &ymax, &ymin, &Zmax_lay[1], &Zmin_lay[0]); + cubes=(Zmax_lay[1]-Zmin_lay[0])/500; - /* create cameras */ - /* - fprintf(fp2, "#create cameras\n\n"); - for (i=0; i -22) */ { - fscanf (fp1,"%d\n", &anz1); - - line1 = (vector *) calloc (anz1, sizeof (vector)); - for (j=0;j -22) */ { - - - fprintf(fp2, " Separator {\n"); - fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f}\n", - line1[j].x1, line1[j].y1, line1[j].z1); - /* - fprintf(fp2, " Sphere { radius %3.2f } }\n", cubes ); - */ - fprintf(fp2, " Cube {width %3.2f height %3.2f depth %3.2f } }\n\n",cubes,cubes,cubes); - - - if (i= 0) { - - fprintf(fp2, " Separator {\n"); - fprintf(fp2, " Coordinate3 { point [\n"); - fprintf(fp2, " %7.3f %7.3f %7.3f,\n",line1[j].x1, line1[j].y1, line1[j].z1); - fprintf(fp2, " %7.3f %7.3f %7.3f, ] }\n", line2[m].x1, line2[m].y1, line2[m].z1); - fprintf(fp2, " IndexedLineSet { coordIndex [ 0, 1, -1] } }\n\n"); - - - /* cylinder/cube to mark link */ - /* - mx=(line1[j].x1+line2[m].x1)/2; - my=(line1[j].y1+line2[m].y1)/2; - mz=(line1[j].z1+line2[m].z1)/2; - dx=line1[j].x1-line2[m].x1; - dy=line1[j].y1-line2[m].y1; - dz=line1[j].z1-line2[m].z1; - du=sqrt(dx*dx+dy*dy); - dl=sqrt(dx*dx+dy*dy+dz*dz); - - rotz=0; - if(dy == 0.0) {rotz=-M_PI/2;} else {rotz = -atan(dx/dy);} - - rotx=0; - if(du == 0.0) {rotx=M_PI/2;} - - if(du != 0.0) { - if(dx>=0.0 && dy>=0.0 && dz> 0.0) {rotx = atan(dz/du);} - if(dx>=0.0 && dy< 0.0 && dz> 0.0) {rotx = -atan(dz/du);} - if(dx< 0.0 && dy> 0.0 && dz> 0.0) {rotx = atan(dz/du);} - if(dx< 0.0 && dy<=0.0 && dz> 0.0) {rotx = -atan(dz/du);} - if(dx>=0.0 && dy>=0.0 && dz< 0.0) {rotx = atan(dz/du);} - if(dx>=0.0 && dy< 0.0 && dz< 0.0) {rotx = -atan(dz/du);} - if(dx< 0.0 && dy> 0.0 && dz< 0.0) {rotx = atan(dz/du);} - if(dx< 0.0 && dy<=0.0 && dz< 0.0) {rotx = -atan(dz/du);} - } - - fprintf(fp2, " Separator {\n"); - fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f}\n",mx, my, mz); - fprintf(fp2, " Transform {rotation 0 0 1 %7.5f}\n",rotz); - fprintf(fp2, " Transform {rotation 1 0 0 %7.5f}\n",rotx); - - fprintf(fp2, " Cylinder { radius %3.2f height %3.2f } }\n\n",cubes/2, dl); - - fprintf(fp2, " Cube {width %3.2f height %3.2f depth %3.2f } }\n\n",cubes/1.5, dl,cubes/1.5); - */ - /* end of cylinder */ - } - } - } - fprintf(fp2, " }\n\n"); - fprintf(fp2, "# end of time step %d\n\n", i); - strcpy(val, ""); - free(line1); free(line2); - } /* end of sequence loop */ - - fprintf(fp2, "# trajectories finished\n"); - - fclose(fp2); - Tcl_Eval(interp, ".text delete 2"); - Tcl_Eval(interp, ".text insert 2 \"Tracks/Detections written to VRML-File: dt.wrl\""); - 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"); + if (i= 0) { + + fprintf(fp2, " Separator {\n"); + fprintf(fp2, " Coordinate3 { point [\n"); + fprintf(fp2, " %7.3f %7.3f %7.3f,\n",line1[j].x1, line1[j].y1, line1[j].z1); + fprintf(fp2, " %7.3f %7.3f %7.3f, ] }\n", line2[m].x1, line2[m].y1, line2[m].z1); + fprintf(fp2, " IndexedLineSet { coordIndex [ 0, 1, -1] } }\n\n"); + + /* cylinder/cube to mark link */ + /* + mx=(line1[j].x1+line2[m].x1)/2; + my=(line1[j].y1+line2[m].y1)/2; + mz=(line1[j].z1+line2[m].z1)/2; + dx=line1[j].x1-line2[m].x1; + dy=line1[j].y1-line2[m].y1; + dz=line1[j].z1-line2[m].z1; + du=sqrt(dx*dx+dy*dy); + dl=sqrt(dx*dx+dy*dy+dz*dz); + + rotz=0; + if(dy == 0.0) {rotz=-M_PI/2;} else {rotz = -atan(dx/dy);} + + rotx=0; + if(du == 0.0) {rotx=M_PI/2;} + + if(du != 0.0) { + if(dx>=0.0 && dy>=0.0 && dz> 0.0) {rotx = atan(dz/du);} + if(dx>=0.0 && dy< 0.0 && dz> 0.0) {rotx = -atan(dz/du);} + if(dx< 0.0 && dy> 0.0 && dz> 0.0) {rotx = atan(dz/du);} + if(dx< 0.0 && dy<=0.0 && dz> 0.0) {rotx = -atan(dz/du);} + if(dx>=0.0 && dy>=0.0 && dz< 0.0) {rotx = atan(dz/du);} + if(dx>=0.0 && dy< 0.0 && dz< 0.0) {rotx = -atan(dz/du);} + if(dx< 0.0 && dy> 0.0 && dz< 0.0) {rotx = atan(dz/du);} + if(dx< 0.0 && dy<=0.0 && dz< 0.0) {rotx = -atan(dz/du);} + } + + fprintf(fp2, " Separator {\n"); + fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f}\n",mx, my, mz); + fprintf(fp2, " Transform {rotation 0 0 1 %7.5f}\n",rotz); + fprintf(fp2, " Transform {rotation 1 0 0 %7.5f}\n",rotx); + + fprintf(fp2, " Cylinder { radius %3.2f height %3.2f } }\n\n",cubes/2, dl); + + fprintf(fp2, " Cube {width %3.2f height %3.2f depth %3.2f } }\n\n",cubes/1.5, dl,cubes/1.5); + */ + /* end of cylinder */ + } + } + } + fprintf(fp2, " }\n\n"); + fprintf(fp2, "# end of time step %d\n\n", i); + strcpy(val, ""); + free(line1); free(line2); + line1 = line2 = NULL; // added, ad holten 12-2012 + } /* end of sequence loop */ + + if (line1) free(line1); // added, ad holten 12-2012 + if (line1) free(line1); + + fprintf(fp2, "# trajectories finished\n"); + + fclose(fp2); + Tcl_Eval(interp, ".text delete 2"); + Tcl_Eval(interp, ".text insert 2 \"Tracks/Detections written to VRML-File: dt.wrl\""); + 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; + return TCL_OK; } diff --git a/PTV/src_c/vrml2.c b/PTV/src_c/vrml2.c index 40f0d56..b999669 100644 --- a/PTV/src_c/vrml2.c +++ b/PTV/src_c/vrml2.c @@ -1,529 +1,547 @@ /**************************************************************************** -Author/Copyright: Jochen Willneff +Author/Copyright: Jochen Willneff -Address: Institute of Geodesy and Photogrammetry - ETH - Hoenggerberg - CH - 8093 Zurich +Address: Institute of Geodesy and Photogrammetry + ETH - Hoenggerberg + CH - 8093 Zurich -Creation Date: end of 99 ... - -Description: creates VRML files from tracks and - detected particles - -Routines contained: vrmltracks_c, vrmldetections_c, vrmldettracks_c +Creation Date: end of 99 ... + +Description: creates VRML files from tracks and + detected particles + +Routines contained: vrmltracks_c, vrmldetections_c, vrmldettracks_c ****************************************************************************/ #include "ptv.h" int vrmltracks_c(ClientData clientData, Tcl_Interp* interp, int argc, const char** argv) { - int i, anz1, anz2, m, j, k; - FILE *fp1, *fp2; - char val[256]; - vector *line1, *line2; - double color, ymin=0, ymax=0, cubes; - double mx, my, mz, dx, dy, dz, du, dl, rotz, rotx; - - - /* open file for line elements */ - fp2 = fopen ("tracks.wrl", "w"); - - /* create header, background and coordsys for vrml-file */ - fprintf(fp2, "#VRML V2.0 utf8\n\n"); - fprintf(fp2, " Background { skyColor [ 1 1 1] }\n\n"); - - /* create boundaries from object volume */ - volumedimension(&X_lay[1], &X_lay[0], &ymax, &ymin, &Zmax_lay[1], &Zmin_lay[0]); - cubes=(Zmax_lay[1]-Zmin_lay[0])/200; - - /* create viewpoint */ - fprintf(fp2, " DEF V1 Viewpoint {\n"); - fprintf(fp2, " position %7.3f %7.3f %7.3f\n", - (X_lay[0]+X_lay[1])/2,(ymax+ymin)/2,Zmax_lay[1]); - fprintf(fp2, " orientation 1 0 0 0 \n"); - fprintf(fp2, " description \"Start\" }\n\n"); - - /* create object volume */ - fprintf(fp2, " Shape { geometry IndexedLineSet {\n"); - fprintf(fp2, " coord Coordinate { point [\n"); - fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0],ymin, Zmin_lay[0]); - fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0], ymax, Zmin_lay[0]); - fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[1], ymax, Zmin_lay[0]); - fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[1], ymin,Zmin_lay[0]); - fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0], ymin, Zmax_lay[1]); - fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0], ymax, Zmax_lay[1]); - fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[1], ymax,Zmax_lay[1]); - fprintf(fp2, " %5.2f %5.2f %5.2f, ] }\n", X_lay[1], ymin, Zmax_lay[1]); - fprintf(fp2, " coordIndex [ \n"); - fprintf(fp2, " 0, 1, 2, 3, 0, -1,\n"); - fprintf(fp2, " 0, 4, -1,\n"); - fprintf(fp2, " 1, 5, -1,\n"); - fprintf(fp2, " 2, 6, -1,\n"); - fprintf(fp2, " 3, 7, -1,\n"); - fprintf(fp2, " 4, 5, 6, 7, 4, -1 ]\n }"); - fprintf(fp2, " appearance Appearance {material Material { diffuseColor 0 0 1 } } }\n"); - - - fprintf(fp2, "\n\n# start trajectories\n\n"); - /* read trackfile from ptv and create vectorfield */ - - for (i=seq_first; i= 0) { - - fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f\n", - line1[j].x1, line1[j].y1, line1[j].z1); - fprintf(fp2, " children [\n"); - fprintf(fp2, " Shape { geometry Box { size %3.2f %3.2f %3.2f }\n appearance USE step%d }\n", - cubes, cubes, cubes, i); + int i, anz1, anz2, m, j; + FILE *fp1, *fp2; + char val[256]; + vector *line1, *line2; + double color, ymin=0, ymax=0, cubes; + double mx, my, mz, dx, dy, dz, du, dl, rotz, rotx; + + + /* open file for line elements */ + fp2 = fopen ("tracks.wrl", "w"); + + /* create header, background and coordsys for vrml-file */ + fprintf(fp2, "#VRML V2.0 utf8\n\n"); + fprintf(fp2, " Background { skyColor [ 1 1 1] }\n\n"); + + /* create boundaries from object volume */ + volumedimension(&X_lay[1], &X_lay[0], &ymax, &ymin, &Zmax_lay[1], &Zmin_lay[0]); + cubes=(Zmax_lay[1]-Zmin_lay[0])/200; + + /* create viewpoint */ + fprintf(fp2, " DEF V1 Viewpoint {\n"); + fprintf(fp2, " position %7.3f %7.3f %7.3f\n", + (X_lay[0]+X_lay[1])/2,(ymax+ymin)/2,Zmax_lay[1]); + fprintf(fp2, " orientation 1 0 0 0 \n"); + fprintf(fp2, " description \"Start\" }\n\n"); + + /* create object volume */ + fprintf(fp2, " Shape { geometry IndexedLineSet {\n"); + fprintf(fp2, " coord Coordinate { point [\n"); + fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0],ymin, Zmin_lay[0]); + fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0], ymax, Zmin_lay[0]); + fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[1], ymax, Zmin_lay[0]); + fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[1], ymin,Zmin_lay[0]); + fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0], ymin, Zmax_lay[1]); + fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0], ymax, Zmax_lay[1]); + fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[1], ymax,Zmax_lay[1]); + fprintf(fp2, " %5.2f %5.2f %5.2f, ] }\n", X_lay[1], ymin, Zmax_lay[1]); + fprintf(fp2, " coordIndex [ \n"); + fprintf(fp2, " 0, 1, 2, 3, 0, -1,\n"); + fprintf(fp2, " 0, 4, -1,\n"); + fprintf(fp2, " 1, 5, -1,\n"); + fprintf(fp2, " 2, 6, -1,\n"); + fprintf(fp2, " 3, 7, -1,\n"); + fprintf(fp2, " 4, 5, 6, 7, 4, -1 ]\n }"); + fprintf(fp2, " appearance Appearance {material Material { diffuseColor 0 0 1 } } }\n"); + + + fprintf(fp2, "\n\n# start trajectories\n\n"); + /* read trackfile from ptv and create vectorfield */ + + line1 = line2 = NULL; // added, ad holten 2012 + for (i=seq_first; i= 0) { + + fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f\n", + line1[j].x1, line1[j].y1, line1[j].z1); + fprintf(fp2, " children [\n"); + fprintf(fp2, " Shape { geometry Box { size %3.2f %3.2f %3.2f }\n appearance USE step%d }\n", + cubes, cubes, cubes, i); - /* create last cubes of ending trajectories */ + fprintf(fp2, " Shape { geometry Sphere { radius %3.2f }\n appearance USE step%d }\n", cubes, i); - if(line2[m].n <= 0) { + fprintf(fp2, " ] }\n"); + + /* create cubes of last time step */ + if(i ==seq_last-1) { + + fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f\n", + line2[m].x1, line2[m].y1, line2[m].z1); + fprintf(fp2, " children [\n"); + fprintf(fp2, " Shape { geometry Box { size %3.2f %3.2f %3.2f }\n appearance USE step%d }\n", + cubes, cubes, cubes, i); - fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f\n", - line2[m].x1, line2[m].y1, line2[m].z1); - fprintf(fp2, " children [\n"); - fprintf(fp2, " Shape { geometry Box { size %3.2f %3.2f %3.2f }\n appearance USE step%d }\n", - cubes, cubes, cubes, i ); + fprintf(fp2, " Shape { geometry Sphere { radius %3.2f }\n appearance USE step%d }\n", cubes, i); - fprintf(fp2, " Shape { geometry Sphere { radius %3.2f }\n appearance USE step%d }\n", cubes, i); + fprintf(fp2, " ] }\n"); + } - fprintf(fp2, " ] }\n"); - } - - mx=(line1[j].x1+line2[m].x1)/2; - my=(line1[j].y1+line2[m].y1)/2; - mz=(line1[j].z1+line2[m].z1)/2; - dx=line1[j].x1-line2[m].x1; - dy=line1[j].y1-line2[m].y1; - dz=line1[j].z1-line2[m].z1; - du=sqrt(dx*dx+dy*dy); - dl=sqrt(dx*dx+dy*dy+dz*dz); - - rotz=0; - if(dy == 0.0) {rotz=-M_PI/2;} else {rotz = -atan(dx/dy);} + /* create last cubes of ending trajectories */ + + if(line2[m].n <= 0) { + + fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f\n", + line2[m].x1, line2[m].y1, line2[m].z1); + fprintf(fp2, " children [\n"); + fprintf(fp2, " Shape { geometry Box { size %3.2f %3.2f %3.2f }\n appearance USE step%d }\n", + cubes, cubes, cubes, i ); + + fprintf(fp2, " Shape { geometry Sphere { radius %3.2f }\n appearance USE step%d }\n", cubes, i); + + fprintf(fp2, " ] }\n"); + } + + mx=(line1[j].x1+line2[m].x1)/2; + my=(line1[j].y1+line2[m].y1)/2; + mz=(line1[j].z1+line2[m].z1)/2; + dx=line1[j].x1-line2[m].x1; + dy=line1[j].y1-line2[m].y1; + dz=line1[j].z1-line2[m].z1; + du=sqrt(dx*dx+dy*dy); + dl=sqrt(dx*dx+dy*dy+dz*dz); + + rotz=0; + if(dy == 0.0) {rotz=-M_PI/2;} + else {rotz = -atan(dx/dy);} - rotx=0; - if(du == 0.0) {rotx=M_PI/2;} - - if(du != 0.0) { - if(dx>=0.0 && dy>=0.0 && dz> 0.0) {rotx = atan(dz/du);} - if(dx>=0.0 && dy< 0.0 && dz> 0.0) {rotx = -atan(dz/du);} - if(dx< 0.0 && dy> 0.0 && dz> 0.0) {rotx = atan(dz/du);} - if(dx< 0.0 && dy<=0.0 && dz> 0.0) {rotx = -atan(dz/du);} - if(dx>=0.0 && dy>=0.0 && dz< 0.0) {rotx = atan(dz/du);} - if(dx>=0.0 && dy< 0.0 && dz< 0.0) {rotx = -atan(dz/du);} - if(dx< 0.0 && dy> 0.0 && dz< 0.0) {rotx = atan(dz/du);} - if(dx< 0.0 && dy<=0.0 && dz< 0.0) {rotx = -atan(dz/du);} - } + rotx=0; + if(du == 0.0) {rotx=M_PI/2;} + + if(du != 0.0) { + if(dx>=0.0 && dy>=0.0 && dz> 0.0) {rotx = atan(dz/du);} + if(dx>=0.0 && dy< 0.0 && dz> 0.0) {rotx = -atan(dz/du);} + if(dx< 0.0 && dy> 0.0 && dz> 0.0) {rotx = atan(dz/du);} + if(dx< 0.0 && dy<=0.0 && dz> 0.0) {rotx = -atan(dz/du);} + if(dx>=0.0 && dy>=0.0 && dz< 0.0) {rotx = atan(dz/du);} + if(dx>=0.0 && dy< 0.0 && dz< 0.0) {rotx = -atan(dz/du);} + if(dx< 0.0 && dy> 0.0 && dz< 0.0) {rotx = atan(dz/du);} + if(dx< 0.0 && dy<=0.0 && dz< 0.0) {rotx = -atan(dz/du);} + } + fprintf(fp2, " Transform { translation %7.3f %7.3f %7.3f\n",mx, my, mz); + fprintf(fp2, " children [ Transform {rotation 0 0 1 %7.5f\n",rotz); + fprintf(fp2, " children [ Transform {rotation 1 0 0 %7.5f \n",rotx); + fprintf(fp2, " children [ Shape { geometry Cylinder { radius %3.2f height %3.2f } \n appearance USE step%d } ]\n",cubes/4, dl, i); + fprintf(fp2, " children [ Shape { geometry Box {size %3.2f %3.2f %3.2f } \n appearance USE step%d } ]\n\n",cubes/2, dl,cubes/2, i); + fprintf(fp2, " } ] } ] }\n"); - fprintf(fp2, " Transform { translation %7.3f %7.3f %7.3f\n",mx, my, mz); - fprintf(fp2, " children [ Transform {rotation 0 0 1 %7.5f\n",rotz); - fprintf(fp2, " children [ Transform {rotation 1 0 0 %7.5f \n",rotx); - fprintf(fp2, " children [ Shape { geometry Cylinder { radius %3.2f height %3.2f } \n appearance USE step%d } ]\n",cubes/4, dl, i); - fprintf(fp2, " children [ Shape { geometry Box {size %3.2f %3.2f %3.2f } \n appearance USE step%d } ]\n\n",cubes/2, dl,cubes/2, i); - fprintf(fp2, " } ] } ] }\n"); - - /* end of cylinder */ - + /* end of cylinder */ - fprintf(fp2, " Shape { geometry IndexedLineSet {\n"); - fprintf(fp2, " coord Coordinate { point [\n"); - fprintf(fp2, " %7.3f %7.3f %7.3f,\n",line1[j].x1, line1[j].y1, line1[j].z1); - fprintf(fp2, " %7.3f %7.3f %7.3f, ] }\n", line2[m].x1, line2[m].y1, line2[m].z1); - fprintf(fp2, " coordIndex [ 0, 1, -1] } \n appearance USE step%d}\n\n", i); - - } - } - - fprintf(fp2, "# end of time step %d\n\n", i); - strcpy(val, ""); - free(line1); free(line2); - } /* end of sequence loop */ - - fprintf(fp2, "# trajectories finished\n"); - - fclose(fp2); - Tcl_Eval(interp, ".text delete 2"); - Tcl_Eval(interp, ".text insert 2 \"Tracks written to VRML-File: tracks.wrl\""); - 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; + fprintf(fp2, " Shape { geometry IndexedLineSet {\n"); + fprintf(fp2, " coord Coordinate { point [\n"); + fprintf(fp2, " %7.3f %7.3f %7.3f,\n",line1[j].x1, line1[j].y1, line1[j].z1); + fprintf(fp2, " %7.3f %7.3f %7.3f, ] }\n", line2[m].x1, line2[m].y1, line2[m].z1); + fprintf(fp2, " coordIndex [ 0, 1, -1] } \n appearance USE step%d}\n\n", i); + } + } + + fprintf(fp2, "# end of time step %d\n\n", i); + strcpy(val, ""); + free(line1); free(line2); + line1 = line2 = NULL; // added, ad holten 2012 + } /* end of sequence loop */ + + if (line1) free(line1); + if (line2) free(line2); + + fprintf(fp2, "# trajectories finished\n"); + + fclose(fp2); + Tcl_Eval(interp, ".text delete 2"); + Tcl_Eval(interp, ".text insert 2 \"Tracks written to VRML-File: tracks.wrl\""); + 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; } int vrmldetections_c(ClientData clientData, Tcl_Interp* interp, int argc, const char** argv) { - int i, anz1, j, k, dumy; - FILE *fp1, *fp2; - char val[256]; - vector *line1; - double color, ymin=0, ymax=0, cubes; - - /* open file for line elements */ - fp2 = fopen ("detections.wrl", "w"); - - /* create header, background and coordsys for vrml-file */ - fprintf(fp2, "#VRML V2.0 utf8\n\n"); - fprintf(fp2, " Background { skyColor [ 1 1 1] }\n\n"); - - /* create boundaries from object volume */ - volumedimension(&X_lay[1], &X_lay[0], &ymax, &ymin, &Zmax_lay[1], &Zmin_lay[0]); - cubes=(Zmax_lay[1]-Zmin_lay[0])/600; - - /* create viewpoint */ - fprintf(fp2, " DEF V1 Viewpoint {\n"); - fprintf(fp2, " position %7.3f %7.3f %7.3f\n", - (X_lay[0]+X_lay[1])/2,(ymax+ymin)/2,Zmax_lay[1]); - fprintf(fp2, " orientation 1 0 0 0 \n"); - fprintf(fp2, " description \"Start\" }\n\n"); - - /* create object volume */ - fprintf(fp2, " Shape { geometry IndexedLineSet {\n"); - fprintf(fp2, " coord Coordinate { point [\n"); - fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0],ymin, Zmin_lay[0]); - fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0], ymax, Zmin_lay[0]); - fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[1], ymax, Zmin_lay[0]); - fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[1], ymin,Zmin_lay[0]); - fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0], ymin, Zmax_lay[1]); - fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0], ymax, Zmax_lay[1]); - fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[1], ymax,Zmax_lay[1]); - fprintf(fp2, " %5.2f %5.2f %5.2f, ] }\n", X_lay[1], ymin, Zmax_lay[1]); - fprintf(fp2, " coordIndex [ \n"); - fprintf(fp2, " 0, 1, 2, 3, 0, -1,\n"); - fprintf(fp2, " 0, 4, -1,\n"); - fprintf(fp2, " 1, 5, -1,\n"); - fprintf(fp2, " 2, 6, -1,\n"); - fprintf(fp2, " 3, 7, -1,\n"); - fprintf(fp2, " 4, 5, 6, 7, 4, -1 ]\n }"); - fprintf(fp2, " appearance Appearance {material Material { diffuseColor 0 0 1 } } }\n\n\n"); - - /* read trackfile from ptv and create particle positions */ - for (i=seq_first; i<=seq_last ;i++) - { - if (i < 10) sprintf (val, "res/rt_is.%1d", i); - else if (i < 100) sprintf (val, "res/rt_is.%2d", i); - else sprintf (val, "res/rt_is.%3d", i); - - printf("Create VRML, read file: %s\n", val); - fp1 = fopen (val, "r"); - - color = ((double)(i-seq_first))/((double)(seq_last+1-seq_first)); - - fscanf (fp1,"%d\n", &anz1); - line1 = (vector *) calloc (anz1, sizeof (vector)); - for (j=0;j -22) */ { - - - fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f\n", - line1[j].x1, line1[j].y1, line1[j].z1); - fprintf(fp2, " children [\n"); - /* - fprintf(fp2, " Shape { geometry Sphere { radius %3.2f } appearance USE step%d }\n", cubes, i ); - */ - fprintf(fp2, " Shape { geometry Box {size %3.2f %3.2f %3.2f } appearance USE step%d }\n",cubes, cubes, cubes, i); - fprintf(fp2, " ] }\n\n"); - - if (i= 0) { - - fprintf(fp2, " Shape { geometry IndexedLineSet {\n"); - fprintf(fp2, " coord Coordinate { point [\n"); - fprintf(fp2, " %7.3f %7.3f %7.3f,\n",line1[j].x1, line1[j].y1, line1[j].z1); - fprintf(fp2, " %7.3f %7.3f %7.3f, ] }\n", line2[m].x1, line2[m].y1, line2[m].z1); - fprintf(fp2, " coordIndex [ 0, 1, -1] } \n appearance USE step%d }\n\n", i); - - - /* cylinder/cube to mark link */ - /* - mx=(line1[j].x1+line2[m].x1)/2; - my=(line1[j].y1+line2[m].y1)/2; - mz=(line1[j].z1+line2[m].z1)/2; - dx=line1[j].x1-line2[m].x1; - dy=line1[j].y1-line2[m].y1; - dz=line1[j].z1-line2[m].z1; - du=sqrt(dx*dx+dy*dy); - dl=sqrt(dx*dx+dy*dy+dz*dz); - - rotz=0; - if(dy == 0.0) {rotz=-M_PI/2;} else {rotz = -atan(dx/dy);} - - rotx=0; - if(du == 0.0) {rotx=M_PI/2;} - - if(du != 0.0) { - if(dx>=0.0 && dy>=0.0 && dz> 0.0) {rotx = atan(dz/du);} - if(dx>=0.0 && dy< 0.0 && dz> 0.0) {rotx = -atan(dz/du);} - if(dx< 0.0 && dy> 0.0 && dz> 0.0) {rotx = atan(dz/du);} - if(dx< 0.0 && dy<=0.0 && dz> 0.0) {rotx = -atan(dz/du);} - if(dx>=0.0 && dy>=0.0 && dz< 0.0) {rotx = atan(dz/du);} - if(dx>=0.0 && dy< 0.0 && dz< 0.0) {rotx = -atan(dz/du);} - if(dx< 0.0 && dy> 0.0 && dz< 0.0) {rotx = atan(dz/du);} - if(dx< 0.0 && dy<=0.0 && dz< 0.0) {rotx = -atan(dz/du);} - } - - - fprintf(fp2, " Transform { translation %7.3f %7.3f %7.3f\n",mx, my, mz); - fprintf(fp2, " children [ Transform {rotation 0 0 1 %7.5f\n",rotz); - fprintf(fp2, " children [ Transform {rotation 1 0 0 %7.5f \n",rotx); - fprintf(fp2, " children [ Shape { geometry Cylinder { radius %3.2f height %3.2f } \n appearance USE step%d } ]\n",cubes/4, dl, i); - fprintf(fp2, " children [ Shape { geometry Box {size %3.2f %3.2f %3.2f } \n appearance USE step%d } ]\n\n",cubes/2, dl,cubes/2, i); - fprintf(fp2, " } ] } ] }\n"); - */ - /* end of cylinder */ - } - } - } - fprintf(fp2, "\n\n"); - fprintf(fp2, "# end of time step %d\n\n", i); - strcpy(val, ""); - free(line1); free(line2); - } /* end of sequence loop */ - - fprintf(fp2, "# trajectories finished\n"); - - fclose(fp2); - Tcl_Eval(interp, ".text delete 2"); - Tcl_Eval(interp, ".text insert 2 \"Tracks/Detections written to VRML-File: dt.wrl\""); - 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; + int i, anz1, anz2, m, j; + FILE *fp1, *fp2; + char val[256]; + vector *line1, *line2; + double color, ymin=0, ymax=0, cubes; + + /* open file for line elements */ + fp2 = fopen ("dt.wrl", "w"); + + /* create boundaries from object volume */ + volumedimension(&X_lay[1], &X_lay[0], &ymax, &ymin, &Zmax_lay[1], &Zmin_lay[0]); + cubes=(Zmax_lay[1]-Zmin_lay[0])/500; + cubes=(ymax-ymin)/800; + + /* create header, background and coordsys for vrml-file */ + fprintf(fp2, "#VRML V2.0 utf8\n\n"); + fprintf(fp2, " Background { skyColor [ 1 1 1] }\n\n"); + + /* create boundaries from object volume */ + volumedimension(&X_lay[1], &X_lay[0], &ymax, &ymin, &Zmax_lay[1], &Zmin_lay[0]); + cubes=(Zmax_lay[1]-Zmin_lay[0])/600; + + /* create viewpoint */ + fprintf(fp2, " DEF V1 Viewpoint {\n"); + fprintf(fp2, " position %7.3f %7.3f %7.3f\n", + (X_lay[0]+X_lay[1])/2,(ymax+ymin)/2,Zmax_lay[1]); + fprintf(fp2, " orientation 1 0 0 0 \n"); + fprintf(fp2, " description \"Start\" }\n\n"); + + /* create object volume */ + fprintf(fp2, " Shape { geometry IndexedLineSet {\n"); + fprintf(fp2, " coord Coordinate { point [\n"); + fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0],ymin, Zmin_lay[0]); + fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0], ymax, Zmin_lay[0]); + fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[1], ymax, Zmin_lay[0]); + fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[1], ymin,Zmin_lay[0]); + fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0], ymin, Zmax_lay[1]); + fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[0], ymax, Zmax_lay[1]); + fprintf(fp2, " %5.2f %5.2f %5.2f,\n", X_lay[1], ymax,Zmax_lay[1]); + fprintf(fp2, " %5.2f %5.2f %5.2f, ] }\n", X_lay[1], ymin, Zmax_lay[1]); + fprintf(fp2, " coordIndex [ \n"); + fprintf(fp2, " 0, 1, 2, 3, 0, -1,\n"); + fprintf(fp2, " 0, 4, -1,\n"); + fprintf(fp2, " 1, 5, -1,\n"); + fprintf(fp2, " 2, 6, -1,\n"); + fprintf(fp2, " 3, 7, -1,\n"); + fprintf(fp2, " 4, 5, 6, 7, 4, -1 ]\n }"); + fprintf(fp2, " appearance Appearance {material Material { diffuseColor 0 0 1 } } }\n"); + + fprintf(fp2, "\n\n# start trajectories\n\n"); + /* read trackfile from ptv and create vectorfield */ + + line1 = line2 = NULL; // added, ad holten 2012 + + 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); + + printf("Create VRML, read file: %s\n", val); + fp1 = fopen_rp (val); // replaced fopen(), ad holten 12-2012 + if (!fp1) break; + + color = ((double)(i-seq_first))/((double)(seq_last-1-seq_first)); + + fscanf (fp1,"%d\n", &anz1); + + line1 = (vector *) calloc (anz1, sizeof (vector)); + for (j=0;j -22) */ { + + fprintf(fp2, " Transform {translation %7.3f %7.3f %7.3f\n", + line1[j].x1, line1[j].y1, line1[j].z1); + fprintf(fp2, " children [\n"); + /* + fprintf(fp2, " Shape { geometry Sphere { radius %3.2f } appearance USE step%d }\n", cubes, i ); + */ + fprintf(fp2, " Shape { geometry Box {size %3.2f %3.2f %3.2f } appearance USE step%d }\n",cubes, cubes, cubes, i); + fprintf(fp2, " ] }\n\n"); + + if (i= 0) { + + fprintf(fp2, " Shape { geometry IndexedLineSet {\n"); + fprintf(fp2, " coord Coordinate { point [\n"); + fprintf(fp2, " %7.3f %7.3f %7.3f,\n",line1[j].x1, line1[j].y1, line1[j].z1); + fprintf(fp2, " %7.3f %7.3f %7.3f, ] }\n", line2[m].x1, line2[m].y1, line2[m].z1); + fprintf(fp2, " coordIndex [ 0, 1, -1] } \n appearance USE step%d }\n\n", i); + + + /* cylinder/cube to mark link */ + /* + mx=(line1[j].x1+line2[m].x1)/2; + my=(line1[j].y1+line2[m].y1)/2; + mz=(line1[j].z1+line2[m].z1)/2; + dx=line1[j].x1-line2[m].x1; + dy=line1[j].y1-line2[m].y1; + dz=line1[j].z1-line2[m].z1; + du=sqrt(dx*dx+dy*dy); + dl=sqrt(dx*dx+dy*dy+dz*dz); + + rotz=0; + if(dy == 0.0) {rotz=-M_PI/2;} else {rotz = -atan(dx/dy);} + + rotx=0; + if(du == 0.0) {rotx=M_PI/2;} + + if(du != 0.0) { + if(dx>=0.0 && dy>=0.0 && dz> 0.0) {rotx = atan(dz/du);} + if(dx>=0.0 && dy< 0.0 && dz> 0.0) {rotx = -atan(dz/du);} + if(dx< 0.0 && dy> 0.0 && dz> 0.0) {rotx = atan(dz/du);} + if(dx< 0.0 && dy<=0.0 && dz> 0.0) {rotx = -atan(dz/du);} + if(dx>=0.0 && dy>=0.0 && dz< 0.0) {rotx = atan(dz/du);} + if(dx>=0.0 && dy< 0.0 && dz< 0.0) {rotx = -atan(dz/du);} + if(dx< 0.0 && dy> 0.0 && dz< 0.0) {rotx = atan(dz/du);} + if(dx< 0.0 && dy<=0.0 && dz< 0.0) {rotx = -atan(dz/du);} + } + + + fprintf(fp2, " Transform { translation %7.3f %7.3f %7.3f\n",mx, my, mz); + fprintf(fp2, " children [ Transform {rotation 0 0 1 %7.5f\n",rotz); + fprintf(fp2, " children [ Transform {rotation 1 0 0 %7.5f \n",rotx); + fprintf(fp2, " children [ Shape { geometry Cylinder { radius %3.2f height %3.2f } \n appearance USE step%d } ]\n",cubes/4, dl, i); + fprintf(fp2, " children [ Shape { geometry Box {size %3.2f %3.2f %3.2f } \n appearance USE step%d } ]\n\n",cubes/2, dl,cubes/2, i); + fprintf(fp2, " } ] } ] }\n"); + */ + /* end of cylinder */ + } + } + } + fprintf(fp2, "\n\n"); + fprintf(fp2, "# end of time step %d\n\n", i); + strcpy(val, ""); + free(line1); free(line2); + line1 = line2 = NULL; // added, ad holten 12-2012 + } /* end of sequence loop */ + + if (line1) free(line1); // added, ad holten 12-2012 + if (line1) free(line1); + fprintf(fp2, "# trajectories finished\n"); + + fclose(fp2); + Tcl_Eval(interp, ".text delete 2"); + Tcl_Eval(interp, ".text insert 2 \"Tracks/Detections written to VRML-File: dt.wrl\""); + 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; }