BEGIN "Crash"
DEFINE !="COMMENT ";

REQUIRE "VIXHDR.SAI[VIS,HPM]" SOURCE_FILE; ! display and utility pic routines;
REQUIRE "REDPIC.SAI[VIS,HPM]" SOURCE_FILE; ! reduced picture routines;
REQUIRE "DISTOR.HDR[VIS,HPM]" SOURCE_FILE; ! camera calibration routines;
REQUIRE "TELHDR.SAI[CAR,HPM]" SOURCE_FILE; ! remote control package;
REQUIRE "⊂⊃<>" DELIMITERS;
REQUIRE "CRASHS.SUB[MIS,HPM]" SOURCE_FILE; ! the code for this program;

DEFINE PI="3.14159265";
REAL PROCEDURE TAN(REAL X); RETURN(SIN(X)/COS(X));



!  OUTLINE
!
!WHILE TRUE DO
!   Take a picture series
!   apply correlator
!   if not enough features
!      apply interest operator
!   solve for 3D location of all features
!   calculate cart motion from features in common with previous position
!   try again for, or drop, inconsistent matches
!   insert new features in world model
!   calculate path around obstacles
!   run forward
!;

!  DATA STRUCTURE
!
!   each feature is identified by
!    a) its appearance, a series of identically sized
!     windows of representing successively smaller regions around
!     the point of interest at successively higher resolutions.
!    b) its location in world 3 space, with an uncertainty
!
!  The world is represented by a list of feature positions with error ellipses
!  this list grows steadily as the interest operator acquires new features.
!  A feature is removed only when its position uncertainty becomes too large;




! FEATURE INFORMATION (PREFIX FEAT)

                                                 PERMANENT        TEMPORARY
                 POINTER ARRAYS                 DATA ARRAYS      DATA ARRAYS

	     WORLD   CAMERA    FREE            IMAGE,X,Y,Z       FX,FY,FZ,FER
                                               ERROR
	    _______  _______  _______         ____________________________
	    |     |  |     |  |     |         |               
1	    | W1  |  | C1  |  | F1  |         |               
	    |_____|  |_____|  |_____|         |________________________
	    |     |  |     |  |     |         |               
2	    | W2  |  | C2  |  | F2  |         |               
	    |_____|  |_____|  |_____|         |________________________
	    |     |  |     |  |     |         |               
3	    | W3  |  | C3  |  | F3  |         |               
	    |_____|  |_____|  |_____|         |________________________
	    |     |  |     |  |     |         |               
4	    | W4  |  |  0  |  | F4  |         |               
	    |_____|  |_____|  |_____|         |_______________________
	    |     |  |     |  |     |         |               
5	    | W5  |  |  0  |  |  0  |         |               
	    |_____|  |_____|  |_____|         |_______________________
	    |     |  |     |  |     |         |               
	       0  |     0  |     0  |                         
	      ...      ...                      ...      ...
			      
	    |_____   |_____   |_____          |______________________
	    |     |  |     |  |     |         |               
FEAT_MAX-2  |  0  |  |  0  |  |  0  |         |               
	    |_____|  |_____|  |_____|         |______________________
	    |     |  |     |  |     |         |               
FEAT_MAX-1  |  0  |  |  0  |  |  0  |         |               
	    |_____|  |_____|  |_____|         |_______________________
	    |     |  |     |  |     |         |               
FEAT_MAX    |  0  |  |  0  |  |  0  |         |               
	    |_____|  |_____|  |_____|         |________________________

;

   BEGIN "Definitions"
   DEFINE FEAT_MAX=40;  ! Maximum number of features to maintain;
   DEFINE FEAT_MIN=20;  ! Minimum number of features to maintain;
   DEFINE FEAT_WIN=8;   ! Feature description window size;
   INTEGER FEAT_WORLD_N;! Number of features currently stored;
   INTEGER FEAT_CAMERA_N;! Number of features currently stored;
   INTEGER FEAT_FREE_N; ! Number of features currently stored;
   INTEGER FEAT_3D;	! Number of features whose 3d position is known;
   REAL CART_AA,CART_BA,CART_CA,CART_AB,CART_BB,CART_CB,CART_AC,CART_BC,CART_CC;
   REAL CART_XC,CART_YC,CART_ZC; ! rotation and position of cart in world co-ords;

   DEFINE INTOP_WIN=6;  ! Interest operator window size;

   REAL ARRAY CAL_G,CAL_IG[1:44];              ! distortion correction;
   REAL CAL_PIXEL;			       ! undistorted pixel width;
   INTEGER CAL_HIG,CAL_WID,CAL_P;              ! distorion corrector test pattern;

   DEFINE  CAM_SUM=1;                                ! Number of frames to average;
   INTEGER CAM_N; INTEGER ARRAY CAM_ID,CAM_TCL,CAM_BCL[1:9]; ! camera parameters;
   INTEGER CAM_YLO, CAM_XLO;                         ! corner of digitized image;
   REAL CAM_TILT;			    ! camera's inclination from vertical;

   REAL SCAN_STEPTIME;                         ! camera scan parameters;
   INTEGER SCAN_NPICS,SCAN_MIDPIC,SCAN_STEPSIZE;

   INTEGER ARRAY PIC_DIM[0:10];		       ! input picture header;
   BOOLEAN PIC_SYNC;			       ! fix vertical sync?;
   INTEGER PIC_HIG,PIC_WID,PIC_BIT;            ! Input picture dimensions;

   STRING IPIC_NAME; INTEGER IPIC_LO,IPIC_HI;  ! File input parameters;
   STRING OPIC_NAME; INTEGER OPIC_LO,OPIC_HI;  ! File output parameters;

   INTEGER DIS_WID,PDPY,DIS_CH1,DIS_TCH,DIS_SCH,DIS_PIC;
   BOOLEAN DIS_SYN;

!  initialize ;

   GET_PICTURE_SOURCE;
   GET_CALIBRATION_FILE;
   GET_OUTPUT_FILENAMES;
   GET_DISPLAY_PARAMETERS;

      BEGIN "run time arrays"
      INTEGER ARRAY FEAT_WORLD,FEAT_CAMERA,FEAT_FREE[1:FEAT_MAX];
      INTEGER ARRAY
	 FEAT_IMAGE[1:FEAT_MAX, 0:WINDIM(PIC_HIG,PIC_WID,PIC_BIT,FEAT_WIN)];
      REAL ARRAY  FEAT_X,FEAT_Y,FEAT_Z, FEAT_ERROR[1:FEAT_MAX];
	    ! The feature descriptions, what each looks like,
               where it is, in world co-ords, with what uncertainty;

	 BEGIN "at each position"
	 INTEGER ARRAY PIC[1:SCAN_NPICS, 0:REDDIM(PIC_HIG,PIC_WID,PIC_BIT)];
	 REAL ARRAY IMAGY,IMAGX,IMAGQ[1:SCAN_NPICS, 1:FEAT_MAX],
                    FX,FY,FZ,FER[1:FEAT_MAX];
         ! feature position, correlation quality and cam-coord 3D position;
         INITIALIZE_WORLD;
        
         WHILE TRUE DO
            BEGIN
	    SCAN_THE_CAMERA;

   !        DISPLAY_SCAN_SET;
            DISPLAY_PICTURE;

	    REACQUIRE_POINTS;   ! into feat_world;
            MATCH_POINTS(<FEAT_WORLD>);
	    CALCULATE_3D_POSITIONS(<FEAT_WORLD>);
	    DISPLAY_DISTANCES(<FEAT_WORLD>);

	    PRUNE_IN_3D;
            RELATE_CAMERA_TO_WORLD;

            DISPLAY_PLAN_VIEW;

	    PICK_NEW_POINTS;   ! into feat_camera;
	    MATCH_POINTS(<FEAT_CAMERA>);
	    CALCULATE_3D_POSITIONS(<FEAT_CAMERA>);
	    DISPLAY_DISTANCES(<FEAT_CAMERA>);
            CONVERT_TO_WORLD_COORDS; ! into feat_world;

            DISPLAY_PLAN_VIEW;

	    EXECUTE_PATH;
            END;
    
	 END "at each position";
      END "run time arrays";
   END "Definitions";
END "Crash";