/* 3D Map and Cylindrical Sensor Model Header File */ /* #define volsenseDBUG 1 */ /* Cylindrical cut sensor model */ #include "customize.h" typedef struct {Float32 rangem; /* maximum range reading of this entry */ Int32 rsize, dsize; /* size in cells of sensor model grid */ Float32 lorv, lodv, hirv, hidv; /* bounds (in m) of sm grid */ Int8 *modelm; /* the model grid: dsize*rsize cells */ Int16 *profile; /* r at each d: dsize cells */ } CylSensorModel; typedef struct {MainInt nmodels; /* how many models in this array */ CylSensorModel *models; /* array of sensor models */ } CylSensorModelArray; typedef struct {MainInt msize[3]; /* cells in each dimension of map */ MainFloat lomv[3], himv[3]; /* co-ords (in meters) of grid */ Int16 *mapm; /* the map: Prod(msize) cells */ } Map3D; /* the following are the array indices for the sensor model parameters (not their values). In each triplet, the first (v0) is the value of the parameter at the sensor mouth. the second (vI) is the value infinitely far ahead of the sensor. The third paramater (vs) is the RECIPROCAL of the distance scale at which the parameter changes from the v0 value to the vI value, usually according to the formula v = vI + (v0-vI)/(d*vs + 1), where d is the distance from the sensor. */ #define NP (5*3) /* Number of parameters */ #define em0 0 /* depth of empty area at z = 0 */ #define emI (em0+1) /* depth of empty area at z = inf */ #define ems (em0+2) /* 1/characteristic_length from em0 to emI */ #define oc0 3 /* height of occupied ridge */ #define ocI (oc0+1) #define ocs (oc0+2) #define an0 6 /* tan of beam half-angle */ #define anI (an0+1) #define ans (an0+2) #define ru0 9 /* range uncertainty */ #define ruI (ru0+1) #define rus (ru0+2) #define ro0 12 /* range offset */ #define roI (ro0+1) #define ros (ro0+2) #ifndef is_volsense /* Routines defined in volsense.c */ extern MainInt LfP(MainFloat p); extern MainInt ILOG2(MainInt i); extern MainInt ILOG2C(MainInt i); extern MainInt ILOG2R(MainInt i); extern MainInt MakeMap3D(MainInt msize[3], MainFloat lov[3], MainFloat hiv[3], Map3D *map); extern void FreeMap3D(Map3D map); extern void AddCylReading(MainFloat range, MainFloat Pos[3], MainFloat Dir[3], CylSensorModelArray smodels, Map3D map); extern void ReadCylModel(char *sfilename, CylSensorModelArray *smodels); extern void WriteCylModel(CylSensorModelArray smodels, char *sfilename); extern void TraceCylModel(CylSensorModelArray smodels, char *sfilename); extern void ClearMap3D(Map3D map); extern void WriteMap3D(Map3D map, char *title, char *footer, char *mfilename); extern MainInt ReadMap3D(char *mfilename, Map3D *map, char *title, char *footer); extern void WriteMap3Db(Map3D map, char *title, char *footer, char *mfilename); extern MainInt ReadMap3Db(char *mfilename, Map3D *map, char *title, char *footer); extern MainInt ReReadMap3Db(char *mfilename, Map3D *map, char *title, char *footer); extern void InitCylModelParams(void); extern void MakeCylModel(MainInt nmods, MainFloat cellsize, MainInt rszmax, MainInt dszmax, MainFloat rngl, MainFloat rngh, CylSensorModelArray *smodels); extern void MakeCylModelStereo(MainInt PicWidth, MainFloat FocalLength, MainFloat CamSeparation, MainFloat Cellsize, MainFloat MaxRange, MainInt EvLo, MainInt EvHi, CylSensorModelArray *smodels); extern void TrimCylModel(CylSensorModelArray smodels); extern void FreeCylModel(CylSensorModelArray smodels); extern MainFloat MatchMap3D(Map3D map1, Map3D map2); extern int SimulSolve(MainFloat a[], MainInt n); extern BestFloat P[NP], Pl[NP], Ph[NP]; /* evidence function parameters */ extern MainFloat ACRtsetup, ACRtfill; /* evidence ray timing */ extern MainInt ACRhits, ACRrays; /* evidence ray touched pixels */ extern int logoddtab[]; extern unsigned int toprobtab[], lep1[]; #endif /* not is_volsense */ #ifdef is_volsense BestFloat P[NP], Pl[NP], Ph[NP]; /* Sensor evidence function parameters */ MainFloat ACRtsetup, ACRtfill; MainInt ACRhits=0, ACRrays=0; unsigned int toprobtab[129]= {0,1,1,1,1,1,1,2,2,2,3,3,4,5,6,7,8,10,11,13,16,19,23,27,32,38,45,54,64,76, 91,108,128,152,181,215,256,304,362,431,512,609,724,861,1024,1218,1448,1722, 2048,2435,2896,3444,4096,4871,5793,6889,8192,9742,11585,13777,16384,19484, 23170,27554,32767,38967,46340,55108,65534,77933,92678,110212,131064,155860, 185348,220413,262112,311698,370664,440781,524160,623306,741199,881382, 1048064,1246250,1481887,1762041,2095106,2491055,2961730,3521192,4186128, 4976338,5915303,7030856,8355967,9929666,11798107,14015824,16647160,19767927, 23467287,27849882,33038210,39175240,46427226,54986663,65075262,76946786, 90889480,107227742,126322567,148570136,174397798,204256584,238609294, 277913296,322597366,373032419,429496729,492137387,560931050,635648414, 715827882,800764184,889516852,980941288,1073741823}; unsigned int lep1[]= {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,2,2,2,3,3,4,5,6,7,8, 10,11,14,16,19,23,27,32,38,45,54,64,76,90,106,125,148,174,205,240,282,330, 385,447,518,599,689,790,902,1024,1158,1302,1457,1623,1798,1983,2177,2378, 2586,2800,3021,3246,3476,3709,3946,4186,4428,4672,4918,5165,5414,5664, 5915,6167,6419,6672,6926,7179,7434,7688,7943,8198,8453,8708,8963,9219, 9474,9730,9986,10241,10497,10753,11009,11265,11521,11777,12032,12288, 12544,12800,13056,13312,13568,13824,14080,14336,14592,14848,15104,15360, 15616,15872,16128,16384}; #endif #define FXP(x) ROUND((1<<16)*((MainFloat) (x))) /* mid point fixed point conversion */ /* log odds version of probabilities nearly 1 and nearly 0 */ #define POS (127) #define NEG (-127) /* PRB(x) converts log odds to integer 2^31*probability */ #define PRBMAX 2147483647 #define PRB(x) ((x)>0?((x)>POS?PRBMAX:(PRBMAX-toprobtab[128-(x)])):((x) Log2[2^i + 1] (to base 2^(1/4)). * Match measure M[p1,p2] = Log[p1*p2 + (1-p1)(1-p2)] becomes * M'[l1,l2] = LP1[l1+l2] - LP1[l1] - LP1[l2] * when probabilities "p" are expressed in log odds "l" = log2(p/(1-p)) \**********************************************************************/ #define LP1(x) ((int) ((((x)+64)>>7)?(((x)<0)?0:((x)<<8)):(lep1+64)[(x)]))