/* OBSOLETE CODE FROM "RAYFISH" DERIVED PROGRAMS, STORED HERE JUST IN CASE */



/* old  wrapfish.c  as of 8/7/96 */
#include <stdio.h>
#include <time.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <pbm.h>
#include <pgm.h>
#include <ppm.h>
#include <assert.h>
#include <cmacs.h>
#include <volsense.h>

/* Program for displaying 3D grid maps, especially from the
   "crayfish" stereo grid program */

char nav[200];  int navl;  /* name of (navigation) map file */
char col[200];  int coll;  /* name of colorization file */
int ncl = 0; /* boxes to be colorized */
int   cllo[4][1024], clhi[4][1024];   float clcol[4][1024];
char  cllabel[1024][100];   int clcnt[1024];
/* colorized viewing parameters */
double cx=1.5, cy=-0.5, cz=0.0,    /* center of attention */
       pan =0.26, tilt=0.52,        /* view orientation of grid */
       dy=12.0, mag = 3.0;         /* viewing distance, magnification */
int colpich=480, colpicw=640;      /* color viewing picture dimensions */


void DisplayMapColorized(Map3D map, int crowd) 
  /* Make 3D map view, coloring marked sites
     if(crowd>0) remove positive cells with fewer than crowd neighbors */
{
    pixel **picout;	/* display picture, with correlation graphics */
    double ct, st, sp, cp, x, y, z, xr, yr, zr;
    double r, g, b;
    double d, v;
    int ix, iy, iz, mx, my, mz, lx, ly, sx, sy, ip, jp;
    int cl, isin;
    FILE *fp;
    
    /* Create map view image */

    mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];           
    lx = ILOG2C(mx);
    ly = ILOG2C(my)+lx;

    cp = COS(pan);   sp = SIN(pan);
    ct = COS(tilt);  st = SIN(tilt);

    for (cl=0; cl<ncl; cl++) clcnt[cl] = 0;   /* zero cell counts in region */

    picout = ppm_allocarray(sx = colpicw, sy = colpich);
    for (ip = 0; ip < sy; ip++) for (jp = 0; jp < sx; jp++)
      PPM_ASSIGN(picout[ip][jp],0,0,0);

    for (iz = 0; iz < mz; iz++) for (iy = 0; iy < my; iy++)
      for (ix = 0; ix < mx; ix++)
	if (map.mapm[(iz<<ly)+(iy<<lx)+ix] > 0)
	{

	    xr = (ix+0.5)*(map.himv[0]-map.lomv[0])/mx + map.lomv[0];
	    yr = (iy+0.5)*(map.himv[1]-map.lomv[1])/my + map.lomv[1];
	    zr = (iz+0.5)*(map.himv[2]-map.lomv[2])/mz + map.lomv[2];

	    isin = 1;
	    if (crowd>0)
	      {	/* remove isolated positive cells */
		  int box = 3;
		  int xl, xh, yl, yh, zl, zh, jx, jy, jz;
		  int buddy;
		  
		  isin = 0;
		  xl = ix - box;  xh = ix + box;
		  yl = iy - box;  yh = iy + box;
		  zl = iz - box;  zh = iz + box;
		  if (xl<0) xl = 0;  if (xh>=mx) xh = mx-1;
		  if (yl<0) yl = 0;  if (yh>=my) yh = my-1;
		  if (zl<0) zl = 0;  if (zh>=mz) zh = mz-1;
		  buddy = -1;
		  for (jx = xl; jx <= xh; jx++)
		    for (jy = yl; jy <= yh; jy++)
		      for (jz = zl; jz <= zh; jz++)
			if (map.mapm[(jz<<ly)+(jy<<lx)+jx] > 0)  buddy++;
		  if (buddy>=crowd) isin = 1;
	      };

	    if (isin)
	      {
		  xr -= (map.lomv[0]+map.himv[0])/2;
		  yr -= (map.lomv[1]+map.himv[1])/2;
		  zr -= (map.lomv[2]+map.himv[2])/2;
		  
		  x = xr*cp - yr*sp;  y = yr*cp + xr*sp;  xr = x;  yr = y;
		  
		  y = yr*ct + zr*st;  z = zr*ct - yr*st;  x = xr;

		  x -= cx;   y -= cy;   z -= cz;

		  y = dy - y;

		  d = SQRT(x*x+y*y+z*z);   v = 0.25 + 1.0/(1+d/(dy*4));
		  
		  ip = mag*sy*z/y + sy/2;  jp = mag*sy*x/y + sx/2;
		  jp = sx-1 - jp;  ip = sy-1 - ip;

		  if(ip>=0 && ip<sy && jp >=0 && jp<sx)
		    {
			r = g = b = v*0.75;
			for (cl = 0; cl < ncl; cl++)
			  if (ix>=cllo[0][cl] && ix<=clhi[0][cl] &&
			      iy>=cllo[1][cl] && iy<=clhi[1][cl] &&
			      iz>=cllo[2][cl] && iz<=clhi[2][cl] )
			    {
				clcnt[cl]++;
				r = clcol[0][cl]*v;
				g = clcol[1][cl]*v;
				b = clcol[2][cl]*v;
			    };
			if (r<0) r = 0;  if (r>1) r = 1;
			if (g<0) g = 0;  if (g>1) g = 1;
			if (b<0) b = 0;  if (b>1) b = 1;
			PPM_ASSIGN(picout[ip][jp],(int) 255*r,(int) 255*g, (int) 255*b);
		    };
	      };
	};
    /* and write out display */
    
    if (crowd>0) sprintf(col,"%s.colorize.trim%d%d%d.ppm",col,
			 crowd/100,(crowd/10)%10,crowd%10);
    else strcat(col,".colorize.ppm");
    fp = fopen(col,"w");    col[coll] = 0;
    ppm_writeppm(fp,picout,sx,sy,256,1);
    fclose(fp);

    ppm_freearray(picout, sy );
};

void DisplayMapSetZ(Map3D map) 
  /* Make display images for horizontal map slices */
{
    gray **picout;	/* display picture, with correlation graphics */
    int ix, iy, mx, my, mz, lx, ly, t, t1, tick, i, wad;
    int zplane, frame, xpos, ypos, sx, sy;
    int horwad, verwad, allwad;
    double dx, dy;
    FILE *fp;
    
    /* Create map slice display image array */

    mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];           
    lx = ILOG2C(mx);
    ly = ILOG2C(my)+lx;

    horwad = (4*256)/my;  verwad = (3*256)/mx;  allwad = horwad*verwad;

    picout = pgm_allocarray(sx = (mx+1)*horwad+1, sy = (my+1)*verwad+1);

    for (wad = 0; wad < mz/allwad;  wad++)
      {
	  for (ix = 0; ix < sx; ix++)  for (iy = 0; iy < sy; iy++)
	    picout[iy][ix] = 0;
	  
	  for (frame = 0; frame < allwad; frame ++)
	    {
		xpos = (frame % horwad)*(mx+1)+1;
		ypos = (frame / horwad)*(my+1)+1;
		zplane = wad*allwad+frame;
		
		for (iy = 0; iy < my; iy++) for (ix = 0; ix < mx; ix++)
		  {
		      t = map.mapm[(zplane<<ly)+(iy<<lx)+ix];
		      picout[ypos+ix][xpos+iy] = PRB(t)>>23;
		  };
	    };
	  
	  for (frame = 0; frame < allwad; frame ++)
	    {
		xpos = (frame % horwad)*(mx+1)+1;
		ypos = (frame / horwad)*(my+1)+1;
		dx = map.himv[0]-map.lomv[0];
		dy = map.himv[1]-map.lomv[1];
		
		for (tick=1; tick<=dx-1; tick++)
		  { ix = mx*tick/dx;
		    for (i = 0; i < 8; i++)
		      picout[ypos+ix-1][xpos+i] = 
			picout[ypos+ix-1][xpos+mx-1-i] = 0; }
		
		for (tick=1; tick<=dy-1; tick++)
		  { iy = my*tick/dy;
		    for (i = 0; i < 8; i++)
		      picout[ypos+i][xpos+iy-1] = 
			picout[ypos+my-i-1][xpos+iy-1] = 0; }
	    };

	  strcat(nav,".Zslices");  t1 = strlen(nav);
	  nav[t1] = '0'+wad;  nav[t1+1] = 0;
	  
	  fp = fopen(strcat(nav,".pgm"),"w"); /* and write out display */
	  nav[navl] = 0;
	  pgm_writepgm(fp,picout,sx,sy,256,1);
	  fclose(fp);
      };
    pgm_freearray(picout, sy );
};

void DisplayMapSetY(Map3D map) 
  /* Make display images for vertical map slices along Y direction */
{
    gray **picout;	/* display picture, with correlation graphics */
    int ix, iy, iz, mx, my, mz, lx, ly, t, t1, tick, i, wad;
    int yplane, frame, xpos, ypos, sx, sy;
    int horwad, verwad, allwad;
    double dx, dz;
    FILE *fp;
    
    /* Create map slice display image array */

    mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];           
    lx = ILOG2C(mx);
    ly = ILOG2C(my)+lx;

    horwad = (4*256)/mx;  verwad = (3*256)/mz;  allwad = horwad*verwad;

    picout = pgm_allocarray(sx = (mx+1)*horwad+1, sy = (mz+1)*verwad+1);

    for (wad = 0; wad < my/allwad;  wad++)
      {
	  for (ix = 0; ix < sx; ix++)  for (iy = 0; iy < sy; iy++)
	    picout[iy][ix] = 0;
	  
	  for (frame = 0; frame < allwad; frame ++)
	    {
		xpos = (frame % horwad)*(mx+1)+1;
		ypos = (frame / horwad)*(mz+1)+1;
		yplane = wad*allwad+frame;
		
		for (iz = 0; iz < mz; iz++) for (ix = 0; ix < mx; ix++)
		  {
		      t = map.mapm[(iz<<ly)+(yplane<<lx)+ix];
		      picout[ypos+mz-1-iz][xpos+mx-1-ix] = PRB(t)>>23;
		  };
	    };

	  for (frame = 0; frame < allwad; frame ++)
	    {
		xpos = (frame % horwad)*(mx+1)+1;
		ypos = (frame / horwad)*(mz+1)+1;
		dx = map.himv[0]-map.lomv[0];
		dz = map.himv[2]-map.lomv[2];
		
		for (tick=1; tick<=dx-1; tick++)
		  { ix = mx*tick/dx;
		    for (i = 0; i < 4; i++)
		      picout[ypos+i][xpos+ix-1] = 
			picout[ypos+mz-i-1][xpos+ix-1] = 0; }
		
		for (tick=1; tick<=dz-1; tick++)
		  { iz = mz-1-mz*tick/dz;
		    for (i = 0; i < 8; i++)
		      picout[ypos+iz-1][xpos+i] = 
			picout[ypos+iz-1][xpos+mx-i-1] = 0; }
	    };

	  strcat(nav,".Yslices");  t1 = strlen(nav);
	  nav[t1] = '0'+wad;  nav[t1+1] = 0;
	  
	  fp = fopen(strcat(nav,".pgm"),"w"); /* and write out display */
	  nav[navl] = 0;
	  pgm_writepgm(fp,picout,sx,sy,256,1);
	  fclose(fp);
      };
    pgm_freearray(picout, sy );
};

void DisplayMapSetX(Map3D map) 
  /* Make display images for vertical map slices along Y direction */
{
    gray **picout;	/* display picture, with correlation graphics */
    int ix, iy, iz, mx, my, mz, lx, ly, t, t1, tick, i, wad;
    int xplane, frame, xpos, ypos, sx, sy;
    int horwad, verwad, allwad;
    double dy, dz;
    FILE *fp;
    
    /* Create map slice display image array */

    mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];           
    lx = ILOG2C(mx);
    ly = ILOG2C(my)+lx;

    horwad = (4*256)/my;  verwad = (3*256)/mz;  allwad = horwad*verwad;

    picout = pgm_allocarray(sx = (my+1)*horwad+1, sy = (mz+1)*verwad+1);

    for (wad = 0; wad < my/allwad;  wad++)
      {
	  for (ix = 0; ix < sx; ix++)  for (iy = 0; iy < sy; iy++)
	    picout[iy][ix] = 0;
	  
	  for (frame = 0; frame < allwad; frame ++)
	    {
		xpos = (frame % horwad)*(mx+1)+1;
		ypos = (frame / horwad)*(mz+1)+1;
		xplane = wad*allwad+frame;
		
		for (iz = 0; iz < mz; iz++) for (iy = 0; iy < my; iy++)
		  {
		      t = map.mapm[(iz<<ly)+(iy<<lx)+xplane];
		      picout[ypos+mz-1-iz][xpos+iy] = PRB(t)>>23;
		  };
	    };

	  for (frame = 0; frame < allwad; frame ++)
	    {
		xpos = (frame % horwad)*(my+1)+1;
		ypos = (frame / horwad)*(mz+1)+1;
		dy = map.himv[1]-map.lomv[1];
		dz = map.himv[2]-map.lomv[2];
		
		for (tick=1; tick<=dy-1; tick++)
		  { iy = my*tick/dy;
		    for (i = 0; i < 4; i++)
		      picout[ypos+i][xpos+iy-1] = 
			picout[ypos+mz-i-1][xpos+iy-1] = 0; }
		
		for (tick=1; tick<=dz-1; tick++)
		  { iz = mz-1-mz*tick/dz;
		    for (i = 0; i < 8; i++)
		      picout[ypos+iz-1][xpos+i] = 
			picout[ypos+iz-1][xpos+mx-i-1] = 0; }
	    };

	  strcat(nav,".Xslices");  t1 = strlen(nav);
	  nav[t1] = '0'+wad;  nav[t1+1] = 0;
	  
	  fp = fopen(strcat(nav,".pgm"),"w"); /* and write out display */
	  nav[navl] = 0;
	  pgm_writepgm(fp,picout,sx,sy,256,1);
	  fclose(fp);
      };
    pgm_freearray(picout, sy );
};


int main(argc, argv)
int argc;
char *argv[];
{

    Map3D map;                 /* 3D evidence grid map */
    char title[200], footer[200], str[1000];
    FILE *fp;
    float  vl[3],vh[3],co[3];
    int i;
 
    InitCmacs();

    if (argc < 2 || argc > 3)
      {
	  printf("Usage: wrapfish mapname <colorize>\n");
	  printf("mapname.map3D is a 3D evidence grid binary file\n");
	  printf("colorize.color is a colorization file of <xl xh yl yh zl zh r g b> lines\n");
	  exit(0);
      };

    pgm_init(&argc, argv);
    ppm_init(&argc, argv);

    strcpy(nav,argv[1]); navl = strlen(nav);

    if (!ReadMap3Db(strcat(nav,".map3D"), &map, title, footer))
      {
	  printf("Couldn't read %s !\n",nav);
	  exit(0);
      };
    nav[navl] = 0;

    ncl = 0;   /* read colorization file */
    strcpy(col,argc==3?argv[2]:nav); coll = strlen(col);
    fp = fopen(strcat(col,".color"),"r");
    if (fp==NULL) printf("No %s file\n",col); else
      while (fgets(str,100,fp) != NULL && str[0]!='*')
	if((str[0]>='0'&&str[0]<='9')||str[0]=='-'||str[0]=='.'||str[0]==' ')
	  {   /* read in a colored box */
	      vl[0]=vh[0]=vl[1]=vh[1]=vl[2]=vh[2]=co[0]=co[1]=co[2] = -12345;
	      sscanf(str,"%g%g%g%g%g%g%g%g%g%s\n",
		     &vl[0],&vh[0],&vl[1],&vh[1],&vl[2],&vh[2],
		     &co[0],&co[1],&co[2],cllabel[ncl]);
	      if (co[2] != -12345)
		{
		    for (i=0; i<3; i++)
		      {
			  cllo[i][ncl] = 
			    (vl[i]-map.lomv[i])*map.msize[i]/(map.himv[i]-map.lomv[i]);
			  clhi[i][ncl] = 
			    (vh[i]-map.lomv[i])*map.msize[i]/(map.himv[i]-map.lomv[i]);
			  if(cllo[i][ncl] > clhi[i][ncl])
			    SWAP(cllo[i][ncl],clhi[i][ncl]);
			  if(cllo[i][ncl]<0)  cllo[i][ncl]=0;
			  if(clhi[i][ncl]>=map.msize[i])  clhi[i][ncl]=map.msize[i]-1;
			  if (co[i]<0) co[i]=0;  if (co[i]>9) co[i]=9;
			  clcol[i][ncl] = co[i]/9.0;
		      };
		    ncl++;
		}
	  }
	else if(str[0]=='P' || str[0]=='p') /* read in picture dimensions */
	      sscanf(&str[1],"%d%d",&colpich,&colpicw);
	else if(str[0]=='C' || str[0]=='c') /* read center of attention */
	  {
	      sscanf(&str[1],"%g%g%g",&vl[0],&vl[1],&vl[2]);
	      cx = vl[0];  cy = vl[1];  cz = vl[2];
	  }
	else if(str[0]=='A' || str[0]=='a') /* read pan and tilt angles */
	  {
	      sscanf(&str[1],"%g%g",&vl[0],&vl[1]);
	      pan = vl[0]*PI/180;  tilt = vl[1]*PI/180;
	  }
	else if(str[0]=='D' || str[0]=='d') /* read distance, magnification */
	  {
	      sscanf(&str[1],"%g%g",&vl[0],&vl[1]);
	      dy = vl[0];  mag = vl[1];
	  };
    
    fclose(fp);  col[coll]=0;

    printf("Map %s (%s) (%s)\n",nav,title,footer);

    printf("\nMap [%d %d %d] [%.3g:%.3g,%.3g:%.3g,%.3g:%.3g]\n",
	   map.msize[0], map.msize[1], map.msize[2],
	   map.lomv[0],map.himv[0],
	   map.lomv[1],map.himv[1],
	   map.lomv[2],map.himv[2]);


/* Display Map in various ways */
    i = 0;
    /* for (i=0; i<100; i += 1 + 0.1*i) */  DisplayMapColorized(map,i);

    for (i=0; i<ncl; i++)
      printf("%8d / %d   	cells in	%s\n",clcnt[i],
	     (clhi[0][i]-cllo[0][i]+1)*(clhi[1][i]-cllo[1][i]+1)
	     *(clhi[2][i]-cllo[2][i]+1),cllabel[i]);

    if (argc != 3)
      {
	  DisplayMapSetZ(map);
	  DisplayMapSetY(map);
	  DisplayMapSetX(map);
	  
	  {   /* Write out a .iv file for SGI ivview display */
	      int ix, iy, iz, mx, my, mz, lx, ly, thiscol, docol, cl; 
	      
	      fp = fopen(strcat(col,".iv"),"w");  col[coll]=0;
	      fprintf(fp,"#Inventor V2.1 ascii\n\n");
	      mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];           
	      lx = ILOG2C(mx);
	      ly = ILOG2C(my)+lx;
	      
	      for (docol = 1; docol<ncl; docol++)
		{
		    fprintf(fp,"Separator {\n    Info {\n	string	\"Object Name: %s\"\n",
			    cllabel[docol]);
		    fprintf(fp,"	}\n	Material { diffuseColor %.3g %.3g %.3g }\n",
			    clcol[0][docol], clcol[1][docol], clcol[2][docol]);
		    fprintf(fp,"	Coordinate3 {\n	point	[\n");
		    
		    for (iz = 0; iz < mz; iz++) for (iy = 0; iy < my; iy++)
		      for (ix = 0; ix < mx; ix++)
			if (map.mapm[(iz<<ly)+(iy<<lx)+ix] > 0)
			  {
			      thiscol = 0;
			      for (cl = 0; cl < ncl; cl++)
				if (ix>=cllo[0][cl] && ix<=clhi[0][cl] &&
				    iy>=cllo[1][cl] && iy<=clhi[1][cl] &&
				    iz>=cllo[2][cl] && iz<=clhi[2][cl] )
				  thiscol = cl;
			      if (thiscol == docol) 
				fprintf(fp,"		%.4g %.4g %.4g,\n",
					(ix+.5)*(map.himv[0]-map.lomv[0])/mx+map.lomv[0],
					(iy+.5)*(map.himv[1]-map.lomv[1])/my+map.lomv[1],
					(iz+.5)*(map.himv[2]-map.lomv[2])/mz+map.lomv[2]);
			  };
		    fprintf(fp,"		0 0 0\n");
		    fprintf(fp,"	]\n    }\n    PointSet {\n	startIndex  0 \n	numPoints   -1\n    }\n}\n\n");
		};
	      fclose(fp);
	  };
      };
    exit(0);
    
};












    ncl = 0;   /* read colorization file */
    strcpy(col,argc==3?argv[2]:nav); coll = strlen(col);
    fp = fopen(strcat(col,".color"),"r");
    if (fp==NULL) printf("No %s file\n",col); else
      while (fgets(str,100,fp) != NULL && str[0]!='*')
	if((str[0]>='0'&&str[0]<='9')||str[0]=='-'||str[0]=='.'||str[0]==' ')
	  {   /* read in a colored box */
	      vl[0]=vh[0]=vl[1]=vh[1]=vl[2]=vh[2]=co[0]=co[1]=co[2] = -12345;
	      sscanf(str,"%g%g%g%g%g%g%g%g%g%s\n",
		     &vl[0],&vh[0],&vl[1],&vh[1],&vl[2],&vh[2],
		     &co[0],&co[1],&co[2],cllabel[ncl]);
	      if (co[2] != -12345)
		{
		    for (i=0; i<3; i++)
		      {
			  cllo[i][ncl] = 
			    (vl[i]-map.lomv[i])*map.msize[i]/(map.himv[i]-map.lomv[i]);
			  clhi[i][ncl] = 
			    (vh[i]-map.lomv[i])*map.msize[i]/(map.himv[i]-map.lomv[i]);
			  if(cllo[i][ncl] > clhi[i][ncl])
			    SWAP(cllo[i][ncl],clhi[i][ncl]);
			  if(cllo[i][ncl]<0)  cllo[i][ncl]=0;
			  if(clhi[i][ncl]>=map.msize[i])  clhi[i][ncl]=map.msize[i]-1;
			  if (co[i]<0) co[i]=0;  if (co[i]>9) co[i]=9;
			  clcol[i][ncl] = co[i]/9.0;
		      };
		    ncl++;
		}
	  }
	else if(str[0]=='P' || str[0]=='p') /* read in picture dimensions */
	      sscanf(&str[1],"%d%d",&colpich,&colpicw);
	else if(str[0]=='C' || str[0]=='c') /* read center of attention */
	  {
	      sscanf(&str[1],"%g%g%g",&vl[0],&vl[1],&vl[2]);
	      cx = vl[0];  cy = vl[1];  cz = vl[2];
	  }
	else if(str[0]=='A' || str[0]=='a') /* read pan and tilt angles */
	  {
	      sscanf(&str[1],"%g%g",&vl[0],&vl[1]);
	      pan = vl[0]*PI/180;  tilt = vl[1]*PI/180;
	  }
	else if(str[0]=='D' || str[0]=='d') /* read distance, magnification */
	  {
	      sscanf(&str[1],"%g%g",&vl[0],&vl[1]);
	      dy = vl[0];  mag = vl[1];
	  };
    
    fclose(fp);  col[coll]=0;









/* old crayfish.c  8/7/96 */
#include <stdio.h>
#include <time.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <pbm.h>
#include <pgm.h>
#include <ppm.h>
#include <assert.h>
#include <cmacs.h>
#include <fisheye.h>
#include <volsense.h>

/* Test program for "fisheye" stereo 3D evidence grid routines
    Builds a grid map from multiple views described in a navigation file */

/*  Navigation file RUN <expanded to "RUN.nav"> file format:

CALIBFILE    <expanded into "CALIBFILE.L.calib" and "CALIBFILE.R.calib">
CameraSeparation CameraTilt CameraZ  <in meters, degrees, meters height>
GridXlo GridXhi  GridYlo GridYhi       <grid map size, in meters>
CameraHeading1 CameraX1 CameraY1 IMAGE1 <deg,m,m, exp. IMAGE1.L.pgm IMAGE1.R.pgm>
CameraHeading2 CameraX2 CameraY2 IMAGE2
CameraHeading3 CameraX3 CameraY3 IMAGE3
...
<EOF or line beginning with non-number ends file>
*/

gray **picL, **picR;  	/* left and right incoming pictures */
gray **recL, **recR;	/* rectified outgoing picture */
int	Pich = 600, /* Picture height, pixels (576 or 480 typical) */
        Picw = 800,  /* Picture width, pixels  (768 or 640 typical) */
        Picmax = 256; /* Maximum pixel value (256 usual) */
int intopL[(600>>3)*(800>>3)];  /* interest operator results, left image */
int intopR[(600>>3)*(800>>3)];  /* interest operator results, right image */
int top;  /* max value of interest operator */
int  joffset;  /* leftmost bound of each search, for CamClose range */
int ichoose, jchoose;
int fudge0 = -1, fudge1 = 1, fudge;  /* fudge for image vertical misalignments */

int correl[800], corri[800];   /* correlation result arrays */
char img[100];   int imgl;		/*  image name */
char nav[100];   int navl;		/*  navigation track name */

/* colorization file parameters, for computing map statistics */

int ncl = 0; /* boxes to be colorized */
int   cllo[4][1024], clhi[4][1024];   float clcol[4][1024];
char  cllabel[1024][100];   int clcnt[1024];
/* colorized viewing parameters */
double cx=1.5, cy=-0.5, cz=0.0,    /* center of attention */
pan =0.26, tilt=0.52,        /* view orientation of grid */
dy=12.0, mag = 3.0;         /* viewing distance, magnification */
int colpich=480, colpicw=640;      /* color viewing picture dimensions */

int ReadColorization(char *nav, Map3D map)    
{
    FILE *fp;  int i;  char str[200];  float vl[3], vh[3], co[3];
    
    ncl = 0;   /* read colorization file */
    fp = fopen(strcat(nav,".color"),"r");
    if (fp==NULL) {printf("No %s file\n",nav);  return(0);} else
      while (fgets(str,100,fp) != NULL && str[0]!='*')
	if((str[0]>='0'&&str[0]<='9')||str[0]=='-'||str[0]=='.'||str[0]==' ')
	  {   /* read in a colored box */
	      vl[0]=vh[0]=vl[1]=vh[1]=vl[2]=vh[2]=co[0]=co[1]=co[2] = -12345;
	      sscanf(str,"%g%g%g%g%g%g%g%g%g%s\n",
		     &vl[0],&vh[0],&vl[1],&vh[1],&vl[2],&vh[2],
		     &co[0],&co[1],&co[2],cllabel[ncl]);
	      if (co[2] != -12345)
		{
		    for (i=0; i<3; i++)
		      {
			  cllo[i][ncl] = 
			    (vl[i]-map.lomv[i])*map.msize[i]/(map.himv[i]-map.lomv[i]);
			  clhi[i][ncl] = 
			    (vh[i]-map.lomv[i])*map.msize[i]/(map.himv[i]-map.lomv[i]);
			  if(cllo[i][ncl] > clhi[i][ncl])
			    SWAP(cllo[i][ncl],clhi[i][ncl]);
			  if(cllo[i][ncl]<0)  cllo[i][ncl]=0;
			  if(clhi[i][ncl]>=map.msize[i])  clhi[i][ncl]=map.msize[i]-1;
			  if (co[i]<0) co[i]=0;  if (co[i]>9) co[i]=9;
			  clcol[i][ncl] = co[i]/9.0;
		      };
		    ncl++;
		}
	  }
	else if(str[0]=='P' || str[0]=='p') /* read in picture dimensions */
	  sscanf(&str[1],"%d%d",&colpich,&colpicw);
	else if(str[0]=='C' || str[0]=='c') /* read center of attention */
	  {
	      sscanf(&str[1],"%g%g%g",&vl[0],&vl[1],&vl[2]);
	      cx = vl[0];  cy = vl[1];  cz = vl[2];
	  }
	else if(str[0]=='A' || str[0]=='a') /* read pan and tilt angles */
	  {
	      sscanf(&str[1],"%g%g",&vl[0],&vl[1]);
	      pan = vl[0]*PI/180;  tilt = vl[1]*PI/180;
	  }
	else if(str[0]=='D' || str[0]=='d') /* read distance, magnification */
	  {
	      sscanf(&str[1],"%g%g",&vl[0],&vl[1]);
	      dy = vl[0];  mag = vl[1];
	  };
    
    fclose(fp);   nav[strlen(nav)-strlen(".color")]=0;
    return(1);
};


void DisplayInterest()  /* Make interest operator display image */
{
    gray **picout;	/* display picture, with interest graphics */
    int i, j, ii, l, ll, p, intcnt, gintcnt, intchoose, inv, glimit;
    FILE *fp;
    
    /* Create interest display image array */
    picout = pgm_allocarray(Picw,Pich);
    for (i=0; i<Pich; i++) for (j=0; j<Picw; j++)
      picout[i][j] = (j>(Picw/2))?recL[i][j]:recR[i][j];
    
    glimit = 1000;   /* quality of feature chosen for correlation display */

    p = 0; intcnt = 0;  gintcnt = 0;
    for (i = 0; i < Pich-7; i += 8) for (j = 0; j < Picw-7; j += 8)	
      {
	  inv = (j>(Picw/2))?intopL[p]:intopR[p];  p++;
	  if (inv>0) intcnt++;
	  if (inv>top/glimit) gintcnt++;
	  l = (800*inv+top-1)/top;  if (l<0) l=0;  if (l>8) l = 8;
	  ll = l/2;  l -= ll;
	  for (ii = 4-l; ii < 4+ll; ii++)  /* draw an X */
	    {
		picout[i+ii][j+ii] = picout[i+ii][j+ii]<128?Picmax:0;
		picout[i+ii][j+7-ii] = picout[i+ii][j+7-ii]<128?Picmax:0;
	    };
      }; 
    
    /* pick a random interest point for correlation display */
    intchoose = RAND*gintcnt;  ichoose = 4;  jchoose = 4;

    gintcnt = 0;  p = 0;
    for (i = 0; i < Pich-7; i += 8) for (j = 0; j < Picw-7; j += 8)	
      {
	  inv = (j>(Picw/2))?intopL[p]:intopR[p];  p++;
	  if (inv>top/glimit) if ((gintcnt++) == intchoose)
	    { ichoose = i+4;  jchoose = j+4; }; 
      };

    for (ii = -20; ii <= 20; ii++)  /* draw a + */
      if (ii>4 || ii<-4)
      {
	  i = ichoose+ii;  j = jchoose+ii;
	  if (i<0) i=0; if (i>=Pich) i = Pich-1;
	  if (j<0) j=0; if (j>=Picw) j = Picw-1;
	  picout[i][jchoose] = picout[i][jchoose]<128?Picmax:0;
	  picout[ichoose][j] = picout[ichoose][j]<128?Picmax:0;
      };

    fp = fopen(strcat(img,".intop.pgm"),"w"); /* and write out display */
    img[imgl] = 0;
    pgm_writepgm(fp,picout,Picw,Pich,256,0);
    fclose(fp);
    pgm_freearray(picout, Pich );
};


void DisplayCorrelate() 
  /* Make display image for a selected correlation */
{
    gray **picout;	/* display picture, with correlation graphics */
    int i, j, l, eye;
    int scani, scanj, scanpic, bestj, joffset;
    int histo[Pich];
    int t, tsum, sil, sjl, sih, sjh;
    double crms[Picw];
    FILE *fp;
    
    /* Create correlation display image array */
    picout = pgm_allocarray(Picw,Pich);
    
    scani = ichoose;  scanj = jchoose;

    eye = (scanj-4)>(Picw/2);
    joffset = Picw/2;
    fudge = eye?fudge1:fudge0;
    
    bestj = FCorrel7(
		     eye?picL:picR, eye?picR:picL, 
		     eye?recL:recR, scani, scanj, Pich, Picw,
		    eye?recR:recL, sil = scani+fudge-0,
		    sjl = eye?scanj-joffset:scanj,
		    sih = scani+fudge+0,
		    sjh = eye?scanj:scanj+joffset,
		    correl,corri);

    if (sjl>sjh) SWAP(sjl,sjh);
    
    if (sjl<4) sjl = 4;   if (sjh>Picw-5) sjh = Picw-5;

    /* copy search image into display, and mark search area */
    for (i=0; i<Pich; i++) for (j=0; j<Picw; j++)
      picout[i][j] = 
	(i==sil-5 || i==sih+4 || j==sjl-5 || j==sjh+4)?
	  ((eye?recR:recL)[i][j]<128?Picmax:0):(eye?recR:recL)[i][j];
    
    /* inset search window from .left image in right image display */
    scanpic = scani<Pich/2?scani+40:scani-40;
    for (i=-32; i<32; i++) for (j=-32; j<32; j++)
      if (scanpic+i>=0 && scanpic+i<Pich && scanj+j>=0 && scanj+j<Picw
	&& scani+i>=0 && scani+i<Pich)
      picout[scanpic+i][scanj+j] =
	(i == -32 || i == 31 || j == -32 || j == 31)?0:
	(i == -5  || i == 4  || j == -5  || j == 4)?
	  ((eye?recL:recR)[scani+i][scanj+j]<128)?
	  Picmax:0:(eye?recL:recR)[scani+i][scanj+j];
    
    /* mark location of best match */
    for (i = scani-16; i < scani+16; i++) 
      if (i>=0 && i<Pich && bestj-5>=0 && bestj+4<Picw)
      { picout[i][bestj-5] = picout[i][bestj-5]<128?Picmax:0;
	picout[i][bestj+4] = picout[i][bestj+4]<128?Picmax:0;  };

    for (j = bestj-5; j <= bestj+4; j++)
      if (corri[bestj]-5>=0 && corri[bestj]+4<Pich && j>=0 && j<Picw)
      { picout[corri[bestj]-5][j] = picout[corri[bestj]-5][j]<128?Picmax:0;
	picout[corri[bestj]+4][j] = picout[corri[bestj]+4][j]<128?Picmax:0;  };
    
    for (j = sjl; j<=sjh; j++)
	  crms[j] = SQRT(correl[j]/49.0);   /* 49 if Correl7 is used */

    for (i=0; i<Pich; i++) histo[i] = 0;
    for (j = sjl; j<=sjh; j++)
      if (correl[j]<1e8) /* compute RMS match from raw sums */
      {  l = SQRT(correl[j]/49.0) + 2;   /* 49 if Correl7 is used */
	 if (l>0 && l<Pich-1) /* and accumulate histogram */
	   {
	       histo[l]++;
	       picout[l][j] = Picmax;	
	       picout[l-1][j] = 0;
	       picout[l+1][j] = 0;
	   }
     };
    
    tsum = 1;
    for (i=0; i<Pich; i++) if (histo[i]>tsum) tsum=histo[i];
    
    for (i=0; i<Pich; i++)
      {   for (j= Picw-50; j<Picw-1; j++) picout[i][j]=0;
	  t = (histo[i]*200)/tsum;
	  for (j=Picw-t-1; j<Picw-1; j++) picout[i][j] = Picmax;
      };
    
    fp = fopen(strcat(img,".correl.pgm"),"w"); /* and write out display */
    img[imgl] = 0;
    pgm_writepgm(fp,picout,Picw,Pich,256,0);
    fclose(fp);
    pgm_freearray(picout, Pich );
    
};


void DisplayMapView(Map3D map) 
  /* Make display images for horizontal map slices */
{
    pixel **picout;	/* display picture, with correlation graphics */
    double cx, cy, cz, tilt, ct, st, pan, sp, cp, x, y, z, xr, yr, zr, dy;
    double r, g, b, e=0.125;
    int ix, iy, iz, mx, my, mz, lx, ly, sx, sy, ip, jp, lonely, iix, iiy, iiz;
    FILE *fp;
    
    /* Create map view image */

    mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];           
    lx = ILOG2C(mx);
    ly = ILOG2C(my)+lx;

    cx = (map.lomv[0]+map.himv[0])/2;    /* coordinate center of view */
    cy = (map.lomv[1]+2*map.himv[1])/3;
    cz = (map.lomv[2]+map.himv[2])/2;

    dy = 2*(map.himv[1]-map.lomv[1]);  if (dy<0) dy = -dy;
    tilt = -30*PI/180;   ct = COS(tilt);  st = SIN(tilt);
    pan = 15*PI/180;    cp = COS(pan);   sp = SIN(pan);

    picout = ppm_allocarray(sx = 800, sy = 600);
    for (ip = 0; ip < sy; ip++) for (jp = 0; jp < sx; jp++)
      PPM_ASSIGN(picout[ip][jp],0,0,0);

    for (iy = 0; iy < my; iy++) for (ix = 0; ix < mx; ix++)
      for (iz = 0; iz < mz; iz++) if (map.mapm[(iz<<ly)+(iy<<lx)+ix] > 0
	      || ((ix==0 || ix==mx-1) && (iz==0 || iz==mz-1)) 
	      || ((ix==0 || ix==mx-1) && (iy==0 || iy==my-1))
              || ((iy==0 || iy==my-1) && (iz==0 || iz==mz-1)) )
	{

	    xr = (ix+0.5)*(map.himv[0]-map.lomv[0])/mx + map.lomv[0];
	    yr = (iy+0.5)*(map.himv[1]-map.lomv[1])/my + map.lomv[1];
	    zr = (iz+0.5)*(map.himv[2]-map.lomv[2])/mz + map.lomv[2];

	    lonely = (xr>0 && xr<4)?0:5;
	    for (iix = ix-1; iix <= ix+1; iix++) if (iix>=0 && iix<mx)
	    for (iiy = iy-1; iiy <= iy+1; iiy++) if (iiy>=0 && iiy<my)
	    for (iiz = iz-1; iiz <= iz+1; iiz++) if (iiz>=0 && iiz<mz)
	      if (map.mapm[(iz<<ly)+(iy<<lx)+ix] <= 0) lonely++;
	    
	    if (lonely<20)
	      {
		  xr -= cx;    yr -= cy;    zr -= cz;
		  
		  x = xr*cp - yr*sp;  y = yr*cp + xr*sp;  xr = x;  yr = y;
		  
		  y = yr*ct - zr*st;  z = zr*ct + yr*st;  x = xr;
		  
		  y = dy - y;
		  
		  ip = 3*sy*z/y + sy/2;  jp = 3*sx*x/y + sx/2;
		  jp = sx-1 - jp;  ip = sy-1 - ip;

		  if(ip>=0 && ip<sy && jp >=0 && jp<sx)
		    {
			r = (1.0*ix)/mx;
			g = e+((1.0-e)*iy)/my;
			b = e+((1.0-e)*iz)/mz;
			
			if (r<0) r = 0;  if (r>1) r = 1;
			if (g<0) g = 0;  if (g>1) g = 1;
			if (b<0) b = 0;  if (b>1) b = 1;
			PPM_ASSIGN(picout[ip][jp],(int) 255*r,(int) 255*g, (int) 255*b);
		    };
	      };
	};
    fp = fopen(strcat(nav,".view.ppm"),"w"); /* and write out display */
    nav[navl] = 0;
    ppm_writeppm(fp,picout,sx,sy,256,0);
    fclose(fp);

    ppm_freearray(picout, sy );
};


void DisplayMapSetZ(Map3D map) 
  /* Make display images for horizontal map slices */
{
    gray **picout;	/* display picture, with correlation graphics */
    int ix, iy, iz, mx, my, mz, lx, ly, t, t1, tick, i, wad;
    int npos, nneg, nzer, nposZ[map.msize[2]];
    int zplane, frame, xpos, ypos, sx, sy;
    double dx, dy;
    FILE *fp;
    
    /* Create map slice display image array */

    mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];           
    lx = ILOG2C(mx);
    ly = ILOG2C(my)+lx;

    npos = nneg = nzer = 0;  for (iz = 0; iz < mz; iz++) nposZ[iz] = 0;
    for (iy = 0; iy < my; iy++) for (ix = 0; ix < mx; ix++)
      for (iz = 0; iz < mz; iz++)
	{
	    t1 = map.mapm[(iz<<ly)+(iy<<lx)+ix];
	    if (t1<0) nneg++; else { if (t1>0) npos++; else nzer++; };
	    if (t1>0) nposZ[iz]++;
	};
    printf ("\n Map counts (+ -):  %d %d, (z0 z1 z2): %d %d %d = %d\n",
	    npos, nneg, nposZ[0],nposZ[1],nposZ[2], nposZ[0]+nposZ[1]+nposZ[2]);

    picout = pgm_allocarray(sx = mx*4+4, sy = my*3+4);

    for (wad = 0; wad < mz/12;  wad++)
      {
	  for (ix = 0; ix < sx; ix++)  for (iy = 0; iy < sy; iy++)
	    picout[iy][ix] = 0;
	  
	  for (frame = 0; frame < 12; frame ++)
	    {
		xpos = (frame % 4)*(mx+1)+1;
		ypos = (frame / 4)*(my+1)+1;
		zplane = wad*12+frame;
		
		for (iy = 0; iy < my; iy++) for (ix = 0; ix < mx; ix++)
		  {
		      t = map.mapm[(zplane<<ly)+(iy<<lx)+ix];
		      picout[ypos+ix][xpos+iy] = PRB(t)>>23;
		  };
	    };
	  
	  for (frame = 0; frame < 12; frame ++)
	    {
		xpos = (frame % 4)*(mx+1)+1;
		ypos = (frame / 4)*(my+1)+1;
		dx = map.himv[0]-map.lomv[0];
		dy = map.himv[1]-map.lomv[1];
		
		for (tick=1; tick<=dx-1; tick++)
		  { ix = mx*tick/dx;
		    for (i = 0; i < 8; i++)
		      picout[ypos+ix-1][xpos+i] = 
			picout[ypos+ix-1][xpos+mx-1-i] = 0; }
		
		for (tick=1; tick<=dy-1; tick++)
		  { iy = my*tick/dy;
		    for (i = 0; i < 8; i++)
		      picout[ypos+i][xpos+iy-1] = 
			picout[ypos+my-i-1][xpos+iy-1] = 0; }
	    };

	  strcat(nav,".Zslices");  t1 = strlen(nav);
	  nav[t1] = '0'+wad;  nav[t1+1] = 0;
	  
	  fp = fopen(strcat(nav,".pgm"),"w"); /* and write out display */
	  nav[navl] = 0;
	  pgm_writepgm(fp,picout,sx,sy,256,0);
	  fclose(fp);
      };
    pgm_freearray(picout, sy );
};

void DisplayMapSetY(Map3D map) 
  /* Make display images for vertical map slices along Y direction */
{
    gray **picout;	/* display picture, with correlation graphics */
    int ix, iy, iz, mx, my, mz, lx, ly, t, t1, tick, i, wad;
    int yplane, frame, xpos, ypos, sx, sy;
    double dx, dz;
    FILE *fp;
    
    /* Create map slice display image array */

    mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];           
    lx = ILOG2C(mx);
    ly = ILOG2C(my)+lx;

    picout = pgm_allocarray(sx = (mx+1)*4+1, sy = (mz+1)*12+1);

    for (wad = 0; wad < my/48;  wad++)
      {
	  for (ix = 0; ix < sx; ix++)  for (iy = 0; iy < sy; iy++)
	    picout[iy][ix] = 0;
	  
	  for (frame = 0; frame < 48; frame ++)
	    {
		xpos = (frame % 4)*(mx+1)+1;
		ypos = (frame / 4)*(mz+1)+1;
		yplane = wad*48+frame;
		
		for (iz = 0; iz < mz; iz++) for (ix = 0; ix < mx; ix++)
		  {
		      t = map.mapm[(iz<<ly)+(yplane<<lx)+ix];
		      picout[ypos+mz-1-iz][xpos+mx-1-ix] = PRB(t)>>23;
		  };
	    };

	  for (frame = 0; frame < 48; frame ++)
	    {
		xpos = (frame % 4)*(mx+1)+1;
		ypos = (frame / 4)*(mz+1)+1;
		dx = map.himv[0]-map.lomv[0];
		dz = map.himv[2]-map.lomv[2];
		
		for (tick=1; tick<=dx-1; tick++)
		  { ix = mx*tick/dx;
		    for (i = 0; i < 4; i++)
		      picout[ypos+i][xpos+ix-1] = 
			picout[ypos+mz-i-1][xpos+ix-1] = 0; }
		
		for (tick=1; tick<=dz-1; tick++)
		  { iz = mz-1-mz*tick/dz;
		    for (i = 0; i < 8; i++)
		      picout[ypos+iz-1][xpos+i] = 
			picout[ypos+iz-1][xpos+mx-i-1] = 0; }
	    };

	  strcat(nav,".Yslices");  t1 = strlen(nav);
	  nav[t1] = '0'+wad;  nav[t1+1] = 0;
	  
	  fp = fopen(strcat(nav,".pgm"),"w"); /* and write out display */
	  nav[navl] = 0;
	  pgm_writepgm(fp,picout,sx,sy,256,0);
	  fclose(fp);
      };
    pgm_freearray(picout, sy );
};

void DisplayMapSetX(Map3D map) 
  /* Make display images for vertical map slices along Y direction */
{
    gray **picout;	/* display picture, with correlation graphics */
    int ix, iy, iz, mx, my, mz, lx, ly, t, t1, tick, i, wad;
    int xplane, frame, xpos, ypos, sx, sy;
    double dy, dz;
    FILE *fp;
    
    /* Create map slice display image array */

    mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];           
    lx = ILOG2C(mx);
    ly = ILOG2C(my)+lx;

    picout = pgm_allocarray(sx = (mx+1)*4+1, sy = (mz+1)*12+1);

    for (wad = 0; wad < my/48;  wad++)
      {
	  for (ix = 0; ix < sx; ix++)  for (iy = 0; iy < sy; iy++)
	    picout[iy][ix] = 0;
	  
	  for (frame = 0; frame < 48; frame ++)
	    {
		xpos = (frame % 4)*(mx+1)+1;
		ypos = (frame / 4)*(mz+1)+1;
		xplane = wad*48+frame;
		
		for (iz = 0; iz < mz; iz++) for (iy = 0; iy < my; iy++)
		  {
		      t = map.mapm[(iz<<ly)+(iy<<lx)+xplane];
		      picout[ypos+mz-1-iz][xpos+iy] = PRB(t)>>23;
		  };
	    };

	  for (frame = 0; frame < 48; frame ++)
	    {
		xpos = (frame % 4)*(my+1)+1;
		ypos = (frame / 4)*(mz+1)+1;
		dy = map.himv[1]-map.lomv[1];
		dz = map.himv[2]-map.lomv[2];
		
		for (tick=1; tick<=dy-1; tick++)
		  { iy = my*tick/dy;
		    for (i = 0; i < 4; i++)
		      picout[ypos+i][xpos+iy-1] = 
			picout[ypos+mz-i-1][xpos+iy-1] = 0; }
		
		for (tick=1; tick<=dz-1; tick++)
		  { iz = mz-1-mz*tick/dz;
		    for (i = 0; i < 8; i++)
		      picout[ypos+iz-1][xpos+i] = 
			picout[ypos+iz-1][xpos+mx-i-1] = 0; }
	    };

	  strcat(nav,".Xslices");  t1 = strlen(nav);
	  nav[t1] = '0'+wad;  nav[t1+1] = 0;
	  
	  fp = fopen(strcat(nav,".pgm"),"w"); /* and write out display */
	  nav[navl] = 0;
	  pgm_writepgm(fp,picout,sx,sy,256,0);
	  fclose(fp);
      };
    pgm_freearray(picout, sy );
};


void DisplaySModels(CylSensorModelArray smodels) 
  /* Make display image for the sensor model array */
{
    gray **picout;	/* display picture, with correlation graphics */
    int i, j, rtot, dmax, rs, ds, ri, di, t, log2rs;
    FILE *fp;
    
    /* Create sensor model display image array */

    rtot = 1;  dmax = 0;
    for (i=0; i<smodels.nmodels; i++)
      {
	  rtot += smodels.models[i].rsize + 1;
	  ds = smodels.models[i].dsize;  if (ds>dmax) dmax = ds;
      };

    /*  printf("dmax = %d, rtot = %d\n",dmax,rtot);  */

    picout = pgm_allocarray(rtot,dmax);
    for (i=0; i<dmax; i++) for (j=0; j<rtot; j++) picout[i][j]=0;

    rtot = 1;
    for (i=0; i<smodels.nmodels; i++)
      {
	  rs = smodels.models[i].rsize;
	  ds = smodels.models[i].dsize;
	  log2rs = ILOG2C(rs);
	  for (ri = 0; ri < rs; ++ri)
	  {
	    for (di = 0; di < ds; ++di)
	      {
		t = smodels.models[i].modelm[(di<<log2rs)+ri];
		picout[di][rtot] = PRB(t)>>23;
	      }
	    rtot++;
	  }
	  rtot++;
      };

    fp = fopen(strcat(nav,".smodels.pgm"),"w"); /* and write out display */
    nav[navl] = 0;
    pgm_writepgm(fp,picout,rtot,dmax,256,0);
    fclose(fp);
    pgm_freearray(picout, dmax );
    
};


int main(argc, argv)
int argc;
char *argv[];
{
    Calib CalL, CalR;  /* calibration parameters and pointers */

    Map3D map;                 /* 3D evidence grid map */
    CylSensorModelArray smd;    /* Prestored sensor model array */
    MainInt msize[3];           /* 3D grid parameters, grid size */
    MainFloat lov[3],  hiv[3],  /* grid physical dimensions */
              PosL[3], PosR[3], Dir[3],   /* evidence ray position, direction */
              rng;              /* evidence ray range */

    /* Camera setup parameters, nominal position is stereo pair midpoint */ 
    MainFloat CamSep,           /* left/right camera separation, in meters */
              CamPos[3], CamHead, CamTilt,  /* camera position and orientation */
              CamFocal,         /* focal length of camera, in pixels */
              CamClose = 1;     /* minimum range to examine, in camera separations */

    MainFloat Feat[3];          /* feature position, in camera frame */
    int time1, timeR, timeI, timeC, timeT, TtimeR, TtimeI, TtimeC, TtimeT, npix;
    int i, j, k, p, intcnt, scani, scanj;
    gray maxval;  int format, rows, cols;   
    int sil, sjl, sih, sjh, bestj, raycnt, eye, eye2, jposL, jposR;
    int ii, jj, sum;
    MainFloat t;
    char calnam[200], calnamL[200], calnamR[200], str[200];
    FILE *fp, *navp;
    float t1, t2, t3, t4, t5, t6;
    MainFloat sinH, cosH, sinT, cosT;  /* sin and cos of camera heading and tilt */

    double VL = 0, VR = 0, ML = 0, MR = 0, nMV = 0;    /* picture mean and varance */

    InitCmacs();

    if (argc != 2)
    {
    printf("Usage: crayfish run.nav\n");
    printf("run.nav is a navigation file, with pointers to camera calibration files\n");
    printf("and a sequence of camera positions and stereo pair file names\n");
    exit(0);
    };

    strcpy(nav,argv[1]); navl = strlen(nav);  nav[navl] = 0;
    pgm_init(&argc, argv);
    ppm_init(&argc, argv);

    /* Open navigation file */
    if ((navp = fopen(strcat(nav,".nav"),"r")) == NULL)
      {printf("No %s file!!\n",nav); exit(0);};
    nav[navl] = 0;

    /* Read calibration files */
    fscanf(navp,"%s\n",calnam);
    strcpy(calnamL,calnam); strcat(calnamL,".L.calib");
    strcpy(calnamR,calnam); strcat(calnamR,".R.calib");
    if (!FishHead(calnamL, &CalL) || !FishHead(calnamR, &CalR))
      {printf("%s %s calibration files didn't work!\n",calnamL,calnamR); exit(0);};
    if ((Picw=CalL.Pw) != CalR.Pw || (Pich=CalL.Ph) != CalR.Ph)
      { printf("L&R calibration sizes [%d,%d], [%d,%d] not equal!!\n",
	      CalL.Ph,CalL.Pw,CalR.Ph,CalR.Pw);  exit(0); };
 
    /* Create source and rectified image arrays */
    picL = pgm_allocarray(Picw,Pich);
    picR = pgm_allocarray(Picw,Pich);
    recL = pgm_allocarray(Picw,Pich);
    recR = pgm_allocarray(Picw,Pich);

    /* check for array contiguity -- needed for fast access */
    for (i=1; i<Pich; i++)
      if (recL[i]-recL[i-1] != Picw || recR[i]-recR[i-1] != Picw)
	{ printf("pgm returned non-contiguous rows - can't use!\n"); exit(0); };

    /* Build rectification tables */
    time1 = -clock() ;  /* time the rectification precalculation */
    if (!FishBody(&CalL,picL) || !FishBody(&CalR,picR))
      {printf("No room for rectification tables!\n"); exit(0); };
    time1 += clock() ;
    printf("L+R Rectification setup time: %g s\n", time1/1e6);
	  
    /* set up camera parameters and sensor model */
    t1 = 0.3;   /* camera separation, in meters */
    t2 = 0;     /* camera downward tilt, in degrees */
    t3 = 1.25;  /* camera height, in meters */
    fscanf(navp,"%g%g%g\n",&t1,&t2,&t3);
    CamSep = t1;    CamTilt = t2*PI/180;    CamPos[2] = t3;
    sinT = SIN(CamTilt);   cosT = COS(CamTilt);

    CamClose = 5;  /* closest point to search, in camera separations */
    
    /* calculate correlation bounds from camera pair geometry */
    CamFocal = (Picw/2)/TAN((PI/180)*CalL.FOV/2);  /* camera focal length, in pixels */
    joffset = CamFocal/CamClose;
    printf("FOV %g, CamFocal = %g, CamClose = %g, joffset = %d\n",
	   CalL.FOV,CamFocal,CamClose,joffset);
    

    t1 = -4;  t2 = 4;  t3 = -4;  t4 = 4;  t5 = 0;  t6 = 2;
   fscanf(navp,"%g%g%g%g%g%g\n",&t1,&t2,&t3,&t4,&t5,&t6);

    /* set up 3D map */
    msize[0] = 256;  lov[0] = t1;  hiv[0] = t2;   /* meters wide */
    msize[1] = 256;  lov[1] = t3;  hiv[1] = t4;   /* meters deep */
    msize[2] = 64;   lov[2] = t5;  hiv[2] = t6;   /* meters high */
    MakeMap3D(msize, lov, hiv, &map);

    printf("\nMap[%.3g:%.3g,%.3g:%.3g,%.3g:%.3g]\n",
	  lov[0],hiv[0],lov[1],hiv[1],lov[2],hiv[2]);

    /* Set up sensor model */
    MakeCylModelStereo(Picw, CamFocal, CamSep,
		       (hiv[0]-lov[0])/msize[0],
		       2.0*(hiv[0]-lov[0]), -2, 16, &smd);  /* make sensor model */
    TrimCylModel(smd);    /* trim away any blank edges */
    
    DisplaySModels(smd);  /* Make display picture of sensor models */

    npix = TtimeR = TtimeI = TtimeC = TtimeT = 0;   /* accumulate nav file total times */

    while (fgets(str,100,navp) != NULL &&
	   ((str[0]>='0' && str[0]<='9') || str[0]=='-'))
      /* Run through nav file */
      {
	  unsigned short int meanL[Pich*Picw], meanR[Pich*Picw];

	  sscanf(str,"%g%g%g %s\n",&t1,&t2,&t3,img);
	  CamHead = t1*PI/180;  CamPos[0] = t2;  CamPos[1] = t3;
	  imgl = strlen(img);
	  printf("\nPan, Tilt %.3g %.3g deg, Position %.3g %.3g %.3g m, image %s\n",
		 CamHead*180/PI,CamTilt*180/PI,CamPos[0],CamPos[1],CamPos[2],img);

	  sinH = SIN(CamHead);   cosH = COS(CamHead);

	  /* Read in raw images */
	  fp = fopen(strcat(img,".L.pgm"),"r");  img[imgl]=0;
	  if (fp == NULL) {printf("No %s file!!\n",img); exit(0); };
	  pgm_readpgminit(fp, &cols, &rows, &maxval, &format );
	  Picw = cols;   Pich = rows;   Picmax = maxval;
	  if (CalL.Ph != Pich || CalL.Pw != Picw)
	    {printf("Calibration size [%d,%d] not equal Image size [%d,%d]!\n",
		   CalL.Ph,CalL.Pw,Pich,Picw);    exit(0); };
	  for (i = 0; i < rows; i++)
	    pgm_readpgmrow(fp,picL[i],cols,maxval,format);
	  fclose(fp);

	  fp = fopen(strcat(img,".R.pgm"),"r");  img[imgl]=0;
	  if (fp == NULL) {printf("No %s file!!\n",img); exit(0); };
	  pgm_readpgminit(fp, &cols, &rows, &maxval, &format );
	  Picw = cols;   Pich = rows;   Picmax = maxval;
	  if (CalL.Ph != Pich || CalL.Pw != Picw)
	    {printf("Calibration size [%d,%d] not equal Image size [%d,%d]!\n",
		   CalL.Ph,CalL.Pw,Pich,Picw);    exit(0); };
	  for (i = 0; i < rows; i++)
	    pgm_readpgmrow(fp,picR[i],cols,maxval,format);
	  fclose(fp);

	  /* apply rectifications */
	  timeR = -clock();
	  FishTail(picL, &CalL,recL);
	  FishTail(picR, &CalR,recR);
	  timeR += clock();

	  /* apply Interest Operator to left and right images */
	  timeI = -clock();
	  top = HInterest(recL,Pich,Picw,intopL);
	  top = HInterest(recR,Pich,Picw,intopR);
	  timeI += clock();
	  
	  DisplayInterest();  /* Make interest operator display image */
	  DisplayCorrelate(); /* And display the correlation of just one feature */

	  /* apply brightness and contrast adjustments */
if(0)	  for (i = 0; i < Pich; i++) for (j = 0; j < Picw; j++)
	    {
		p = 1.087*recR[i][j] - 8.33;
		recR[i][j] = p<0?0:p>255?255:p;
	    };

	  /* accumulate contrast and brightness information */
	      for (i = Pich/8;  i < Pich/2; i++)
		for (j = 0;  j < Picw; j++)
		  {
		      ML += t = recL[i][j];  VL += t*t;
		      MR += t = recR[i][j];  VR += t*t;  nMV++;
		  };

	  /* compute correlation window means (slow, simple now - sliding later) */
	  for (i = 3;  i < Pich-3; i++) for (j = 3; j < Picw-3; j++)
	    { 
		sum = 0;
		for (ii = -3; ii <= 3; ii++) for (jj = -3; jj <=3; jj++)
		  sum += recL[i+ii][j+jj];
		meanL[k = i*Picw+j] = sum;

		sum = 0;
		for (ii = -3; ii <= 3; ii++) for (jj = -3; jj <=3; jj++)
		  sum += recR[i+ii][j+jj];
		meanR[k] = sum;
	    };

	  /* set up positions for ray throwing */
	  PosL[0] = CamPos[0] - sinH*CamSep/2;
	  PosL[1] = CamPos[1] + cosH*CamSep/2;
	  PosL[2] = CamPos[2];

	  PosR[0] = CamPos[0] + sinH*CamSep/2;
	  PosR[1] = CamPos[1] - cosH*CamSep/2;
	  PosR[2] = CamPos[2];

	  raycnt = 0;
	  timeC = timeT = 0;
	  intcnt = 0;
	  
	  for (eye = 0; eye <= 1; eye++)  /* right eye, then left */
	    {
		fudge = eye?fudge1:fudge0;
		
		/* find features matching left interest points in right image */
		p = 0;
		for (i = 0; i < Pich-7; i += 8) for (j = 0; j < Picw-7; j += 8)	
		  if((eye?intopL[p++]:intopR[p++]) > 0
		     && (eye?(j>Picw/2):(j<Picw/2))) 
		    {	
			timeC -= clock(); /* time correlations */
			intcnt++;
			scani = i+4;  scanj = j+4;
			/* Correlate !! */
			bestj = 
			  F2Correl7(eye?meanL:meanR, eye?meanR:meanL,
				  eye?recL:recR, scani, scanj, Pich, Picw,
				  eye?recR:recL,
				  sil = sih = scani+fudge,
				  sjl = eye?scanj-joffset:scanj,
				  sjh = eye?scanj:scanj+joffset,
				  correl);
			timeC += clock(); /* time correlations */
			
			/* throw found feature into 3D grid */
			timeT -= clock(); /* time ray throwing */

			jposL = eye?scanj:bestj;   jposR = eye?bestj:scanj;
			
			for (eye2 = 1; eye2 >= 0; eye2--) /* throw from each eye */
			  {
			      /* feature position, in camera coordinates */
			      t=jposL-jposR; if(t<=0) t=1;  t /= CamSep;
			      Feat[0] = CamFocal/t;        /* forward */
			      Feat[1] = ((eye2?jposL:jposR)-Picw/2)/t;  /* lateral */
			      Feat[2] = (Pich/2-scani)/t;  /* height */
			      
			      /* ray throwing direction, in map coordinates */
			      t = (Feat[0]*cosT+Feat[2]*sinT);
			      Dir[0]= t*cosH+Feat[1]*sinH;
			      Dir[1]= t*sinH-Feat[1]*cosH;
			      Dir[2]= Feat[2]*cosT - Feat[0]*sinT;
			      rng = SQRT(Feat[0]*Feat[0]
					 +Feat[1]*Feat[1]+Feat[2]*Feat[2]);
			      raycnt++;

			      /* Throw the Ray !! */
			      if (rng>0.5 && rng<30.0)
				AddCylReading(rng,eye2?PosL:PosR,Dir,smd,map);
			  };
			timeT += clock();
		    }; 
		
	    };
	  npix++;
	  TtimeR += timeR; TtimeI += timeI; TtimeC += timeC; TtimeT += timeT;
	  printf("            Timing for %d features(Seconds)\n",intcnt);
	  printf("Rectification   Interest Op     Correlation     Ray Throw\n");
	  printf("%8.3g%16.3g%16.3g%16.3g\n",
		 timeR/1e6,timeI/1e6,timeC/1e6,timeT/1e6);
      };

    /* Print contrast and brightness corrections */
    VL /= nMV;  VR /= nMV;  ML /= nMV;  MR /= nMV;
    VL = SQRT(VL - ML*ML);  VR = SQRT(VR - MR*MR);
    printf("Ladj  %.3g p + %.3g    Radj  %.3g p + %.3g\n",
	   VL,ML,VR,MR);
    printf("R->L fix  %.3g R + %.3g\n",VL/VR,ML-VL*MR/VR);

    printf("            Average over %d stereo pairs (Seconds)\n",npix);
    printf("Rectification   Interest Op     Correlation     Ray Throw\n");
    printf("%8.3g%16.3g%16.3g%16.3g\n",
	   TtimeR/(npix*1e6),TtimeI/(npix*1e6),TtimeC/(npix*1e6),TtimeT/(npix*1e6));

    /* write out 3D map */
    WriteMap3Db(map,"DB Room", "", strcat(nav,".map3D"));  nav[navl] = 0;


    /* compute map statistics */

    {	/* grid and floor cell counts */
	int npos, nneg, nzer, nposZ[map.msize[2]], ix, iy, iz, mx, my, mz, lx, ly;
	mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];           
	lx = ILOG2C(mx);
	ly = ILOG2C(my)+lx;
	npos = nneg = nzer = 0;  for (iz = 0; iz < mz; iz++) nposZ[iz] = 0;
	for (iy = 0; iy < my; iy++) for (ix = 0; ix < mx; ix++)
	  for (iz = 0; iz < mz; iz++)
	    {
		t1 = map.mapm[(iz<<ly)+(iy<<lx)+ix];
		if (t1<0) nneg++; else { if (t1>0) npos++; else nzer++; };
		if (t1>0) nposZ[iz]++;
	    };
	printf ("\n Map counts (+ -):  %d %d, (z0 z1 z2): %d %d %d = %d\n",
		npos, nneg, nposZ[0],nposZ[1],nposZ[2], nposZ[0]+nposZ[1]+nposZ[2]);
    };

    /* count + cells in uncolored space */
    if (ReadColorization(nav,map))
      {
	  int mx, my, mz, lx, ly, ix, iy, iz, cl, clin, clout, term;

	  mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];           
	  lx = ILOG2C(mx);
	  ly = ILOG2C(my)+lx;


	  clin = clout = 0;   /* cell count inside and outside object regions */

	  for (iz = 0; iz < mz; iz++) for (iy = 0; iy < my; iy++)
	    for (ix = 0; ix < mx; ix++)
	      if (map.mapm[(iz<<ly)+(iy<<lx)+ix] > 0)
		{
		    term = 1;
		    for (cl = 1; cl < ncl; cl++)
		      if (ix>=cllo[0][cl] && ix<=clhi[0][cl] &&
			  iy>=cllo[1][cl] && iy<=clhi[1][cl] &&
			  iz>=cllo[2][cl] && iz<=clhi[2][cl] )
			{    term = 0;  clin++; break; };
		    clout += term;
		};
	  printf("%8d in objects, %d pigs in space, %d score\n",
		 clin,clout, clin-clout);
      };


    exit(0);

};







double FilterMap(Map3D map, int *adds, int *subs)  /* compute a score for a map */
{
    char scratch[500];
    Map3D Counts;
    MainInt mx, my, mz, lx, ly, lz;  /* Map access constants */
    MainInt loc, hic, msize, psize, rsize;
    MainInt i, iter, magc, magm, c, m;
    Int16  *mmapm;				/* map array */
    Int16  *countm;
    
    WriteMap3Db(map,"temp","","temp.map3D");
    MakeMap3D(map.msize, map.lomv, map.himv, &Counts);

    mmapm = map.mapm;
    countm = Counts.mapm;
    mx = map.msize[0];  my = map.msize[1];  mz = map.msize[2];
    lx = ILOG2C(mx);   ly = ILOG2C(my)+lx;   lz = ILOG2C(mz)+ly;

    rsize = mx;		/* row size   */
    psize = rsize*my;	/* plane size */
    msize = psize*mz;	/* map size   */

    loc =  1 + rsize + psize;
    hic = msize - loc;

#define m2(b,d) mmapm[(b)+(d)] + mmapm[(b)-(d)]
#define m8(b,h,v) m2(b,h) + m2(b,v) + m2(b,h+v) + m2(b,h-v)

    *adds = *subs = 0;			/* number of cells that needed changing */
    for (iter = 0; iter < 1; iter++)
      {
	  /* count neighborhood */
	  for (i = loc; i < hic; i++)  countm[i] =
	    (m2(i,1)+m8(i-1,rsize,psize)+m8(i,rsize,psize)+m8(i+1,rsize,psize))/26;

	  /* remove and score sign changing anomalies */
	  for (i = loc; i < hic; i++)
	    {
		magc = c = countm[i];  if (magc<0) magc = -magc;
		magm = m = mmapm[i];   if (magm<0) magm = -magm;
		if (magc>magm) { if (c>0 && m<0) (*adds)++;
				       else if (c<0 && m>0) (*subs)++; 
				 mmapm[i] = c; };
	    };
      };

#undef m8
#undef m2

    FreeMap3D(Counts);
    ReReadMap3Db("temp.map3D",&map,scratch,scratch);
    return((double) 0.0);
};

/***********************************************************************/

void MapColors()    
{
    /* count + cells in colored and uncolored space */
    int cl, clin, clout, isin, adds, subs;
    int  thresh, bestin, bestout, bestthresh;
    double score, bestscore;
    
    /* locate best threshold */		
    bestin = bestout = 0;  bestthresh = 0;  bestscore = -1e7;
    for (thresh = 0; thresh < 1; thresh++)
      {
	  /* cell count inside and outside object regions */
	  clin = clout = 0;
	  
	  for (iz = 0; iz < mz; iz++) for (iy = 0; iy < my; iy++)
	    for (ix = 0; ix < mx; ix++)
	      if (map.mapm[(iz<<ly)+(iy<<lx)+ix] > thresh)
		{
		    isin = 0;
		    for (cl = 1; cl < ncl; cl++)
		      if (ix>=cllo[0][cl] && ix<=clhi[0][cl] &&
			  iy>=cllo[1][cl] && iy<=clhi[1][cl] &&
			  iz>=cllo[2][cl] && iz<=clhi[2][cl] )
			{    isin = 1;  break; };
		    if (isin) clin++; else clout++;
		};
	  FilterMap(map, &adds, &subs);  score = -adds;
	  if (score > bestscore) {bestscore = score;
				  bestthresh = thresh;
				  bestin = clin;
				  bestout = clout; };
	  if (Trace>5)
	    {
		printf("Thresh %d  IO(%d %d) Score  %g\n",
		       thresh,clin,clout,score);
	    };
      };
    
    if (bestscore > topscore)
      {
	  int l;
	  topscore = bestscore;  topin = bestin;  topout = bestout;
	  topevE = evE;  topevO = evO;  topthresh = bestthresh;
	  
	  sprintf(topstr,"Cwt %4.2g Ev(%d %d) -> io(%d %d) [+%d -%d] %g",
		  Corwtscale, topevE, topevO,
		  topin, topout, adds, subs, topscore);
	  printf("*** %s *** 	\n",topstr);
	  /* threshold map to give best score */
	  for (iz = 0; iz < mz; iz++) for (iy = 0; iy < my; iy++)
	    for (ix = 0; ix < mx; ix++)
	      if (map.mapm[l = (iz<<ly)+(iy<<lx)+ix] > 0)
		map.mapm[l] = map.mapm[l] <= topthresh ? 0 :
		  map.mapm[l] - topthresh;
	  
	  /* write out 3D map */
	  WriteMap3Db(map,"DB Room", topstr, strcat(nav,".lrn.map3D"));
	  nav[navl] = 0;
      }
    else printf("    Cwt %4.2g Ev(%d %d) -> io(%d %d) [+%d -%d] %g		\n",
		Corwtscale, evE, evO,
		bestin, bestout, adds, subs, bestscore);
};