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";