/*****************************************************************************/ /* */ /* NAME: dots */ /* FUNCTION: calculate barrel/pincushion distortion from a grid of dots */ /* USAGE: see below */ /* COMPILE: cc dots.c -lm -o dots */ /* RUN: dots -w -c 7 -r 6 -o ../zzz ../spots */ /* AUTHOR: distortion stuff - Lee Campbell (elwin@media-lab.mit.edu) */ /* file I/O stuff - Stephen Intille */ /* Gauss-Jordan elimination - Press, et al.: Numerical Recipes */ /* command line parsing - public domain */ /* CHANGES: 11/29/93 polynomial changed from rnew = ar + br^2 + cr^3 */ /* to rnew = ar + br^3 + cr^5 because only odd powers are */ /* needed in radial polynomial */ /* (steve) 1995mar03: commented out one drawplus; set other one to 255 255 */ /* diff with ~elwin/lens/code/dots.c old_dots.c */ /* added print to stderr to show what to type on swarp command line*/ /* commented out ``invert brightness value'' */ /* (steve) 1995mar29: with elwin; got gleung and tpminka to help debug */ /* (was string length malloc 11 for 11 char + null = 12) */ /*****************************************************************************/ /* Reads a raw image file stored as a 'data' file and 'descriptor' file */ /* and calculates center of distortion and radial polynomial for correcting */ /* distortion. */ /* Should work for float and char images. Works on sequences. */ /* dots [-w] [-z] [-l int] [-c int] [-i] [-o outfile] [infile] -w : Allow outfiles to overwrite existing files (if used, this must be the first flag -z : instead of analyzing the input image, generate a synthetic test image with perfect rows & cols of dots. Use this to debug warp/dewarp & analysis software. (still needs infile to get image dimensions) -i : Interpolate instead of remove pixels. Picture size will not be altered. Not currently functional. -r int : Number of Rows of dots. Default is 8. -c int : Number of Columns of dots. Default is 8. -o outfile : Save the synthetic test image in specified directory infile : Name of original file */ #include #include #include #include #include #include extern int optind; extern char *optarg; #define ROWCOLMIN 4 #define ROWCOLMAX 16 #define SZN 10 #define SZM 2 #define OPTIONS "wzir:c:o:" #define NIF 1 /* maximal number of input files. only allow one */ char *cmd; /* used to store the name of the program */ char *cmt; /* used to store the entire command line */ char *usage = "[-w] [-z] [-i] [-r int] [-c int] [-o outfile] [infile]"; struct parmstruct { int countx, county; float dotcentx[ROWCOLMAX][ROWCOLMAX]; float dotcenty[ROWCOLMAX][ROWCOLMAX]; float dotcentw[ROWCOLMAX][ROWCOLMAX]; float hlinerho[ROWCOLMAX]; float hlinetheta[ROWCOLMAX]; float vlinerho[ROWCOLMAX]; float vlinetheta[ROWCOLMAX]; float hparabolas[ROWCOLMAX][3]; float vparabolas[ROWCOLMAX][3]; float xbar, ybar, mmt, xunitbar, yunitbar, unitmmt, spacing; double poly[6]; } parmstruct; /*** globals ***/ int dot_rows = 8; int dot_cols = 8; /*---------------------------------------------------------------------------*/ get_input_file_name(ifile, ifile_desc, argv) char **ifile; char **ifile_desc; char *argv[]; { *ifile = (char *) malloc (sizeof(char) * (strlen(argv[optind]) + 10)); *ifile_desc = (char *) malloc (sizeof(char) * (strlen(argv[optind]) + 12)); /* bug in string; null char at end so ``/descriptor'' needs 11+1=12 bytes*/ strcpy(*ifile, argv[optind]); strcpy(*ifile_desc, argv[optind]); strcat(*ifile, "/data"); strcat(*ifile_desc, "/descriptor"); } /*-----------------------------------------------------------*/ get_output_file_name(ofile, ofile_desc, overwrite_files) char **ofile; char **ofile_desc; int overwrite_files; { FILE *fp; struct stat stbuf; /* used to store result of stat operation */ /* used to check if files already exist */ char command[256]; /* stores unix command */ *ofile = (char *) malloc (sizeof(char) * (strlen(optarg) + 11)); *ofile_desc = (char *) malloc (sizeof(char) * (strlen(optarg) + 12)); /* bug in string; null char at end so ``/descriptor'' needs 11+1=12 bytes*/ strcpy(*ofile, optarg); if (stat(*ofile, &stbuf) == -1) { fprintf(stderr,"Couldn't access %s. Creating directory.\n", *ofile); sprintf(command,"mkdir %s", *ofile); if ((fp = popen(command,"r")) == NULL) fprintf(stderr,"Couldn't run %s.", command); close(fp); } else if ((stbuf.st_mode & S_IFMT) != S_IFDIR) { fprintf(stderr, "File %s exists. Aborted. No dir created.\n", *ofile); exit(1); } /* $$$ */ strcpy(*ofile_desc, *ofile); strcat(*ofile_desc, "/descriptor"); strcat(*ofile, "/data"); if (!overwrite_files) { if (stat(*ofile, &stbuf) != -1) { fprintf(stderr,"File %s exists. Aborted.\n",*ofile); exit(1); } if (stat(*ofile_desc, &stbuf) != -1) { fprintf(stderr,"File %s exists. Aborted.\n",*ofile_desc); exit(1); } } } /*-----------------------------------------------------------*/ get_frame_params(ifile_desc, nframe, sets, bytes_pixel, xdim, ydim) /* Read input file descriptor to find size and file type */ char *ifile_desc; int *nframe, *sets, *bytes_pixel; int *xdim, *ydim; { char label[20], junk[20], junk2[20]; char line[80]; FILE *ifs_desc = stdin; if ((ifs_desc = fopen(ifile_desc, "r")) == NULL) {fprintf(stderr, "%s: Cannot open x descriptor input file\n", cmd); exit(1);} while (fgets(line,80,ifs_desc) != NULL) { sscanf(line,"%s",label); if (!strcmp (label, "(_data_files")) sscanf(line,"%s%d%s", junk, nframe, junk2); else if (!strcmp (label, "(_dimensions")) sscanf(line,"%s%d%d%s", junk, ydim, xdim, junk2); else if (!strcmp (label, "(_data_sets")) sscanf(line,"%s%d%s", junk, sets, junk2); else if (!strcmp (label, "(_data_type")) { sscanf(line,"%s%s", junk, junk2); if ((!strcmp (junk2, "\"unsigned_1\")")) || (!strcmp (junk2, "unsigned_1"))) *bytes_pixel = 1; if ((!strcmp (junk2, "\"float\")")) || (!strcmp (junk2, "float"))) *bytes_pixel = 4; } } fclose (ifs_desc); } /*-----------------------------------------------------------*/ read_frame_data(ifile, frame, nframe, xdim, ydim, bytes_pixel, data) /* get the image */ char *ifile; int frame, nframe; int *data; { char *current_ifile; char junk[20]; int bytes; FILE *ifs; struct stat stbuf; /* used to store result of stat operation */ /* used to check if files already exist */ current_ifile = (char *) malloc (sizeof(char) * (strlen(ifile) + 6)); strcpy(current_ifile, ifile); if (nframe != 1) /* if only one frame, don't tag number on end */ strcat(current_ifile, sprintf(junk, "%d",frame - 1)); fprintf(stderr,"Working on %s", current_ifile); fprintf(stderr,".\n"); /* open input file and read data */ if (stat(current_ifile, &stbuf) == -1) { fprintf(stderr,"Error: Couldn't access %s. \n", current_ifile); exit(1); } if ((ifs = fopen(current_ifile, "r")) == NULL) { fprintf(stderr, "%s: Cannot open input file\n", cmd); exit(1); } bytes = fread(data, bytes_pixel, xdim * ydim, ifs); fprintf(stderr,"Read %d bytes of input image data.\n",bytes); fclose(ifs); free(current_ifile); } /*-----------------------------------------------------------*/ write_frame_data(ofile, frame, nframe, xdim, ydim, bytes_pixel, vimage) char *ofile; int frame, nframe; int xdim, ydim; int bytes_pixel; int *vimage; { char *current_ofile; char junk[20]; int countout; FILE *ofs = stdout; current_ofile = (char *) malloc (sizeof(char) * (strlen(ofile) + 6)); strcpy(current_ofile, ofile); if (nframe != 1) /* if only one frame, don't tag number on end */ strcat(current_ofile, sprintf(junk, "%d",frame - 1)); fprintf(stderr,"assigned file to ofile %s\n", current_ofile); if ((ofs = fopen(current_ofile, "w")) == NULL) { fprintf(stderr, "%s: Cannot open output file\n", cmd); exit(1); } countout = fwrite(vimage, bytes_pixel, xdim*ydim, ofs); if (countout != xdim*ydim) fprintf(stderr,"output err: wrote %d pixels; should be %d \n", countout, xdim*ydim); fclose(ofs); free(current_ofile); } /*-----------------------------------------------------------*/ write_output_params(ofile_desc, nframe, xdim, ydim, bytes_pixel) char *ofile_desc; int nframe; int xdim, ydim; int bytes_pixel; { char *dat_type; FILE *ofs_desc = stdout; if ((ofs_desc = fopen(ofile_desc, "w")) == NULL) {fprintf(stderr, "%s: Cannot open x descriptor output file\n", cmd); exit(1);} /* Write descriptor files */ dat_type = (char *) malloc(sizeof(char) * 30); if (bytes_pixel == 4) strcpy(dat_type, "float"); else strcpy(dat_type, "unsigned_1"); fprintf(ofs_desc, "(_data_files %d)\n(_channels 1)\n(_dimensions %d %d)\n(_data_type %s)", nframe, ydim, xdim, dat_type); fclose(ofs_desc); } /*-----------------------------------------------------------*/ main(argc, argv) /* process command line args */ int argc; char *argv[]; { int errflag = 0; /* set to 1 on error */ int oflag = 0; /* infiles must be entered */ int overwrite_files = 0; /* set to 1 (allow) if user specifies -w */ int efile_requested = 0; /* default: don't output file of removed lines */ int synthim = 0; int interpolate = 0; char c; /* c is a temporary character used for breaking the */ /* command line into its individual flags, names, etc*/ char *ofile = NULL; /* set default output file name to NULL */ char *ofile_desc = NULL; char *ifile = NULL; /* set default input file name to NULL */ char *ifile_desc = NULL; char **datafile; char **descfile; cmd = argv[0]; /* save the name of program */ while ((c = getopt(argc, argv, OPTIONS)) != EOF) switch (c) { case 'w': overwrite_files = 1; break; case 'z': synthim = 1; break; case 'i': interpolate = 1; break; case 'r': dot_rows = atoi(optarg); if ((dot_rows < ROWCOLMIN)) { fprintf(stderr,"Error: -r too few rows. Min is %d.\n",ROWCOLMIN); exit(1); } if ((dot_rows > ROWCOLMAX)) { fprintf(stderr,"Error: -r too many rows. Max is %d.\n",ROWCOLMAX); exit(1); } break; case 'c': dot_cols = atoi(optarg); if ((dot_cols < ROWCOLMIN)) { fprintf(stderr,"Error: -r too few cols. Min is %d.\n",ROWCOLMIN); exit(1); } if ((dot_cols > ROWCOLMAX)) { fprintf(stderr,"Error: -r too many cols. Max is %d.\n",ROWCOLMAX); exit(1); } break; case 'o': if (!oflag) { datafile = &ofile; descfile = &ofile_desc; get_output_file_name(datafile, descfile, overwrite_files); oflag = 1; } break; case '?': errflag = 1; break; } /* check if error was found while parsing command line */ if ((synthim == 1) && (oflag == 0)) fprintf(stderr,"synth image option should have output file \n"); if (errflag /* error in options */ /* || !oflag */ || argc > optind + NIF) /* we desire only NIF input files. At this */ /* point in the execution, getopt should */ /* have parsed all the command line prompts*/ /* except the input file names. Therefore, */ /* if this relationship is not true, extra */ /* flags, arguments, or names were in the */ /* command prompt */ {fprintf(stderr, "\nUsage: %s %s\n\n", cmd, usage); fprintf(stderr, "Read ~elwin/lens/code/dots.c for description.\n\n"); exit(1);} /* get input file */ datafile = &ifile; descfile = &ifile_desc; get_input_file_name(datafile, descfile, argv); /*do the processing*/ do_it(ifile, ifile_desc, ofile, ofile_desc, synthim, oflag); free(ifile); free(ifile_desc); if (oflag == 1) { free(ofile); free(ofile_desc); /* Freedom, FREEdom! */ } fprintf(stderr, " Th'th'that's all, FFolks! \n"); } /*-----------------------------------------------------------*/ /* This part of the code loops through frames in a sequence and calls the subroutines that actually do the real work */ do_it(ifile, ifile_desc, ofile, ofile_desc, synthim, oflag) char *ifile, *ifile_desc, *ofile, *ofile_desc; int synthim, oflag; { struct stat stbuf; /* used to store result of stat operation */ /* used to check if files already exist */ char junk[20], junk2[20]; unsigned char *data, *vimage; int xdim, ydim; int bytes; int nframe; int frame; int sets; int countout; int bytes_pixel; struct parmstruct parm; nframe = 1; bytes_pixel = 1; /* default to picture of chars */ sets = 0; get_frame_params(ifile_desc, &nframe, &sets, &bytes_pixel, &xdim, &ydim); fprintf(stderr,"Input image size: (%d (y), %d (x)).",ydim, xdim); fprintf(stderr," Frames: %d.",nframe); fprintf(stderr," Bytes/pixel : %d.\n",bytes_pixel); data = (unsigned char *) malloc(sizeof(char) * bytes_pixel * (xdim * ydim)); vimage = (unsigned char *)malloc(sizeof(*vimage) * xdim * ydim); parm.countx = dot_cols; parm.county = dot_rows; fprintf(stderr,"Hold on, working...\n"); for (frame = 1; frame <= nframe; frame++) { read_frame_data(ifile, frame, nframe, xdim, ydim, bytes_pixel, data); /* get the image, store it in data */ /****** THIS IS WHERE THE ACTION IS *******/ if (synthim == 0) { finddots(xdim, ydim, data, vimage, &parm); fitlines(xdim, ydim, vimage, &parm); finddesired(xdim, ydim, vimage, &parm); searchcod(xdim, ydim, 0.5*xdim, 0.5*ydim, vimage, &parm); /* testline(xdim, ydim, vimage); */ makedisplay(xdim, ydim, vimage); } else synth_image((float) parm.countx, (float) parm.county, vimage, xdim, ydim); } /* for frame */ if (oflag == 1) { /* write output file data and descriptor */ write_frame_data(ofile, frame, nframe, xdim, ydim, bytes_pixel, vimage); write_output_params(ofile_desc, nframe, xdim, ydim, bytes_pixel); } free(data); /* "Freedom's just another word for nothin left to lose;" */ free(vimage); /* "Nothin ain't worth nothin but its free..." */ } /* Kris Kristofferson, Rhodes Scholar */ /*-----------------------------------------------------------*/ #define THEPIXEL(col,row) ((row)*imx + (col)) finddots(imx, imy, rawimage, vimage, parm) int imx, imy; unsigned char *rawimage; unsigned char *vimage; struct parmstruct *parm; { /* subroutine finds dots in raw image by imposing a regular grid on the image and in each box of grid, finding min & max pixel value. Anything in lower half of brightness range is assumed to be part of background; the rest is part of the dot. The dot center is a sum of pixel coords weighted by pixel brightness. This is a quick and dirty attempt to let pixel edge blurring work FOR us. Dots have to lie fully within box; dots that fall into 2 boxes will mess everything up. */ int xx, yy, dd, tt; int xxmax, yymax, pixmax; int hbox, vbox; int dimmest, brightest; int cut, cutsize, pcount; float dotx, doty; float dotweight, pixweight; /* image is in rawimage[] */ pixmax = 0; xxmax = 0; yymax = 0; fprintf(stderr,"dots.c: assuming dots are white on a black background\n"); for (vbox = 1; vbox <= parm->county; vbox++) /* loop over rows of boxes */ { for (hbox = 1; hbox <= parm->countx; hbox++) /* loop over boxes in row */ { dimmest = 255; brightest = 0; for (yy = ((vbox-1)*imy)/parm->county; yy <= (vbox*imy)/parm->county - 1; yy++) /* loop over pixels */ { for (xx = ((hbox-1)*imx)/parm->countx; xx <= (hbox*imx)/parm->countx - 1; xx++) { dd = rawimage[THEPIXEL(xx,yy)]; /* dd = 256 - dd; */ /* invert brightness value */ rawimage[THEPIXEL(xx,yy)] = dd; /* */ if ((dd < dimmest) && (dd > 0)) dimmest = dd; if (dd > brightest) /* find min and max pixel and cut */ brightest = dd; /* assume pixel brighter than cut */ } /* end for xx */ /* is part of dot; dimmer is part */ } /* end for yy */ /* of background. */ cut = (brightest + dimmest)/2; cutsize = (brightest - cut); /* fprintf(stderr, " b d cut %5d %5d %5d ", brightest, dimmest, cut); */ pcount = 0; dotweight = .001; /* make sure nobody gets zero weight */ dotx = xx*dotweight; doty = yy*dotweight; for (yy = ((vbox-1)*imy)/parm->county; yy <= (vbox*imy)/parm->county - 1; yy++) { for (xx =((hbox-1)*imx)/parm->countx; xx <= (hbox*imx)/parm->countx - 1; xx++) { dd = rawimage[THEPIXEL(xx,yy)]; /* tt = THEPIXEL(xx,yy); if (tt>pixmax) pixmax = tt; if (xx>xxmax) xxmax = xx; if (yy>yymax) yymax = yy; if (yy>(imy-3)) fprintf(stderr," %7d (%3d) ",tt,rawimage[tt]); */ /* if ((vbox==1) && (hbox==1)) fprintf(stderr,"<%4d %4d %5d> ",xx,yy,dd); /* */ if (dd > cut) { pcount++; pixweight = ((float)(dd-cut))/cutsize; dotweight += pixweight; /* accumulate a weighted */ dotx += xx*pixweight; /* sum of pixels in dot. */ doty += yy*pixweight; tt = ((unsigned char)(128*pixweight)); vimage[THEPIXEL(xx,yy)] = tt; /* draw dot */ } else { /* draw dashed lines at box borders */ if (((xx==(hbox*imx)/parm->countx-1)&&((yy & 1)==0)) || ((yy == (vbox*imy)/parm->county-1)&&((xx & 1)==0))) tt = ((unsigned char)(25)); else tt = ((unsigned char)(2)); vimage[THEPIXEL(xx,yy)] = tt; } } /* end for xx */ } /* end for yy */ /* calculate dot center. */ parm->dotcentx[hbox][vbox] = dotx/dotweight; parm->dotcenty[hbox][vbox] = doty/dotweight; parm->dotcentw[hbox][vbox] = dotweight; /* fprintf(stderr, "%6.1f %6.1f (%3d) ", dotx/dotweight, doty/dotweight, pcount); */ /* if (hbox == parm->countx) fprintf(stderr, "\n"); /* */ } /* end for hbox */ } /* end for vbox */ /* printf(" maxes %6d %6d %6d %6d %6d \n",xxmax,yymax,pixmax,imx,imy); */ } /*-----------------------------------------------------------*/ fitlines(imx, imy, vimage, parm) int imx, imy; unsigned char *vimage; struct parmstruct *parm; { /* subroutine calculates best fit straight lines through the and columns of dots. lines are returned in form x * sin(theta) - y * cos(theta) + rho = 0 because y = mx + b form has probs as line gets near vertical. See B. K. P. Horn, Robot Vision, pp. 50-52 for more details */ int x,y,i,j; int line, hvcase, limit; int linecnt, dotcnt; float xx, yy, cs, sn; float sumx, sumx2, sumy, sumy2, sumxy; float linerror; float pi = 3.14159265; double rho, theta; for (hvcase = 1; hvcase <= 2; hvcase++) /* 1 == horiz.; 2 == vert. */ { if (hvcase == 1) { linecnt = parm->county; dotcnt = parm->countx; } else { linecnt = parm->countx; dotcnt = parm->county; } for (line = 1; line <= linecnt; line++) { sumx = 0; sumx2 = 0; sumy = 0; sumy2 = 0; sumxy = 0; for (i = 1; i <= dotcnt; i++) { if (hvcase == 1) { xx = parm->dotcentx[i][line]; yy = parm->dotcenty[i][line]; } else { xx = parm->dotcentx[line][i]; yy = parm->dotcenty[line][i]; } sumx += xx; sumy += yy; sumxy += xx*yy; sumx2 += xx*xx; sumy2 += yy*yy; } yy = 2.0*(sumxy*dotcnt - sumx*sumy); xx = dotcnt*(sumx2-sumy2) - sumx*sumx + sumy*sumy; theta = 0.5*atan2(((double) yy),((double) xx)); rho = (sumy*cos(theta) - sumx*sin(theta))/dotcnt; cs = cos(theta); sn = sin(theta); linerror = sumx2*sn*sn + sumy2*cs*cs + dotcnt*rho*rho + 2.0*rho*(sumx*sn - sumy*cs) - 2.0*sumxy*sn*cs; if (hvcase == 1) { /* fprintf(stderr," hline %3d %10.5f %10.5f ", line, rho, theta);*/ parm->hlinerho[line] = rho; parm->hlinetheta[line] = theta; for (x = 0; x < imx; x++) /* draw horizontal lines */ { y = ((int) (rho + x*sn)/cs + 0.5); vimage[THEPIXEL(x,y)] = 45; /* */ } } else { /* fprintf(stderr," vline %3d %10.5f %10.5f ", line, rho, theta);*/ parm->vlinerho[line] = rho; parm->vlinetheta[line] = theta; for (y = 0; y < imy; y++) /* draw vertical lines */ { x = (int) ((y*cs - rho)/sn + 0.5); vimage[THEPIXEL(x,y)] = 45; /* */ } } /* fprintf(stderr, " LE %9.5f \n",linerror); */ } } } /*-----------------------------------------------------------*/ double linearity(polyc, codguessx, codguessy, vimage, parm) double *polyc; float codguessx, codguessy; unsigned char *vimage; struct parmstruct *parm; { /* subroutine calculates & totals error from linearity of each row and column of corrected points. Derivation on P.3b of my first research notebook. */ int x,y,i,j; int line, hvcase, limit; int linecnt, dotcnt; double xx, yy, xxc, yyc; double codtryx, codtryy; double tt, r, rc, cs, sn; double sumx, sumx2, sumy, sumy2, sumxy; double thiserror, linerror; double rho, theta; linerror = 0; codtryx = (double) codguessx; codtryy = (double) codguessy; for (hvcase = 1; hvcase <= 2; hvcase++) /* 1 == horiz.; 2 == vert. */ { if (hvcase == 1) { linecnt = parm->county; dotcnt = parm->countx; } else { linecnt = parm->countx; dotcnt = parm->county; } for (line = 1; line <= linecnt; line++) { sumx = 0; sumx2 = 0; sumy = 0; sumy2 = 0; sumxy = 0; for (i = 1; i <= dotcnt; i++) { if (hvcase == 1) { xx = (double) parm->dotcentx[i][line]; yy = (double) parm->dotcenty[i][line]; } else { xx = (double) parm->dotcentx[line][i]; yy = (double) parm->dotcenty[line][i]; } r = sqrt((xx-codtryx)*(xx-codtryx) + (yy-codtryy)*(yy-codtryy)); tt = r*r; rc = tt*tt*r*polyc[5] + tt*tt*polyc[4] + tt*r*polyc[3] + tt*polyc[2] + r*polyc[1]; xxc = codtryx + (xx-codtryx)*rc/r; /* corrected coords */ yyc = codtryy + (yy-codtryy)*rc/r; sumx += xxc; sumy += yyc; sumxy += xxc*yyc; sumx2 += xxc*xxc; sumy2 += yyc*yyc; } yy = 2.0*(sumxy*dotcnt - sumx*sumy); xx = dotcnt*(sumx2-sumy2) - sumx*sumx + sumy*sumy; theta = 0.5*atan2(((double) yy),((double) xx)); rho = (sumy*cos(theta) - sumx*sin(theta))/dotcnt; cs = cos(theta); sn = sin(theta); thiserror = sumx2*sn*sn + sumy2*cs*cs + dotcnt*rho*rho + 2.0*rho*(sumx*sn - sumy*cs) - 2.0*sumxy*sn*cs; if (thiserror < 0) fprintf(stderr,"BUG neg linerror %10.5f hv%d line%d ignored\n", thiserror, hvcase, line); if (thiserror > 0) linerror += thiserror; } } return (linerror); } /*-----------------------------------------------------------*/ finddesired(imx, imy, vimage, parm) int imx, imy; unsigned char *vimage; struct parmstruct *parm; { int x,y; float xx, yy; float xbar, ybar, xsqr, ysqr; float xunitbar, yunitbar; float xunitsqr, yunitsqr; float mmt, unitmmt, spacing; /* calculate center of mass and moment of inertia of array of dots */ /* (weight = 1). Do the same for dots of 1 pixel spacing and we can */ /* get the average pixel spacing. */ xbar = 0; ybar = 0; xsqr = 0; ysqr = 0; xunitbar = 0; yunitbar = 0; xunitsqr = 0; yunitsqr = 0; for (y = 1; y <= parm->county; y++) /* loop over rows of dots */ for (x = 1; x <= parm->countx; x++) /* loop over dots in row */ { xx = parm->dotcentx[x][y]; yy = parm->dotcenty[x][y]; xbar += xx; ybar += yy; xsqr += xx*xx; ysqr += yy*yy; xunitbar += x; yunitbar += y; xunitsqr += x*x; yunitsqr += y*y; } unitmmt = xunitsqr + yunitsqr - (xunitbar*xunitbar + yunitbar*yunitbar)/(parm->countx*parm->county); unitmmt = sqrt(unitmmt/(parm->countx*parm->county)); mmt = xsqr + ysqr - (xbar*xbar + ybar*ybar)/(parm->countx*parm->county); mmt = sqrt(mmt/(parm->countx*parm->county)); spacing = mmt/unitmmt; parm->spacing = spacing; parm->xbar = xbar/(parm->countx*parm->county); parm->ybar = ybar/(parm->countx*parm->county); parm->mmt = mmt; parm->xunitbar = xunitbar/(parm->countx*parm->county); parm->yunitbar = yunitbar/(parm->countx*parm->county); parm->unitmmt = unitmmt; fprintf(stderr," C O M %10.5f %10.5f %10.5f %10.5f \n", parm->xbar, parm->ybar, parm->xunitbar, parm->yunitbar); fprintf(stderr," moment %10.5f %10.5f %10.5f \n", mmt, unitmmt, spacing); fprintf(stderr," X , Y %10.5f %10.5f \n", sqrt(((xsqr-xbar*xbar)/(xunitsqr-xunitbar*xunitbar))), sqrt(((ysqr-ybar*ybar)/(yunitsqr-yunitbar*yunitbar)) )); } /*-----------------------------------------------------------*/ float fitdistort(imx, imy, codx, cody, vimage, parm) int imx, imy; float codx, cody; unsigned char *vimage; struct parmstruct *parm; #define THEC(line,term) (term+SZN*line) #define THEB(line,term) (term+SZM*line) { /* This subroutine makes a first guess at a radial polynomial. It does so by creating an imaginary grid of points with the same 2nd moment as the supplied grid, but with zero distortion. That defines desired location of a dot in this subroutine. Then it least square fits a radial polynomial in r, the distance of the dots from the center of distortion. Minimizes r-rd where rd is dist of desired location of dot. The output is a polynomial which is used as a first guess by the searchlinearity() routine. */ int x,y,i,j; int msize, bsize; float xx, yy, tt; float xxc, yyc, xxd, yyd; float sumx, sumx2, sumy, sumy2, sumxy; float sr, sr2, sr3, sr4, sr5, sr6, sr7, sr8, sr9, sr10; float t, t1, t2, t3, t4, t5; float r, rd, rc; float er2, ed2, elin2; float xbar, ybar, spacing; float xunitbar, yunitbar; float m[SZN*SZN]; float s[SZN*SZN]; float b[SZN*SZM]; spacing = parm->spacing; xunitbar = parm->xunitbar; yunitbar = parm->yunitbar; xbar = parm->xbar; ybar = parm->ybar; /* calculate distortion correction polynomial. Based on least squares mimimization of [actual_radius - desired radius]^2 */ sr = 0; sr2 = 0; sr3 = 0; sr4 = 0; sr5 = 0; sr6 = 0; sr7 = 0; sr8 = 0; sr9 = 0; sr10 = 0; t = 0; t1 = 0; t2 = 0; t3 = 0; t4 = 0; t5 = 0; er2 = 0; ed2 = 0; elin2 = 0; for (y = 1; y <= parm->county; y++) { for (x = 1; x <= parm->countx; x++) { xxd = x * spacing + xbar - (xunitbar*spacing) + .5; /* desired */ yyd = y * spacing + ybar - (yunitbar*spacing) + .5; /* coords */ xx = parm->dotcentx[x][y]; yy = parm->dotcenty[x][y]; /* calculate actual and desired dist from c.o.d. */ r = sqrt((xx-codx)*(xx-codx) + (yy-cody)*(yy-cody)); rd = sqrt((xxd-codx)*(xxd-codx) + (yyd-cody)*(yyd-cody)); /* er2 += (rd-r)*(rd-r); ed2 += (xxd-xx)*(xxd-xx) + (yyd-yy)*(yyd-yy); */ sr += r; t += rd; t1 += r*rd; tt = r*r; sr2 += tt; t2 += rd*tt; tt = tt*r; sr3 += tt; t3 += rd*tt; tt = r*tt; sr4 += tt; t4 += rd*tt; tt = r*tt; sr5 += tt; t5 += rd*tt; tt = r*tt; sr6 += tt; tt = r*tt; sr7 += tt; tt = r*tt; sr8 += tt; tt = r*tt; sr9 += tt; sr10 += r*tt; } } /* set up a matrix for gauss-jordan elimination */ /* fit eqn f(r) = a*r^3 + b*r^2 + c*r = 0 (no d because we want f(0) = 0). */ for (i = 0; i < SZN*SZN; i++) m[i] = 0; for (i = 0; i < SZN*SZM; i++) b[i] = 0; for (i = 0; i < 6; i++) parm->poly[i] = 0; /* msize = 4; bsize = 1; /* fit a*r^4 + b*r^3 + c*r^2 + dr = r_d */ /* m[THEC(1,1)]=sr8; m[THEC(2,1)]=sr7; m[THEC(3,1)]=sr6; m[THEC(4,1)]=sr5; m[THEC(1,2)]=sr7; m[THEC(2,2)]=sr6; m[THEC(3,2)]=sr5; m[THEC(4,2)]=sr4; m[THEC(1,3)]=sr6; m[THEC(2,3)]=sr5; m[THEC(3,3)]=sr4; m[THEC(4,3)]=sr3; m[THEC(1,4)]=sr5; m[THEC(2,4)]=sr4; m[THEC(3,4)]=sr3; m[THEC(4,4)]=sr2; b[THEB(1,1)]=t4; b[THEB(1,2)]=t3; b[THEB(1,3)]=t2; b[THEB(1,4)]=t1; /* */ /* msize = 4; bsize = 1; /* fit a*r^3 + b*r^2 + c*r + d = r_d */ /* m[THEC(1,1)]=sr6; m[THEC(2,1)]=sr5; m[THEC(3,1)]=sr4; m[THEC(4,1)]=sr3; m[THEC(1,2)]=sr5; m[THEC(2,2)]=sr4; m[THEC(3,2)]=sr3; m[THEC(4,2)]=sr2; m[THEC(1,3)]=sr4; m[THEC(2,3)]=sr3; m[THEC(3,3)]=sr2; m[THEC(4,3)]=sr; m[THEC(1,4)]=sr3; m[THEC(2,4)]=sr2; m[THEC(3,4)]=sr; m[THEC(4,4)]=(parm->countx*parm->county); b[THEB(1,1)]=t3; b[THEB(1,2)]=t2; b[THEB(1,3)]=t1; b[THEB(1,4)]=t; /* */ /* msize = 3; bsize = 1; /* fit a*r^3 + b*r^2 + c*r = r_d */ /* m[THEC(1,1)]=sr6; m[THEC(2,1)]=sr5; m[THEC(3,1)]=sr4; m[THEC(1,2)]=sr5; m[THEC(2,2)]=sr4; m[THEC(3,2)]=sr3; m[THEC(1,3)]=sr4; m[THEC(2,3)]=sr3; m[THEC(3,3)]=sr2; b[THEB(1,1)]=t3; b[THEB(1,2)]=t2; b[THEB(1,3)]=t1; /* */ msize = 3; bsize = 1; /* fit a*r^5 + b*r^3 + c*r = r_d */ m[THEC(1,1)]=sr10; m[THEC(2,1)]=sr8; m[THEC(3,1)]=sr6; m[THEC(1,2)]=sr8; m[THEC(2,2)]=sr6; m[THEC(3,2)]=sr4; m[THEC(1,3)]=sr6; m[THEC(2,3)]=sr4; m[THEC(3,3)]=sr2; b[THEB(1,1)]=t5; b[THEB(1,2)]=t3; b[THEB(1,3)]=t1; /* */ /* for (y = 1; y <= msize; y++) fprintf(stderr," M %10.4e %10.4e %10.4e %10.4e \n", m[THEC(1,y)], m[THEC(2,y)], m[THEC(3,y)], m[THEC(4,y)]); fprintf(stderr," b %10.4e %10.4e %10.4e %10.4e \n", b[THEB(1,1)], b[THEB(1,2)], b[THEB(1,3)], b[THEB(1,4)]); /* */ for (i=0; ipoly[i] = b[THEB(1,msize+1-i)]; /* store coeffs so poly[n] is coeff of r^n */ for (i = 1; i <= msize; i++) parm->poly[2*i-1] = b[THEB(1,msize+1-i)]; /* store coeffs so poly[n] is coeff of r^n */ /* fprintf(stderr," GaussJ: %10.5e %10.5e %10.5e %10.5e \n", b[THEB(1,1)], b[THEB(1,2)], b[THEB(1,3)], b[THEB(1,4)]); /* */ /* fprintf(stderr," check:"); for (i=1; i <= msize; i++) fprintf(stderr," %10.5e", b[THEB(1,1)] * s[THEC(1,i)] + b[THEB(1,2)] * s[THEC(2,i)] + b[THEB(1,3)] * s[THEC(3,i)] + b[THEB(1,4)] * s[THEC(4,i)] ); fprintf(stderr,"\n"); /* */ /* fprintf(stderr," uncorrected error r,d %10.5e %10.5e \n",er2,ed2); /* */ /* calculate error of above correction */ er2 = 0; ed2 = 0; elin2 = 0; for (y = 1; y <= parm->county; y++) { for (x = 1; x <= parm->countx; x++) { xxd = x * spacing + xbar - (xunitbar*spacing) + .5; /* desired */ yyd = y * spacing + ybar - (yunitbar*spacing) + .5; /* coords */ rd = sqrt((xxd-codx)*(xxd-codx) + (yyd-cody)*(yyd-cody)); xx = parm->dotcentx[x][y]; /* uncorrected coords */ yy = parm->dotcenty[x][y]; r = sqrt((xx-codx)*(xx-codx) + (yy-cody)*(yy-cody)); tt = r*r; rc = parm->poly[5]*tt*tt*r + parm->poly[4]*tt*tt + parm->poly[3]*tt*r + parm->poly[2]*tt + parm->poly[1]*r; xxc = codx + (xx-codx)*rc/r; /* corrected coords */ yyc = cody + (yy-cody)*rc/r; rc = sqrt((xxc-codx)*(xxc-codx) + (yyc-cody)*(yyc-cody)); er2 += (rd-rc)*(rd-rc); /* sum radial error squared */ ed2 += (xxd-xxc)*(xxd-xxc) + (yyd-yyc)*(yyd-yyc); /* sum dist^2 */ } } /* fprintf(stderr," corrected error r,d %10.5e %10.5e \n",er2,ed2); /* */ /* test gaussj */ /* msize = 3; bsize = 1; m[THEC(1,1)] = 1; m[THEC(2,1)] = 2; m[THEC(3,1)] = 3; m[THEC(1,2)] = 4; m[THEC(2,2)] = 5; m[THEC(3,2)] = 8; m[THEC(1,3)] = 7; m[THEC(2,3)] = 8; m[THEC(3,3)] = 9; b[THEB(1,1)] = 4; b[THEB(1,2)] = 8; b[THEB(1,3)] = 6; for (i=0; icounty; y++) /*loop over rows of dots*/ for (x = 1; x <= parm->countx; x++) /*loop over dots in row*/ { xx = parm->dotcentx[x][y]; yy = parm->dotcenty[x][y]; r = sqrt((xx-xp)*(xx-xp) + (yy-yp)*(yy-yp)); tt = r*r; rc = poly[5]*tt*tt*r + poly[4]*tt*tt + poly[3]*tt*r + poly[2]*tt + poly[1]*r; xxc = xp + (xx-xp)*rc/r; yyc = yp + (yy-yp)*rc/r; xbar += xxc; ybar += yyc; rsqr += xxc*xxc + yyc*yyc; } mmt = rsqr - (xbar*xbar+ybar*ybar)/(parm->countx*parm->county); mmt = sqrt(mmt/(parm->countx*parm->county)); return (mmt); } /*-----------------------------------------------------------*/ float searchlinearity(imx, imy, ipoly, codguessx, codguessy, vimage, parm) int imx, imy; double *ipoly; float codguessx, codguessy; unsigned char *vimage; struct parmstruct *parm; { /* subroutine searches for best correction polynomial by trying to minimize deviations from collinearity of rows and columns of dots as measured by linearity(). Approach is to vary the radial polynomial such that the 2nd moment of the dots stays constant but the linearity improves. */ int x, y, i, j; int stepunit, bestdir, dir; float unit, r, rc, tt; float xx, yy, xxc, yyc, atx, aty; float besterr, thiserr; double rat, startmmt, thismmt; double mpoly[6]; float static dirsteps[9][2] = { {0,0}, {-1, 1}, {0, 1}, {1, 1}, {1, 0}, {1, -1}, {0,-1}, {-1,-1}, {-1,0} }; atx = codguessx; aty = codguessy; for (i=1;i<6;i++) mpoly[i] = 0.0; mpoly[1] = 1.0; thismmt = secondmoment(ipoly, codguessx, codguessy, parm); rat = parm->mmt/thismmt; for (j=1; j<6; j++) ipoly[j] = rat*ipoly[j]; startmmt = secondmoment(ipoly, codguessx, codguessy, parm); /* fprintf(stderr," moments: %9.5e %9.5e %9.5e %9.6e \n", startmmt, parm->mmt, thismmt, parm->mmt/startmmt-1); /* */ besterr = linearity(ipoly, atx, aty, vimage, parm); /* fprintf(stderr," Poly Err %10.5f %9.6e %9.6e %9.6e %9.6e \n", besterr, ipoly[3], ipoly[2], ipoly[1], rat-1); /* */ bestdir = 1; unit = 4.0; for (stepunit = 0; stepunit < 13; stepunit++) { unit = unit/2; bestdir = 1; /* fprintf(stderr," poly unit = %f \n", unit); /* */ for (i = 0; ((i < 100) && (bestdir != 0)); i++) { bestdir = 0; for (dir = 1; dir < 9; dir++) { /* guess a new polynomial (a & b only) */ mpoly[3] = ipoly[3] * (1.0 + (.01*unit*dirsteps[dir][0])); mpoly[2] = ipoly[2] * (1.0 + (.01*unit*dirsteps[dir][1])); mpoly[1] = ipoly[1]; thismmt = secondmoment(mpoly, codguessx, codguessy, parm); rat = parm->mmt/thismmt; for (j=1; j<6; j++) mpoly[j] = rat*mpoly[j]; /* */ thiserr = linearity(mpoly, atx, aty, vimage, parm); if (thiserr < besterr) { besterr = thiserr; for (j=1;j<6;j++) ipoly[j] = mpoly[j]; bestdir = dir; /* fprintf(stderr," Bestpoly %10.5f %9.6e %9.6e %9.6e %9.6e \n", besterr, ipoly[3], ipoly[2], ipoly[1], rat-1); /* */ } } } } /* fprintf(stderr," Poly best %10.5f %9.6e %9.6e %9.6e \n", besterr, ipoly[3], ipoly[2], ipoly[1]); /* */ return (besterr); } /*-----------------------------------------------------------*/ float newtonlinearity(imx, imy, ipoly, codguessx, codguessy, vimage, parm) int imx, imy; double *ipoly; float codguessx, codguessy; unsigned char *vimage; struct parmstruct *parm; { /* subroutine uses Newton's method to find polynomial which will minimize deviations from collinearity of rows and columns of dots as measured by linearity(). Approach is to vary the radial polynomial such that the 2nd moment of the dots stays constant but the linearity improves. +--+--+--+ |f5|f2|f6| Derivative estimates: evaluate linearity in 9 +--+--+--+ nearby places (spacing is h). |f1|f0|f3| Partial df/dx = fx = (f3-f1)/(2h) +--+--+--+ Partial d^2f/dx^2 = fxx = (f1-2f0+f3)/(h^2) |f7|f4|f8| Partial d^2f/dxdy = fxy = (f6-f5-f8+f7)/(4h^2) +--+--+--+ Newton's meth / fxx fxy fxz \ /x2-x1\ / fx \ for mimima | fxy fyy fyz | |y2-y1| = - | fy | 3 dimensions \ fxz fyz fzz / \z2-z1/ \ fz / Here we're trying to vary coeff's 2 & 3 of the polynomial, and aspect ratio to find a minimum in value returned by linearity. Added twist: coeff's 1, 2, 3 must be renormalized so 2nd moment of dots is constant. */ int i, j, k; int iter, exitflag; float atx, aty; double stepfactor, besterr, thiserr; double f0, f1, f2, f3, f4, f5, f6, f7, f8; double h2, h3, fx, fy, fxx, fyy, fxy; double det, deltax, deltay; double rat, startmmt, thismmt; double mpoly[6]; atx = codguessx; aty = codguessy; for (i=1;i<6;i++) mpoly[i] = 0.0; mpoly[1] = 1.0; thismmt = secondmoment(ipoly, codguessx, codguessy, parm); rat = parm->mmt/thismmt; for (j=1; j<6; j++) ipoly[j] = rat*ipoly[j]; startmmt = secondmoment(ipoly, codguessx, codguessy, parm); /* fprintf(stderr," moments: %9.5e %9.5e %9.5e %9.6e \n", startmmt, parm->mmt, thismmt, parm->mmt/startmmt-1); /* */ besterr = linearity(ipoly, atx, aty, vimage, parm); /* fprintf(stderr," Poly Err %10.5f %9.6e %9.6e %9.6e %9.6e \n", besterr, ipoly[5], ipoly[3], ipoly[1], rat-1); /* */ stepfactor = .9; for (iter = 0; ((iter < 15) && (stepfactor > .15)); iter++) { /* fprintf(stderr," poly iter = %f \n", iter); /* */ h3 = fabs((double)ipoly[5]*0.00001); if (h3 < 1E-14) h3 = 1E-14; h2 = fabs((double)ipoly[3]*0.00001); if (h2 < 1E-14) h2 = 1E-14; mpoly[5] = ipoly[5]; /* call this the 'y' variable */ mpoly[3] = ipoly[3]; /* call this the 'x' variable */ mpoly[1] = ipoly[1]; f0 = linearity(mpoly, atx, aty, vimage, parm); mpoly[3] = ipoly[3] - h2; f1 = linearity(mpoly, atx, aty, vimage, parm); mpoly[5] = ipoly[5] + h3; f5 = linearity(mpoly, atx, aty, vimage, parm); mpoly[3] = ipoly[3]; f2 = linearity(mpoly, atx, aty, vimage, parm); mpoly[3] = ipoly[3] + h2; f6 = linearity(mpoly, atx, aty, vimage, parm); mpoly[5] = ipoly[5]; f3 = linearity(mpoly, atx, aty, vimage, parm); mpoly[5] = ipoly[5] - h3; f8 = linearity(mpoly, atx, aty, vimage, parm); mpoly[3] = ipoly[3]; f4 = linearity(mpoly, atx, aty, vimage, parm); mpoly[3] = ipoly[3] - h2; f7 = linearity(mpoly, atx, aty, vimage, parm); /* fprintf(stderr," f0..f8 %10.5e %10.5e %10.5e %10.5e %10.5e %10.5e %10.5e %10.5e %10.5e %10.5e %10.5e \n", f0,f1-f0,f2-f0,f3-f0,f4-f0,f5-f0,f6-f0,f7-f0,f8-f0, h2, h3); */ fx = (f3 - f1)/(2*h2); fy = (f2 - f4)/(2*h3); fxx = (f1 + f3 - 2*f0)/(h2*h2); fyy = (f2 + f4 - 2*f0)/(h3*h3); fxy = (f6 - f5 - f8 + f7)/(4*h2*h3); /* fprintf(stderr," fx..fxy %10.5e %10.5e %10.5e %10.5e %10.5e \n", fx, fy, fxx, fyy, fxy); /* */ det = 1.0/(fxx*fyy - fxy*fxy); exitflag = 0; for (k=1; ((k<8) && (exitflag!=1)); k++) { deltax = stepfactor*det*(fyy*fx - fxy*fy); deltay = stepfactor*det*(fxx*fy - fxy*fx); /* fprintf(stderr," delta x,y %10.5e %10.5e \n", deltax, deltay); /* */ mpoly[3] -= deltax; mpoly[5] -= deltay; thismmt = secondmoment(mpoly, codguessx, codguessy, parm); rat = parm->mmt/thismmt; for (j=1; j<6; j++) mpoly[j] = rat*mpoly[j]; /* */ thiserr = linearity(mpoly, atx, aty, vimage, parm); if (thiserr < besterr) { exitflag = 1; besterr = thiserr; for (j=1;j<6;j++) ipoly[j] = mpoly[j]; stepfactor = .9; /* fprintf(stderr," thispoly %10.5f %9.6e %9.6e %9.6e %9.6e \n", thiserr, mpoly[5], mpoly[3], mpoly[1], rat-1); /* */ } else stepfactor = 0.4*stepfactor; } if ((iter==1) && (stepfactor < .15)) stepfactor = .5; } /* fprintf(stderr," Poly best %10.5f %9.6e %9.6e %9.6e \n", besterr, ipoly[5], ipoly[3], ipoly[1]); /* */ return (besterr); } /*-----------------------------------------------------------*/ searchcod(imx, imy, codguessx, codguessy, vimage, parm) int imx, imy; float codguessx, codguessy; unsigned char *vimage; struct parmstruct *parm; { /* subroutine searches for center of distortion by trying to minimize radial error measured by fitdistort(). Approach is not true gradient descent because it does not necessarily go in direction of steepest descent. It keeps stepping until each of 8 directions is no lower than present point, then tries a smaller stepsize (unit). */ int x, y, i, j; int stepunit; int bestdir, dir; float unit, r, rc, tt; float xx, yy, xxc, yyc, atx, aty; float starterr, besterr, thiserr; float static dirsteps[9][2] = { {0,0}, {-1, 1}, {0, 1}, {1, 1}, {1, 0}, {1, -1}, {0,-1}, {-1,-1}, {-1,0} }; atx = codguessx; aty = codguessy; for (i=1;i<6;i++) parm->poly[i] = 0.0; parm->poly[1] = 1.0; starterr = linearity(parm->poly, atx, aty, vimage, parm); fprintf(stderr," linearity error (no correction) %10.5f \n", starterr); besterr = fitdistort(imx, imy, atx, aty, vimage, parm); /* */ /* this creates first guess polynomial */ besterr = newtonlinearity(imx, imy, parm->poly, atx, aty, vimage, parm); fprintf(stderr," StartErr %10.5f %8.2f %8.2f \n", besterr, atx, aty); tt = linearity(parm->poly, atx, aty, vimage, parm); fprintf(stderr," linearity error %10.5f \n", tt); bestdir = 1; unit = 8.0; if (besterr < starterr) /* loop will mess up if it can't improve */ for (stepunit = 0; stepunit < 5; stepunit++) { unit = unit/2; bestdir = 1; fprintf(stderr," unit = %f \n", unit); for (i = 0; ((i < 100) && (bestdir != 0)); i++) { bestdir = 0; for (dir = 1; dir < 9; dir++) { xx = atx + unit*dirsteps[dir][0]; yy = aty + unit*dirsteps[dir][1]; /* thiserr = fitdistort(imx, imy, xx, yy, vimage, parm); /* */ thiserr = newtonlinearity(imx, imy, parm->poly, xx, yy, vimage, parm); /* */ if (thiserr < besterr) { besterr = thiserr; atx = xx; aty = yy; bestdir = dir; /* fprintf(stderr," BestErr %10.5f %8.2f %8.2f \n", besterr, atx, aty); fprintf(stderr," poly out %10.5f %9.6e %9.6e %9.6e \n", thiserr, parm->poly[3], parm->poly[2], parm->poly[1]); */ } } } } if (besterr >= starterr) { for (i=1;i<6;i++) parm->poly[i] = 0.0; parm->poly[1] = 1.0; besterr = starterr; } fprintf(stderr,"Final Best quarter pixel resolution: %10.5f %8.2f %8.2f \n", besterr, atx, aty); fprintf(stderr,"Final poly %9.6e %9.6e %9.6e %9.6e %9.6e \n", parm->poly[5], parm->poly[4], parm->poly[3], parm->poly[2], parm->poly[1] ); tt = newtonlinearity(imx, imy, parm->poly, atx, aty, vimage, parm);/* */ /* draw "+" marks to show desired & actual corrections */ for (y = 1; y <= parm->county; y++) /* loop over rows of dots */ for (x = 1; x <= parm->countx; x++) /* loop over dots in row */ { drawplus(imx, imy, 30, 60, parm->dotcentx[x][y], parm->dotcenty[x][y], vimage); /* actual center */ #if (1==2) xx = x * parm->spacing + parm->xbar - (parm->xunitbar*parm->spacing) + .5; /* desired coords */ yy = y * parm->spacing + parm->ybar - (parm->yunitbar*parm->spacing) + .5; drawplus(imx, imy, 255, 255, xx, yy, vimage); /* desired center */ #endif xx = parm->dotcentx[x][y]; /* uncorrected coords */ yy = parm->dotcenty[x][y]; r = sqrt((xx-atx)*(xx-atx) + (yy-aty)*(yy-aty)); tt = r*r; rc = parm->poly[5]*tt*tt*r + parm->poly[4]*tt*tt + parm->poly[3]*tt*r + parm->poly[2]*tt + parm->poly[1]*r; drawplus(imx, imy, 255, 255, atx + (xx-atx)*rc/r, aty + (yy-aty)*rc/r, vimage); /* corrected center */ } tt = linearity(parm->poly, atx, aty, vimage, parm); fprintf(stderr," linearity error %10.5f \n", tt); fprintf(stderr,"now run: swarp -w -x %7.2f -y %7.2f -a %12.9e -b %12.9e -c %12.9e -o outfile infile\n",atx,aty, parm->poly[1], parm->poly[3], parm->poly[5]); } /*-----------------------------------------------------------*/ #define WHITE 220.0 #define BLACK 20.0 synth_image(xcnt, ycnt, output, xdim, ydim) float xcnt, ycnt; unsigned char *output; int xdim, ydim; { int x, y, xi, yi, dotx, doty, trnc, cc; float xx, yy, xoff, yoff, xfrac, yfrac; float dotspacing, xweight, yweight, temp; int dotsize; cc = 0; dotspacing = 0.49 * (xdim/xcnt + ydim/ycnt); temp = (floor(0.2*dotspacing)); dotsize = (int) temp; if (dotsize < 2) dotsize = 2; if (dotsize > 11) dotsize = 11; xoff = 0.5*(xdim - (xcnt-1)*dotspacing); yoff = 0.5*(ydim - (ycnt-1)*dotspacing); fprintf(stderr," spc siz offs %9.4f %4d %9.4f %9.4f %9.4f \n", dotspacing, dotsize, temp, xoff, yoff); for (y = 0; y < ydim; y++) for (x = 0; x < xdim; x++) output[y * xdim + x] = (unsigned char) WHITE; /* paint it white */ for (y = 0; y < ycnt; y++) for (x = 0; x < xcnt; x++) { xx = x*dotspacing + xoff - 0.5*dotsize; yy = y*dotspacing + yoff - 0.5*dotsize; xi = (int) floor(xx); yi = (int) floor(yy); xfrac = xx - xi; yfrac = yy - yi; for (doty = 0; doty < (dotsize+1); doty++) for (dotx = 0; dotx < (dotsize+1); dotx++) { yweight = 1.0; if (doty == 0) yweight = (1.0 - yfrac); if (doty == dotsize) yweight = yfrac; xweight = 1.0; if (dotx == 0) xweight = (1.0 - xfrac); if (dotx == dotsize) xweight = xfrac; trnc = (unsigned char) (WHITE - (WHITE-BLACK)*xweight*yweight); output[(yi+doty)*xdim + (xi+dotx)] = trnc; } } } /*-----------------------------------------------------------*/ testline(imx, imy, vimage) int imx, imy; unsigned char *vimage; { int x,y,i,j; int count; float xx, yy, tt; float sumx, sumx2, sumy, sumy2, sumxy; float pi = 3.14159265; double rho, theta; float xi[4], yi[4]; xi[1] = 100.0; xi[2] = 100.0; xi[3] = 99.99; yi[1] = 1.0; yi[2] = 2.0; yi[3] = 3.01; count = 3; sumx = 0; sumx2 = 0; sumy = 0; sumy2 = 0; sumxy = 0; for (i = 1; i <= count; i++) { xx = xi[i]; yy = yi[i]; sumx += xx; sumy += yy; sumxy += xx*yy; sumx2 += xx*xx; sumy2 += yy*yy; } yy = 2.0*(sumxy*count - sumx*sumy); xx = count*(sumx2 - sumy2) - sumx*sumx + sumy*sumy; theta = 0.5*atan2(((double) yy),((double) xx)); rho = (sumy*cos(theta) - sumx*sin(theta))/count; fprintf(stderr," test line %10.5f %10.5f \n", rho, theta); fprintf(stderr," intercept %10.5f %10.5f \n", -rho/sin(theta), rho/cos(theta) ); xx = cos(theta); yy = sin(theta); for (x = 0; x < imx; x++) /* draw horizontal lines */ { /* y = ((int) (rho + x*yy)/xx + 0.5); vimage[THEPIXEL(x,y)] = 45; /* */ } /* */ for (y = 0; y < imy; y++) /* draw vertical lines */ { x = (int) ((y*xx - rho)/yy + 0.5); vimage[THEPIXEL(x,y)] = 45; /* */ } /* */ } /*-----------------------------------------------------------*/ makedisplay(width, height, image) /* just POP open an xwindow and ... */ int width, height; unsigned char *image; { /* makes an xwindow and displays in it the data in image. */ int x,y,i,j; FILE *fp; char command[256]; sprintf(command,"rawtorle -w %d -h %d -n 1 | rleflip -v | getx11 -w -n 255 -= +250+200 ", width,height); fp = popen(command,"w"); fwrite(image,1,width*height,fp); fclose(fp); free(image); } /*-----------------------------------------------------------*/ /* from "Numerical Recipes in C Section 2.2 */ /* MODIFICATIONS: I switched from Numerical Recipes style array of vectors to simple C vectors with a function to do the indexing. This entailed modifying the calling sequence (**a --> *a) and replacing array references (a[j][k] --> a[j+SZN*k]). A rude crude hack but it works */ #define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;} gaussj(a,n,b,m) float *a; int n; float *b; int m; { int *indxc,*indxr,*ipiv; int i,icol,irow,j,k,l,ll; float big,dum,pivinv,temp; indxc = (int *) malloc (sizeof(int) * (n + 1)); indxr = (int *) malloc (sizeof(int) * (n + 1)); ipiv = (int *) malloc (sizeof(int) * (n + 1)); for (j=1;j<=n;j++) ipiv[j]=0; for (i=1;i<=n;i++) { big=0.0; for (j=1;j<=n;j++) if (ipiv[j] != 1) for (k=1;k<=n;k++) { if (ipiv[k] == 0) { if (fabs(a[j+SZN*k]) >= big) { big=fabs(a[j+SZN*k]); irow=j; icol=k; } } else if (ipiv[k] > 1) fprintf(stderr, "nrerr gaussj: Singular Matrix-1"); } ++(ipiv[icol]); if (irow != icol) { for (l=1;l<=n;l++) SWAP(a[irow+SZN*l],a[icol+SZN*l]) for (l=1;l<=m;l++) SWAP(b[irow+SZM*l],b[icol+SZM*l]) } indxr[i]=irow; indxc[i]=icol; if (a[icol+SZN*icol]==0.0) fprintf(stderr, "nrerror gaussj: Singular Matrix-2"); pivinv=1.0/a[icol+SZN*icol]; a[icol+SZN*icol]=1.0; for (l=1;l<=n;l++) a[icol+SZN*l] *= pivinv; for (l=1;l<=m;l++) b[icol+SZM*l] *= pivinv; for (ll=1;ll<=n;ll++) if (ll != icol) { dum=a[ll+SZN*icol]; a[ll+SZN*icol]=0.0; for (l=1;l<=n;l++) a[ll+SZN*l] -= a[icol+SZN*l]*dum; for (l=1;l<=m;l++) b[ll+SZM*l] -= b[icol+SZM*l]*dum; } } for (l=n;l>=1;l--) { if (indxr[l] != indxc[l]) for (k=1;k<=n;k++) SWAP(a[k+SZN*indxr[l]],a[k+SZN*indxc[l]]); } free(ipiv); free(indxr); free(indxc); } #undef SWAP /*****************************************************************************/