END_TEST
START_TEST(dummy_distortion_round_trip)
{
/* This is the most basic distortion test: if there is no distortion,
a point distorted/corrected would come back as the same point, up to
floating point errors.
*/
double x=1., y=1.;
double xres, yres;
ap_52 ap = {0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0}; /* no distortion */
distort_brown_affin (x, y, ap, &xres, &yres);
correct_brown_affin (xres, yres, ap, &xres, &yres);
ck_assert_msg( fabs(xres - x) < EPS &&
fabs(yres - y) < EPS,
"Expected %f, %f, but got %f %f\n", x, y, xres, yres);
}
END_TEST
START_TEST(radial_distortion_round_trip)
{
/* Less basic distortion test: with radial distortion, a point
distorted/corrected would come back as the same point, up to floating
point errors and an error from the short iteration.
*/
double x=1., y=1.;
double xres, yres;
double iter_eps = 1e-2; /* Verified manually with calculator */
/* huge radial distortion */
ap_52 ap = {0.05, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0};
distort_brown_affin (x, y, ap, &xres, &yres);
correct_brown_affin (xres, yres, ap, &xres, &yres);
ck_assert_msg( fabs(xres - x) < iter_eps &&
fabs(yres - y) < iter_eps,
"Expected %f, %f, but got %f %f\n", x, y, xres, yres);
}
END_TEST
START_TEST(shear_round_trip)
{
/* input */
double x = -1.0; // [mm]
double y = 10.0; // [mm]
ap_52 ap = {0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0}; // affine parameters, see calibration
/* output */
double xp, yp, x1, y1;
distort_brown_affin (x, y, ap, &xp, &yp);
correct_brown_affin (xp, yp, ap, &x1, &y1);
ck_assert_msg( fabs(x1 - x) < EPS &&
fabs(y1 - y) < EPS,
"Expected %f, %f, but got %f %f\n", x,y,x1,y1);
}
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 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;
}
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) && (xa<xmax) && (yb>ymin) && (ya<ymax)) { /* sensor area */
/* binarized search for start point of candidate search */
for (j0=num/2, dj=num/4; dj>1; 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<num; j++) { /* candidate search */
if (crd[j].x > xb+tol_band_width) return; /* finish search */
// 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)
{
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 ("- - -");
}
else *count = -1; /* out of sensor area */
}
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)
// 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) { temp = xa; xa = xb; xb = temp; }
if (ya > yb) { temp = ya; ya = yb; yb = temp; }
if ((xb>xmin) && (xa<xmax) && (yb>ymin) && (ya<ymax)) { /* sensor area */
/* binarized search for start point of candidate search */
for (j0=num/2, dj=num/4; dj>1; 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<num; j++) { /* candidate search */
if (crd[j].x > xb+tol_band_width) return; /* finish search */
// 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)
{
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");
}
}
}
}
}
}
else *count = -1; /* out of sensor area */
}
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 = -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 (xa > xb) { temp = xa; xa = xb; xb = temp; } /* sort search window */
if (ya > yb) { temp = ya; ya = yb; yb = temp; }
if ((xb>xmin) && (xa<xmax) && (yb>ymin) && (ya<ymax)) { /* sensor area */
/* 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<num; j++) { /* candidate search */
if (crd[j].x > 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 */
}
int calibration_proc_c (/*ClientData clientData, Tcl_Interp* interp,*/ int argc, const char** argv)
{
int i, j, sel, i_img, k, n, sup;
int intx1, inty1, intx2, inty2;
coord_2d apfig1[11][11]; /* regular grid for ap figures */
coord_2d apfig2[11][11]; /* ap figures */
coord_3d fix4[4]; /* object points for preorientation */
coord_2d crd0[4][4]; /* image points for preorientation */
char filename[256], val[256];
const char *valp;
//Tk_PhotoHandle img_handle;
//Tk_PhotoImageBlock img_block;
/* read support of unsharp mask */
fp1 = fopen ("parameters/unsharp_mask.par", "r");
if (! fp1) sup = 12;
else { fscanf (fp1, "%d\n", &sup); fclose (fp1); }
/* Get Selection value from TclTk */
// ChrisB: what does this do?? Set a value......
//valp = Tcl_GetVar(interp, "sel", TCL_GLOBAL_ONLY);
//sel = atoi (valp);
sel = 1; // set a value....
switch (sel)
{
case 1: /* read calibration parameter file */
fp1 = fopen_r ("parameters/cal_ori.par");
fscanf (fp1,"%s\n", fixp_name);
for (i=0; i<4; i++)
{
fscanf (fp1, "%s\n", img_name[i]);
fscanf (fp1, "%s\n", img_ori0[i]);
}
fscanf (fpp, "%d\n", &tiff_flag);
fscanf (fp1, "%d\n", &chfield);
fclose (fp1);
/* create file names */
for (i=0; i<n_img; i++)
{
strcpy (img_ori[i], img_name[i]);
strcat (img_ori[i], ".ori");
strcpy (img_addpar0[i], img_name[i]);
strcat (img_addpar0[i], ".addpar0");
strcpy (img_addpar[i], img_name[i]);
strcat (img_addpar[i], ".addpar");
strcpy (img_hp_name[i], img_name[i]);
strcat (img_hp_name[i], "_hp");
}
for (i=0; i<n_img; i++)
{
zoom_x[i] = imx/2, zoom_y[i] = imy/2, zoom_f[i] = 1;
read_image (/*interp,*/ img_name[i], img[i]);
sprintf(val, "camcanvas %d", i+1);
//Tcl_Eval(interp, val);
//img_handle = Tk_FindPhoto( interp, "temp");
//Tk_PhotoGetImage (img_handle, &img_block);
//tclimg2cimg (interp, img[i], &img_block);
sprintf(val, "newimage %d", i+1);
//Tcl_Eval(interp, val);
}
break;
case 2: puts ("Detection procedure"); strcpy(val,"");
/* Highpass Filtering */
pre_processing_c (/*clientData, interp,*/ argc, argv);
/* reset zoom values */
for (i=0; i<n_img; i++)
{
zoom_x[i] = imx/2; zoom_y[i] = imy/2; zoom_f[i] = 1;
}
/* copy images because the target recognition
will set greyvalues to zero */
for (i=0; i<n_img; i++)
{
copy_images (img[i], img0[i]);
}
/* target recognition */
for (i=0; i<n_img; i++)
{
targ_rec (/*interp,*/ img[i], img0[i], "parameters/detect_plate.par",
0, imx, 1, imy, pix[i], i, &num[i]);
sprintf (buf,"image %d: %d, ", i+1, num[i]);
strcat(val, buf);
if (num[i] > nmax) exit (1);
}
/* save pixel coord as approx. for template matching */
if (examine) for (i=0; i<n_img; i++)
{
sprintf (filename, "%s_pix", img_name[i]);
fp1 = fopen (filename, "w");
for (j=0; j<num[i]; j++)
fprintf (fp1, "%4d %8.3f %8.3f\n",
pix[i][j].pnr, pix[i][j].x, pix[i][j].y);
fclose (fp1);
}
sprintf(buf,"Number of detected targets, interaction enabled");
//Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY);
//Tcl_Eval(interp, ".text delete 2");
//Tcl_Eval(interp, ".text insert 2 $tbuf");
//Tcl_SetVar(interp, "tbuf", val, TCL_GLOBAL_ONLY);
//Tcl_Eval(interp, ".text delete 3");
//Tcl_Eval(interp, ".text insert 3 $tbuf");
break;
case 3: pp1=0; pp2=0; pp3=0; pp4=0;
for (i=0; i<n_img; i++)
{
sprintf (buf, "%d targets remain", num[i]);
puts (buf);
}
fp1 = fopen_r ("parameters/man_ori.par");
for (i=0; i<n_img; i++)
{
fscanf (fp1, "%d %d %d %d\n", &nr[i][0], &nr[i][1], &nr[i][2], &nr[i][3]);
}
fclose (fp1);
for (i=0; i<n_img; i++)
{
sprintf(val, "measure %d %d %d %d %d", nr[i][0], nr[i][1], nr[i][2], nr[i][3], i+1);
//Tcl_Eval(interp, val);
#if 0
// ChrisB: do we need this?
valp = Tcl_GetVar(interp, "px0", TCL_GLOBAL_ONLY);
pix0[i][0].x = atoi (valp);
valp = Tcl_GetVar(interp, "py0", TCL_GLOBAL_ONLY);
pix0[i][0].y = atoi (valp);
valp = Tcl_GetVar(interp, "px1", TCL_GLOBAL_ONLY);
pix0[i][1].x = atoi (valp);
valp = Tcl_GetVar(interp, "py1", TCL_GLOBAL_ONLY);
pix0[i][1].y = atoi (valp);
valp = Tcl_GetVar(interp, "px2", TCL_GLOBAL_ONLY);
pix0[i][2].x = atoi (valp);
valp = Tcl_GetVar(interp, "py2", TCL_GLOBAL_ONLY);
pix0[i][2].y = atoi (valp);
valp = Tcl_GetVar(interp, "px3", TCL_GLOBAL_ONLY);
pix0[i][3].x = atoi (valp);
valp = Tcl_GetVar(interp, "py3", TCL_GLOBAL_ONLY);
pix0[i][3].y = atoi (valp);
#endif
}
/* write measured coordinates to file for next trial */
fp1 = fopen ("man_ori.dat", "w");
for (i=0; i<n_img; i++)
for (j=0; j<4; j++)
fprintf (fp1, "%f %f\n", pix0[i][j].x, pix0[i][j].y);
fclose (fp1);
break;
case 4: /* read pixel coordinates of older pre-orientation */
/* read point numbers of pre-clicked points */
fp1 = fopen_r ("parameters/man_ori.par");
for (i=0; i<n_img; i++)
{
fscanf (fp1, "%d %d %d %d\n",
&nr[i][0], &nr[i][1], &nr[i][2], &nr[i][3]);
}
fclose (fp1);
/* read coordinates of pre-clicked points */
fp1 = fopen ("man_ori.dat", "r");
if (! fp1) break;
for (i_img=0; i_img<n_img; i_img++) for (i=0; i<4; i++)
{
#if 0
fscanf (fp1, "%lf %lf\n",
&pix0[i_img][i].x, &pix0[i_img][i].y);
drawcross (interp, (int) pix0[i_img][i].x,
(int) pix0[i_img][i].y, cr_sz+2, i_img, "red");
draw_pnr (interp, (int) pix0[i_img][i].x, (int) pix0[i_img][i].y,
nr[i_img][i], i_img, "red");
#endif
}
fclose (fp1);
break;
case 5: puts ("Sort grid points");
for (i=0; i<n_img; i++)
{
/* read control point coordinates for man_ori points */
fp1 = fopen_r (fixp_name);
k = 0;
while ( fscanf (fp1, "%d %lf %lf %lf", &fix[k].pnr,
&fix[k].x, &fix[k].y, &fix[k].z) != EOF) k++;
fclose (fp1);
nfix = k;
/* take clicked points from control point data set */
for (j=0; j<4; j++) for (k=0; k<nfix; k++)
{
if (fix[k].pnr == nr[i][j]) fix4[j] = fix[k];
}
/* get approx for orientation and ap */
read_ori (&Ex[i], &I[i], img_ori0[i]);
fp1 = fopen (img_addpar0[i], "r");
if (! fp1) fp1 = fopen ("addpar.raw", "r");
if (fp1) {
fscanf (fp1, "%lf %lf %lf %lf %lf %lf %lf",
&ap[i].k1,&ap[i].k2,&ap[i].k3,
&ap[i].p1,&ap[i].p2,
&ap[i].scx,&ap[i].she);
fclose (fp1);} else {
printf("no addpar.raw\n");
ap[i].k1=ap[i].k2=ap[i].k3=ap[i].p1=ap[i].p2=ap[i].she=0.0;
ap[i].scx=1.0;
}
/* transform clicked points */
for (j=0; j<4; j++)
{
pixel_to_metric (pix0[i][j].x, pix0[i][j].y,
imx,imy, pix_x, pix_y,
&crd0[i][j].x, &crd0[i][j].y,
chfield);
correct_brown_affin (crd0[i][j].x, crd0[i][j].y, ap[i],
&crd0[i][j].x, &crd0[i][j].y);
}
/* raw orientation with 4 points */
raw_orient (Ex[i], I[i], ap[i], mmp, 4, fix4, crd0[i], &Ex[i]);
sprintf (filename, "raw%d.ori", i);
write_ori (Ex[i], I[i], filename);
/* sorting of detected points by back-projection */
sortgrid_man (/*interp,*/ Ex[i], I[i], ap[i], mmp,
imx,imy, pix_x,pix_y,
nfix, fix, num[i], pix[i], chfield, i);
/* adapt # of detected points */
num[i] = nfix;
for (j=0; j<nfix; j++)
{
#if 0
if (pix[i][j].pnr < 0) continue;
intx1 = (int) pix[i][j].x ;
inty1 = (int) pix[i][j].y ;
drawcross (interp, intx1, inty1, cr_sz, i, "white");
draw_pnr (interp, intx1, inty1, fix[j].pnr, i, "white");
#endif
}
}
/* dump dataset for rdb */
if (examine == 4)
{
/* create filename for dumped dataset */
sprintf (filename, "dump_for_rdb");
fp1 = fopen (filename, "w");
/* write # of points to file */
fprintf (fp1, "%d\n", nfix);
/* write point and image coord to file */
for (i=0; i<nfix; i++)
{
fprintf (fp1, "%4d %10.3f %10.3f %10.3f %d ",
fix[i].pnr, fix[i].x, fix[i].y, fix[i].z, 0);
for (i_img=0; i_img<n_img; i_img++)
{
if (pix[i_img][i].pnr >= 0)
{
/* transform pixel coord to metric */
pixel_to_metric (pix[i_img][i].x,
pix[i_img][i].y, imx,imy, pix_x, pix_y,
&crd[i_img][i].x, &crd[i_img][i].y,
chfield);
fprintf (fp1, "%4d %8.5f %8.5f ",
pix[i_img][i].pnr,
crd[i_img][i].x, crd[i_img][i].y);
}
else
{
fprintf (fp1, "%4d %8.5f %8.5f ",
pix[i_img][i].pnr, 0.0, 0.0);
}
}
fprintf (fp1, "\n");
}
fclose (fp1);
printf ("dataset dumped into %s\n", filename);
}
break;
case 6: puts ("Orientation"); strcpy(buf, "");
for (i_img=0; i_img<n_img; i_img++)
{
for (i=0; i<nfix ; i++)
{
pixel_to_metric (pix[i_img][i].x, pix[i_img][i].y,
imx,imy, pix_x, pix_y,
&crd[i_img][i].x, &crd[i_img][i].y,
chfield);
crd[i_img][i].pnr = pix[i_img][i].pnr;
}
/* save data for special use of resection routine */
if (examine == 4)
{
printf ("try write resection data to disk\n");
/* point coordinates */
sprintf (filename, "resect_%s.fix", img_name[i_img]);
write_ori (Ex[i_img], I[i_img], img_ori[i_img]);
fp1 = fopen (filename, "w");
for (i=0; i<nfix; i++)
fprintf (fp1, "%3d %10.5f %10.5f %10.5f\n",
fix[i].pnr, fix[i].x, fix[i].y, fix[i].z);
fclose (fp1);
/* metric image coordinates */
sprintf (filename, "resect_%s.crd", img_name[i_img]);
fp1 = fopen (filename, "w");
for (i=0; i<nfix; i++)
fprintf (fp1,
"%3d %9.5f %9.5f\n", crd[i_img][i].pnr,
crd[i_img][i].x, crd[i_img][i].y);
fclose (fp1);
/* orientation and calibration approx data */
write_ori (Ex[i_img], I[i_img], "resect.ori0");
fp1 = fopen ("resect.ap0", "w");
fprintf (fp1, "%f %f %f %f %f %f %f",
ap[i_img].k1, ap[i_img].k2, ap[i_img].k3,
ap[i_img].p1, ap[i_img].p2,
ap[i_img].scx, ap[i_img].she);
fclose (fp1);
printf ("resection data written to disk\n");
}
/* resection routine */
/* ================= */
if (examine != 4)
orient (/*interp,*/ Ex[i_img], I[i_img], ap[i_img], mmp,
nfix, fix, crd[i_img],
&Ex[i_img], &I[i_img], &ap[i_img], i_img);
/* ================= */
/* resection with dumped datasets */
if (examine == 4)
{
printf("Resection with dumped datasets? (y/n)");
scanf("%s",buf);
if (buf[0] != 'y') continue;
strcpy (buf, "");
/* read calibration frame datasets */
for (n=0, nfix=0, dump_for_rdb=0; n<100; n++)
{
sprintf (filename, "resect.fix%d", n);
fp1 = fopen (filename, "r");
if (! fp1) continue;
printf("reading file: %s\n", filename);
printf ("reading dumped resect data #%d\n", n);
k = 0;
while ( fscanf (fp1, "%d %lf %lf %lf",
&fix[nfix+k].pnr, &fix[nfix+k].x,
&fix[nfix+k].y, &fix[nfix+k].z)
!= EOF) k++;
fclose (fp1);
/* read metric image coordinates */
sprintf (filename, "resect_%d.crd%d", i_img, n);
printf("reading file: %s\n", filename);
fp1 = fopen (filename, "r");
for (i=nfix; i<nfix+k; i++)
fscanf (fp1, "%d %lf %lf",
&crd[i_img][i].pnr,
&crd[i_img][i].x, &crd[i_img][i].y);
nfix += k;
}
/* resection */
orient (/*interp,*/ Ex[i_img], I[i_img], ap[i_img], mmp,
nfix, fix, crd[i_img],
&Ex[i_img], &I[i_img], &ap[i_img], i_img);
}
/* save orientation and additional parameters */
write_ori (Ex[i_img], I[i_img], img_ori[i_img]);
fp1 = fopen (img_addpar[i_img], "w");
fprintf (fp1, "%f %f %f %f %f %f %f",
ap[i_img].k1, ap[i_img].k2, ap[i_img].k3,
ap[i_img].p1, ap[i_img].p2,
ap[i_img].scx, ap[i_img].she);
fclose (fp1);
}
//Tcl_Eval(interp, ".text delete 3");
//Tcl_Eval(interp, ".text delete 1");
//Tcl_Eval(interp, ".text insert 1 \"Orientation and self calibration \"");
//Tcl_Eval(interp, ".text delete 2");
//Tcl_Eval(interp, ".text insert 2 \"...done, sigma0 for each image -> \"");
//Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY);
//Tcl_Eval(interp, ".text insert 3 $tbuf");
break;
case 7: checkpoint_proc (/*interp*/);
#if 0
sprintf(val,"blue: planimetry, yellow: height");
Tcl_SetVar(interp, "tbuf", val, TCL_GLOBAL_ONLY);
Tcl_Eval(interp, ".text delete 2");
Tcl_Eval(interp, ".text insert 2 $tbuf");
Tcl_SetVar(interp, "tbuf", buf, TCL_GLOBAL_ONLY);
Tcl_Eval(interp, ".text delete 3");
Tcl_Eval(interp, ".text insert 3 $tbuf");
#endif
break;
case 8: /* draw additional parameter figures */
//Tcl_Eval(interp, "clearcam");
/* read orientation and additional parameters */
for (i=0; i<n_img; i++) read_ori (&Ex[i], &I[i], img_ori[i]);
for (i=0; i<n_img; i++)
{
fp1 = fopen_r (img_addpar[i]);
fscanf (fp1,"%lf %lf %lf %lf %lf %lf %lf",
&ap[i].k1, &ap[i].k2, &ap[i].k3,
&ap[i].p1, &ap[i].p2, &ap[i].scx, &ap[i].she);
fclose (fp1);
}
for (i_img=0; i_img<n_img; i_img++)
{
/* create undistorted grid */
for (i=0; i<11; i++) for (j=0; j<11; j++)
{
apfig1[i][j].x = i * imx/10;
apfig1[i][j].y = j * imy/10;
}
/* draw undistorted grid */
for (i=0; i<10; i++) for (j=0; j<10; j++)
{
intx1 = (int) apfig1[i][j].x;
inty1 = (int) apfig1[i][j].y;
intx2 = (int) apfig1[i+1][j].x;
inty2 = (int) apfig1[i][j+1].y;
//drawvector (interp, intx1, inty1, intx2, inty1, 1, i_img, "black");
//drawvector (interp, intx1, inty1, intx1, inty2, 1, i_img, "black");
}
for (j=0; j<10; j++)
{
intx1 = (int) apfig1[10][j].x;
inty1 = (int) apfig1[10][j].y;
inty2 = (int) apfig1[10][j+1].y;
//drawvector (interp, intx1, inty1, intx1, inty2, 1, i_img, "black");
}
for (i=0; i<10; i++)
{
intx1 = (int) apfig1[i][10].x;
inty1 = (int) apfig1[i][10].y;
intx2 = (int) apfig1[i+1][10].x;
//drawvector (interp, intx1, inty1, intx2, inty1, 1, i_img, "black");
}
/* distort grid */
for (i=0; i<11; i++) for (j=0; j<11; j++)
{
/* transform to metric, distort and re-transform */
pixel_to_metric (apfig1[i][j].x, apfig1[i][j].y,
imx,imy, pix_x,pix_y,
&apfig2[i][j].x, &apfig2[i][j].y, chfield);
distort_brown_affin (apfig2[i][j].x, apfig2[i][j].y,
ap[i_img], &apfig2[i][j].x, &apfig2[i][j].y);
metric_to_pixel (apfig2[i][j].x, apfig2[i][j].y,
imx,imy, pix_x,pix_y,
&apfig2[i][j].x, &apfig2[i][j].y, chfield);
/* exaggerate distortion by factor 5 */
apfig2[i][j].x = 5*apfig2[i][j].x - 4*apfig1[i][j].x;
apfig2[i][j].y = 5*apfig2[i][j].y - 4*apfig1[i][j].y;
}
/* draw distorted grid */
for (i=0; i<10; i++) for (j=0; j<10; j++)
{
intx1 = (int) apfig2[i][j].x;
inty1 = (int) apfig2[i][j].y;
intx2 = (int) apfig2[i+1][j].x;
inty2 = (int) apfig2[i+1][j].y;
//drawvector (interp, intx1, inty1, intx2, inty2, 3, i_img, "magenta");
intx2 = (int) apfig2[i][j+1].x ;
inty2 = (int) apfig2[i][j+1].y ;
//drawvector (interp, intx1, inty1, intx2, inty2, 3, i_img, "magenta");
}
for (j=0; j<10; j++)
{
intx1 = (int) apfig2[10][j].x;
inty1 = (int) apfig2[10][j].y;
intx2 = (int) apfig2[10][j+1].x;
inty2 = (int) apfig2[10][j+1].y;
//drawvector (interp, intx1, inty1, intx2, inty2, 3, i_img, "magenta");
}
for (i=0; i<10; i++)
{
intx1 = (int) apfig2[i][10].x;
inty1 = (int) apfig2[i][10].y;
intx2 = (int) apfig2[i+1][10].x;
inty2 = (int) apfig2[i+1][10].y ;
//drawvector (interp, intx1, inty1, intx2, inty2, 3, i_img, "magenta");
}
}
break;
}
return TCL_OK;
}
int correspondences_proc_c()
{
int i, i_img;
double x,y;
if( verbose )puts ("\nTransformation to metric coordinates\n");
/* rearrange point numbers after manual deletion of points */
for (i_img=0; i_img<n_img; i_img++)
for (i=0; i<num[i_img]; i++) pix[i_img][i].pnr = i;
/* transformations pixel coordinates -> metric coordinates */
/* transformations metric coordinates -> corrected metric coordinates */
for (i_img=0; i_img<n_img; i_img++)
{
for (i=0; i<num[i_img]; i++)
{
pixel_to_metric (pix[i_img][i].x, pix[i_img][i].y,
imx,imy, pix_x, pix_y,
&crd[i_img][i].x, &crd[i_img][i].y, chfield);
crd[i_img][i].pnr = pix[i_img][i].pnr;
x = crd[i_img][i].x - I[i_img].xh;
y = crd[i_img][i].y - I[i_img].yh;
correct_brown_affin (x, y, ap[i_img], &geo[i_img][i].x, &geo[i_img][i].y);
geo[i_img][i].pnr = crd[i_img][i].pnr;
}
}
/* sort coordinates for binary search in correspondences_proc */
for (i_img=0; i_img<n_img; i_img++)
{
quicksort_coord2d_x (geo[i_img], num[i_img]);
}
/* read illuminated layer data for demo version */
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]);
/* read criteria for accepted match (shape, tolerance), globals */
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);
/* init multimedia radial displacement LUTs */
/* ======================================== */
if ( !mmp.lut && (mmp.n1 != 1 || mmp.n2[0] != 1 || mmp.n3 != 1))
{
if( verbose )puts ("Init multimedia displacement LUTs");
for (i_img=0; i_img<n_img; i_img++) init_mmLUT(i_img);
mmp.lut = 1;
}
correspondences_4 (/* interp*/);
/* --------------- */
/* save pixel coords for tracking */
for (i_img=0; i_img<n_img; i_img++)
{
if( useCompression ) {
// ChrisB: write compressed output:
sprintf (filename, "%s_targets.gz", img_name[i_img]);
fgz = gzopen (filename, "wb");
gzprintf(fgz,"%d\n", num[i_img]);
for (i=0; i<num[i_img]; i++)
{
gzprintf (fgz, "%4d %9.4f %9.4f %5d %5d %5d %5d %5d\n", pix[i_img][i].pnr, pix[i_img][i].x,
pix[i_img][i].y, pix[i_img][i].n ,
pix[i_img][i].nx ,pix[i_img][i].ny,
pix[i_img][i].sumg, pix[i_img][i].tnr);
}
gzclose( fgz );
} else {
// ChrisB: write uncompressed output:
sprintf (filename, "%s_targets", img_name[i_img]);
fp1 = fopen (filename, "w");
fprintf(fp1,"%d\n", num[i_img]);
for (i=0; i<num[i_img]; i++) {
fprintf (fp1, "%4d %9.4f %9.4f %5d %5d %5d %5d %5d\n", pix[i_img][i].pnr, pix[i_img][i].x,
pix[i_img][i].y, pix[i_img][i].n ,
pix[i_img][i].nx ,pix[i_img][i].ny,
pix[i_img][i].sumg, pix[i_img][i].tnr);
}
fclose (fp1);
}
}
return TCL_OK;
}
int mouse_proc_c (int click_x, int click_y, int kind, int num_image, volume_par *vpar, \
control_par *cpar){
int i, j, n, zf;
double x, y;
double xa12, xb12, ya12, yb12;
int k, pt1, intx1, inty1, count, intx2, inty2, pt2;
candidate cand[maxcand];
printf("entered mouse_proc_c \n");
if (zoom_f[0] == 1) {zf = 2;} else { zf = zoom_f[0];}
n=num_image;
if (examine) zf *= 2;
switch (kind)
{
/* -------------------------- MIDDLE MOUSE BUTTON ---------------------------------- */
case 3: /* generate epipolar line segments */
/* get geometric coordinates of nearest point in img[n] */
x = (float) (click_x - cpar->imx/2)/zoom_f[n] + zoom_x[n];
y = (float) (click_y - cpar->imy/2)/zoom_f[n] + zoom_y[n];
pixel_to_metric (&x, &y, x,y, cpar);
x -= I[n].xh; y -= I[n].yh;
correct_brown_affin (x, y, ap[n], &x, &y);
k = nearest_neighbour_geo (geo[n], num[n], x, y, 0.05);
if (k == -999){
printf ("No point near click coord! Click again! \n");
return -1;
}
pt1 = geo[n][k].pnr;
intx1 = (int) ( cpar->imx/2 + zoom_f[n] * (pix[n][pt1].x-zoom_x[n]));
inty1 = (int) ( cpar->imy/2 + zoom_f[n] * (pix[n][pt1].y-zoom_y[n]));
rclick_points_intx1=intx1;
rclick_points_inty1=inty1;
//drawcross (interp, intx1, inty1, cr_sz+2, n, "BlueViolet");
printf ( "pt1,nx,ny,n,sumg: %d %d %d %d %d\n", pt1, pix[n][pt1].nx, pix[n][pt1].ny,
pix[n][pt1].n, pix[n][pt1].sumg);
for (i = 0; i < cpar->num_cams; i++) if (i != n) {
/* calculate epipolar band in img[i] */
epi_mm (i, geo[n][k].x,geo[n][k].y,
Ex[n],I[n], G[n], Ex[i],I[i], G[i], *(cpar->mm), vpar,
&xa12, &ya12, &xb12, &yb12);
/* search candidate in img[i] */
printf("\ncandidates in img: %d\n", i);
find_candidate_plus_msg (geo[i], pix[i], num[i],
xa12, ya12, xb12, yb12,
pix[n][pt1].n, pix[n][pt1].nx, pix[n][pt1].ny,
pix[n][pt1].sumg, cand, &count, i, vpar, cpar);
distort_brown_affin (xa12,ya12, ap[i], &xa12,&ya12);
distort_brown_affin (xb12,yb12, ap[i], &xb12,&yb12);
xa12 += I[i].xh; ya12 += I[i].yh;
xb12 += I[i].xh; yb12 += I[i].yh;
metric_to_pixel(&xa12, &ya12, xa12, ya12, cpar);
metric_to_pixel(&xb12, &yb12, xb12, yb12, cpar);
intx1 = (int) ( cpar->imx/2 + zoom_f[i] * (xa12 - zoom_x[i]));
inty1 = (int) ( cpar->imy/2 + zoom_f[i] * (ya12 - zoom_y[i]));
intx2 = (int) ( cpar->imx/2 + zoom_f[i] * (xb12 - zoom_x[i]));
inty2 = (int) ( cpar->imy/2 + zoom_f[i] * (yb12 - zoom_y[i]));
rclick_intx1[i]=intx1;
rclick_inty1[i]=inty1;
rclick_intx2[i]=intx2;
rclick_inty2[i]=inty2;
// drawvector ( interp, intx1, inty1, intx2, inty2, 1, i, val);
rclick_count[i]=count;
for (j=0; j<count; j++)
{
pt2 = cand[j].pnr;
intx2 = (int) ( cpar->imx/2 + zoom_f[i] * (pix[i][pt2].x - zoom_x[i]));
inty2 = (int) ( cpar->imy/2 + zoom_f[i] * (pix[i][pt2].y - zoom_y[i]));
rclick_points_x1[i][j]=intx2;
rclick_points_y1[i][j]=inty2;
//drawcross (interp, intx2, inty2, cr_sz+2, i, "orange");
}
}
break;
case 4: /* delete points, which should not be used for orientation */
j = kill_in_list (n, num[n], click_x, click_y);
if (j != -1)
{
num[n] -= 1;
printf ("point %d deleted", j);
}
else {
printf ("no point near click coord !");
}
break;
}
printf("finished mouse_proc_c \n");
return 0;
}
/* orient() calculates orientation of the camera, updating its calibration
structure using the definitions and algorithms well described in [1].
Arguments:
Calibration* cal_in - camera calibration object
control_par *cpar - control parameters
int nfix - number of 3D known points
vec3d fix[] - each of nfix items is one 3D position of known point on
the calibration object.
target pix[] - image coordinates corresponding to each point in ``fix``.
can be obtained from the set of detected 2D points using
sortgrid(). The points which are associated with fix[] have real
pointer (.pnr attribute), others have -999.
orient_par flags - structure of all the flags of the parameters to be
(un)changed, read from orient.par parameter file using
read_orient_par(), defaults are zeros except for x_scale which is
by default 1.
Output:
Calibration *cal_in - if the orientation routine converged, this structure
is updated, otherwise, returned untouched. The routine works on a copy of
the calibration structure, cal.
double sigmabeta[] - array of deviations for each of the interior and
exterior parameters and glass interface vector (19 in total).
Returns:
On success, a pointer to an array of residuals. For each observation point
i = 0..n-1, residual 2*i is the Gauss-Markof residual for the x coordinate
and residual 2*i + 1 is for the y. Then come 10 cells with the delta
between initial guess and final solution for internal and distortion
parameters, which are also part of the G-M model and described in it.
On failure returns NULL.
*/
double* orient (Calibration* cal_in, control_par *cpar, int nfix, vec3d fix[],
target pix[], orient_par *flags, double sigmabeta[20])
{
int i,j,n, itnum, stopflag, n_obs=0, maxsize;
double ident[IDT], XPX[NPAR][NPAR], XPy[NPAR], beta[NPAR], omega=0;
double xp, yp, xpd, ypd, xc, yc, r, qq, p, sumP;
int numbers;
double al,be,ga,nGl,e1_x,e1_y,e1_z,e2_x,e2_y,e2_z,safety_x,safety_y,safety_z;
double *P, *y, *yh, *Xbeta, *resi;
vec3d glass_dir, tmp_vec, e1, e2;
Calibration *cal;
/* small perturbation for translation/rotation in meters and in radians */
double dm = 0.00001, drad = 0.0000001;
cal = malloc (sizeof (Calibration));
memcpy(cal, cal_in, sizeof (Calibration));
maxsize = nfix*2 + IDT;
P = (double *) calloc(maxsize, sizeof(double));
y = (double *) calloc(maxsize, sizeof(double));
yh = (double *) calloc(maxsize, sizeof(double));
Xbeta = (double *) calloc(maxsize, sizeof(double));
resi = (double *) calloc(maxsize, sizeof(double));
double (*X)[NPAR] = malloc(sizeof (*X) * maxsize);
double (*Xh)[NPAR] = malloc(sizeof (*Xh) * maxsize);
for(i = 0; i < maxsize; i++) {
for(j = 0; j < NPAR; j++) {
X[i][j] = 0.0;
Xh[i][j] = 0.0;
}
y[i] = 0;
P[i] = 1;
}
for(i = 0; i < NPAR; i++)
sigmabeta[j] = 0.0;
if(flags->interfflag){
numbers = 18;
} else{
numbers = 16;
}
vec_set(glass_dir,
cal->glass_par.vec_x, cal->glass_par.vec_y, cal->glass_par.vec_z);
nGl = vec_norm(glass_dir);
e1_x = 2*cal->glass_par.vec_z - 3*cal->glass_par.vec_x;
e1_y = 3*cal->glass_par.vec_x - 1*cal->glass_par.vec_z;
e1_z = 1*cal->glass_par.vec_y - 2*cal->glass_par.vec_y;
vec_set(tmp_vec, e1_x, e1_y, e1_z);
unit_vector(tmp_vec, e1);
e2_x = e1_y*cal->glass_par.vec_z - e1_z*cal->glass_par.vec_x;
e2_y = e1_z*cal->glass_par.vec_x - e1_x*cal->glass_par.vec_z;
e2_z = e1_x*cal->glass_par.vec_y - e1_y*cal->glass_par.vec_y;
vec_set(tmp_vec, e2_x, e2_y, e2_z);
unit_vector(tmp_vec, e2);
al = 0;
be = 0;
ga = 0;
/* init identities */
ident[0] = cal->int_par.cc;
ident[1] = cal->int_par.xh;
ident[2] = cal->int_par.yh;
ident[3] = cal->added_par.k1;
ident[4] = cal->added_par.k2;
ident[5] = cal->added_par.k3;
ident[6] = cal->added_par.p1;
ident[7] = cal->added_par.p2;
ident[8] = cal->added_par.scx;
ident[9] = cal->added_par.she;
safety_x = cal->glass_par.vec_x;
safety_y = cal->glass_par.vec_y;
safety_z = cal->glass_par.vec_z;
/* 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 < NUM_ITER)) {
itnum++;
for (i = 0, n = 0; i < nfix; i++) {
/* check for correct correspondence
note that we do not use anymore pointer in fix, the points are read by
the order of appearance and if we want to use every other point
we use 'i', just check it is not -999 */
if(pix[i].pnr != i) continue;
switch (flags->useflag) {
case 1: if ((i % 2) == 0) continue; break;
case 2: if ((i % 2) != 0) continue; break;
case 3: if ((i % 3) == 0) continue; break;
}
/* get metric flat-image coordinates of the detected point */
pixel_to_metric (&xc, &yc, pix[i].x, pix[i].y, cpar);
correct_brown_affin (xc, yc, cal->added_par, &xc, &yc);
/* Projected 2D position on sensor of corresponding known point */
rotation_matrix(&(cal->ext_par));
img_coord (fix[i], cal, cpar->mm, &xp, &yp);
/* derivatives of distortion parameters */
r = sqrt (xp*xp + yp*yp);
X[n][7] = cal->added_par.scx;
X[n+1][7] = sin(cal->added_par.she);
X[n][8] = 0;
X[n+1][8] = 1;
X[n][9] = cal->added_par.scx * xp * r*r;
X[n+1][9] = yp * r*r;
X[n][10] = cal->added_par.scx * xp * pow(r,4.0);
X[n+1][10] = yp * pow(r,4.0);
X[n][11] = cal->added_par.scx * xp * pow(r,6.0);
X[n+1][11] = yp * pow(r,6.0);
X[n][12] = cal->added_par.scx * (2*xp*xp + r*r);
X[n+1][12] = 2 * xp * yp;
X[n][13] = 2 * cal->added_par.scx * xp * yp;
X[n+1][13] = 2*yp*yp + r*r;
qq = cal->added_par.k1*r*r; qq += cal->added_par.k2*pow(r,4.0);
qq += cal->added_par.k3*pow(r,6.0);
qq += 1;
X[n][14] = xp * qq + cal->added_par.p1 * (r*r + 2*xp*xp) + \
2*cal->added_par.p2*xp*yp;
X[n+1][14] = 0;
X[n][15] = -cos(cal->added_par.she) * yp;
X[n+1][15] = -sin(cal->added_par.she) * yp;
/* numeric derivatives of projection coordinates over external
parameters, 3D position and the angles */
num_deriv_exterior(cal, cpar, dm, drad, fix[i], X[n], X[n + 1]);
/* Num. deriv. of projection coords over sensor distance from PP */
cal->int_par.cc += dm;
rotation_matrix(&(cal->ext_par));
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][6] = (xpd - xp) / dm;
X[n+1][6] = (ypd - yp) / dm;
cal->int_par.cc -= dm;
/* ditto, over water-glass-air interface position vector */
al += dm;
cal->glass_par.vec_x += e1[0]*nGl*al;
cal->glass_par.vec_y += e1[1]*nGl*al;
cal->glass_par.vec_z += e1[2]*nGl*al;
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][16] = (xpd - xp) / dm;
X[n+1][16] = (ypd - yp) / dm;
al -= dm;
cal->glass_par.vec_x = safety_x;
cal->glass_par.vec_y = safety_y;
cal->glass_par.vec_z = safety_z;
be += dm;
cal->glass_par.vec_x += e2[0]*nGl*be;
cal->glass_par.vec_y += e2[1]*nGl*be;
cal->glass_par.vec_z += e2[2]*nGl*be;
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][17] = (xpd - xp) / dm;
X[n+1][17] = (ypd - yp) / dm;
be -= dm;
cal->glass_par.vec_x = safety_x;
cal->glass_par.vec_y = safety_y;
cal->glass_par.vec_z = safety_z;
ga += dm;
cal->glass_par.vec_x += cal->glass_par.vec_x*nGl*ga;
cal->glass_par.vec_y += cal->glass_par.vec_y*nGl*ga;
cal->glass_par.vec_z += cal->glass_par.vec_z*nGl*ga;
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][18] = (xpd - xp) / dm;
X[n+1][18] = (ypd - yp) / dm;
ga -= dm;
cal->glass_par.vec_x = safety_x;
cal->glass_par.vec_y = safety_y;
cal->glass_par.vec_z = safety_z;
y[n] = xc - xp;
y[n+1] = yc - yp;
n += 2;
}
n_obs = n;
/* identities */
for (i = 0; i < IDT; i++)
X[n_obs + i][6 + i] = 1;
y[n_obs+0] = ident[0] - cal->int_par.cc;
y[n_obs+1] = ident[1] - cal->int_par.xh;
y[n_obs+2] = ident[2] - cal->int_par.yh;
y[n_obs+3] = ident[3] - cal->added_par.k1;
y[n_obs+4] = ident[4] - cal->added_par.k2;
y[n_obs+5] = ident[5] - cal->added_par.k3;
y[n_obs+6] = ident[6] - cal->added_par.p1;
y[n_obs+7] = ident[7] - cal->added_par.p2;
y[n_obs+8] = ident[8] - cal->added_par.scx;
y[n_obs+9] = ident[9] - cal->added_par.she;
/* weights */
for (i = 0; i < n_obs; i++)
P[i] = 1;
P[n_obs+0] = ( ! flags->ccflag) ? POS_INF : 1;
P[n_obs+1] = ( ! flags->xhflag) ? POS_INF : 1;
P[n_obs+2] = ( ! flags->yhflag) ? POS_INF : 1;
P[n_obs+3] = ( ! flags->k1flag) ? POS_INF : 1;
P[n_obs+4] = ( ! flags->k2flag) ? POS_INF : 1;
P[n_obs+5] = ( ! flags->k3flag) ? POS_INF : 1;
P[n_obs+6] = ( ! flags->p1flag) ? POS_INF : 1;
P[n_obs+7] = ( ! flags->p2flag) ? POS_INF : 1;
P[n_obs+8] = ( ! flags->scxflag) ? POS_INF : 1;
P[n_obs+9] = ( ! flags->sheflag) ? POS_INF : 1;
n_obs += IDT;
sumP = 0;
for (i = 0; i < n_obs; i++) { /* homogenize */
p = sqrt (P[i]);
for (j = 0; j < NPAR; j++)
Xh[i][j] = p * X[i][j];
yh[i] = p * y[i];
sumP += P[i];
}
/* Gauss Markoff Model it is the least square adjustment
of the redundant information contained both in the spatial
intersection and the resection, see [1], eq. 23 */
ata ((double *) Xh, (double *) XPX, n_obs, numbers, NPAR );
matinv ((double *) XPX, numbers, NPAR);
atl ((double *) XPy, (double *) Xh, yh, n_obs, numbers, NPAR);
matmul ((double *) beta, (double *) XPX, (double *) XPy,
numbers, numbers,1, NPAR, NPAR);
stopflag = 1;
for (i = 0; i < numbers; i++) {
if (fabs (beta[i]) > CONVERGENCE) stopflag = 0;
}
if ( ! flags->ccflag) beta[6] = 0.0;
if ( ! flags->xhflag) beta[7] = 0.0;
if ( ! flags->yhflag) beta[8] = 0.0;
if ( ! flags->k1flag) beta[9] = 0.0;
if ( ! flags->k2flag) beta[10] = 0.0;
if ( ! flags->k3flag) beta[11] = 0.0;
if ( ! flags->p1flag) beta[12] = 0.0;
if ( ! flags->p2flag) beta[13] = 0.0;
if ( ! flags->scxflag)beta[14] = 0.0;
if ( ! flags->sheflag) beta[15] = 0.0;
cal->ext_par.x0 += beta[0];
cal->ext_par.y0 += beta[1];
cal->ext_par.z0 += beta[2];
cal->ext_par.omega += beta[3];
cal->ext_par.phi += beta[4];
cal->ext_par.kappa += beta[5];
cal->int_par.cc += beta[6];
cal->int_par.xh += beta[7];
cal->int_par.yh += beta[8];
cal->added_par.k1 += beta[9];
cal->added_par.k2 += beta[10];
cal->added_par.k3 += beta[11];
cal->added_par.p1 += beta[12];
cal->added_par.p2 += beta[13];
cal->added_par.scx += beta[14];
cal->added_par.she += beta[15];
if (flags->interfflag) {
cal->glass_par.vec_x += e1[0]*nGl*beta[16];
cal->glass_par.vec_y += e1[1]*nGl*beta[16];
cal->glass_par.vec_z += e1[2]*nGl*beta[16];
cal->glass_par.vec_x += e2[0]*nGl*beta[17];
cal->glass_par.vec_y += e2[1]*nGl*beta[17];
cal->glass_par.vec_z += e2[2]*nGl*beta[17];
}
}
/* compute residuals etc. */
matmul ( (double *) Xbeta, (double *) X, (double *) beta, n_obs,
numbers, 1, n_obs, NPAR);
omega = 0;
for (i = 0; i < n_obs; i++) {
resi[i] = Xbeta[i] - y[i];
omega += resi[i] * P[i] * resi[i];
}
sigmabeta[NPAR] = sqrt (omega / (n_obs - numbers));
for (i = 0; i < numbers; i++) {
sigmabeta[i] = sigmabeta[NPAR] * sqrt(XPX[i][i]);
}
free(X);
free(P);
free(y);
free(Xbeta);
free(Xh);
if (stopflag){
rotation_matrix(&(cal->ext_par));
memcpy(cal_in, cal, sizeof (Calibration));
return resi;
}
else {
free(resi);
return NULL;
}
}
