└── src ├── mapper ├── read-ini.c ├── mapper_messages.h ├── Makefile ├── settings.ini ├── mapview.h ├── gui.ui.h ├── write-map.c ├── line.c ├── ipc.c ├── global-map.c ├── main.cpp ├── mapview.cpp ├── match-map.c ├── run_file.c ├── run_online.c ├── Makefile.rules ├── map2d.c ├── map2d.h ├── gui.ui └── map.c ├── fast-slam ├── Makefile ├── graphics.h ├── line.c ├── fast-slam.h ├── read-ini.c ├── graphics.cpp ├── main.cpp └── utils.c ├── Makefile └── Makefile.rules /src/mapper/read-ini.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSLAM-org/openslam_gridslam/HEAD/src/mapper/read-ini.c -------------------------------------------------------------------------------- /src/fast-slam/Makefile: -------------------------------------------------------------------------------- 1 | include $(CARMEN_HOME)/src/Makefile.conf 2 | 3 | MODULE_NAME = "GRID_FASTSLAM" 4 | MODULE_COMMENT = "Grid based Fast SLAM by D. Haehnel" 5 | 6 | CFLAGS += -Wno-shadow 7 | IFLAGS += -I. 8 | LFLAGS += -llogtools -lreadlog -lglobal -lipc 9 | 10 | CXXFLAGS += -Wno-non-virtual-dtor 11 | 12 | QT3_DIR = /usr/lib/qt3 13 | IFLAGS += -I$(QT3_DIR)/include 14 | 15 | ifeq ($(PROCESSOR),x86_64) 16 | LFLAGS += -L$(QT3_DIR)/lib64 17 | else 18 | LFLAGS += -L$(QT3_DIR)/lib 19 | endif 20 | 21 | 22 | LFLAGS += -lqt-mt `/usr/bin/Magick-config --libs` 23 | 24 | LINK = g++ 25 | 26 | GUI_FILES = ./graphics-moc.cpp 27 | 28 | ADD_CLEAN += $(GUI_FILES) 29 | 30 | SOURCES = read-ini.c line.c utils.c graphics.cpp main.cpp 31 | 32 | PUBLIC_INCLUDES = 33 | PUBLIC_LIBRARIES = 34 | PUBLIC_BINARIES = 35 | 36 | MAN_PAGES = 37 | 38 | TARGETS = fast-slam 39 | 40 | # rules 41 | 42 | depend: $(GUI_FILES) 43 | 44 | fast-slam: $(GUI_FILES) read-ini.o line.o utils.o graphics.o \ 45 | graphics-moc.o main.o 46 | 47 | include ../Makefile.rules 48 | -------------------------------------------------------------------------------- /src/mapper/mapper_messages.h: -------------------------------------------------------------------------------- 1 | #ifndef CMRT_MAP2D_MESSAGES_H 2 | #define CMRT_MAP2D_MESSAGES_H 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #define REGISTRATION_MODE 0 9 | #define MAPPING_MODE 1 10 | 11 | typedef struct { 12 | int mode; 13 | double timestamp; 14 | char host[10]; 15 | } navigation_mapper_set_mode_message; 16 | 17 | #define NAVIGATION_MAPPER_SET_MODE_NAME "navigation_mapper_set_mode" 18 | 19 | #define NAVIGATION_MAPPER_SET_MODE_FMT "{ int, double, [char:10] }" 20 | 21 | 22 | typedef struct { 23 | double robot_x; 24 | double robot_y; 25 | double robot_theta; 26 | double corr_x; 27 | double corr_y; 28 | double corr_theta; 29 | double timestamp; 30 | char host[10]; 31 | } navigation_mapper_status_message; 32 | 33 | #define NAVIGATION_MAPPER_STATUS_NAME "navigation_mapper_status" 34 | 35 | #define NAVIGATION_MAPPER_STATUS_FMT "{ double, double, double, double, double, double, double, [char:10] }" 36 | 37 | 38 | #ifdef __cplusplus 39 | } 40 | #endif 41 | 42 | #endif 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /src/fast-slam/graphics.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #ifdef __cplusplus 10 | extern "C" { 11 | #endif 12 | 13 | #include "fast-slam.h" 14 | 15 | #ifdef __cplusplus 16 | } 17 | #endif 18 | 19 | class MapPainter : public QScrollView { 20 | Q_OBJECT 21 | 22 | public: 23 | MapPainter( QWidget* parent = 0, const char *name = 0 ); 24 | ~MapPainter( void ){}; 25 | 26 | void centerView( MAP2 map, logtools_rpos2_t pos ); 27 | void drawContents( QPainter *p, int cx, int cy, int cw, int ch ); 28 | void setSize( int size_x, int size_y ); 29 | void update( MAP2 map ); 30 | void refresh( MAP2 map ); 31 | void showscan( MAP2 map, logtools_lasersens2_data_t lsens, double maxrange ); 32 | void drawrobot( MAP2 map, logtools_rpos2_t pos, int color ); 33 | void drawparticles( MAP2 map, SAMPLE_SET, int color, int showpath ); 34 | void dumpscreen( void ); 35 | void doPaint(); 36 | 37 | private: 38 | QImage * image; 39 | QPainter * pt; 40 | QPixmap * pix; 41 | QPixmap * pm; 42 | 43 | }; 44 | -------------------------------------------------------------------------------- /src/mapper/Makefile: -------------------------------------------------------------------------------- 1 | include $(CARMEN_HOME)/src/Makefile.conf 2 | 3 | MODULE_NAME = "SCAN_MATCHER" 4 | MODULE_COMMENT = "Grid based Scan Matcher by D. Haehnel" 5 | 6 | CFLAGS += -Wno-shadow 7 | IFLAGS += -I. 8 | LFLAGS += -lglobal -lipc -llogtools -lreadlog 9 | 10 | CXXFLAGS += -Wno-non-virtual-dtor 11 | 12 | QT3_DIR = /usr/lib/qt3 13 | IFLAGS += -I$(QT3_DIR)/include 14 | 15 | ifeq ($(PROCESSOR),x86_64) 16 | LFLAGS += -L$(QT3_DIR)/lib64 17 | else 18 | LFLAGS += -L$(QT3_DIR)/lib 19 | endif 20 | 21 | 22 | LFLAGS += -lqt-mt 23 | 24 | GUI_FILES = ./gui.h ./gui-moc.cpp ./gui.cpp ./mapview-moc.cpp 25 | 26 | ADD_CLEAN += $(GUI_FILES) 27 | 28 | SOURCES = line.c map.c read-ini.c match-map.c global-map.c \ 29 | write-map.c map2d.c ipc.c run_file.c run_online.c \ 30 | main.cpp mapview.cpp 31 | 32 | PUBLIC_INCLUDES = 33 | PUBLIC_LIBRARIES = 34 | PUBLIC_BINARIES = 35 | 36 | MAN_PAGES = 37 | 38 | TARGETS = ml-mapper 39 | 40 | # rules 41 | 42 | depend: $(GUI_FILES) 43 | 44 | ml-mapper: $(GUI_FILES) line.o map.o read-ini.o match-map.o global-map.o \ 45 | write-map.o map2d.o ipc.o run_file.o run_online.o \ 46 | main.o mapview-moc.o mapview.o gui-moc.o gui.o 47 | 48 | include ../Makefile.rules 49 | -------------------------------------------------------------------------------- /src/mapper/settings.ini: -------------------------------------------------------------------------------- 1 | #******************************************** 2 | # MAP2D SETTINGS 3 | #******************************************** 4 | 5 | USE_GRAPHICS 1 6 | 7 | 8 | 9 | POS_DIFF_MIN_DIST 0.01 10 | 11 | POS_DIFF_MIN_ROTATION 0.001745277 12 | 13 | POS_DIFF_MAX_ROTATION 3.1415 14 | 15 | 16 | 17 | USE_CORRECTION 1 18 | 19 | LOCAL_MAP_MAX_RANGE 2500.0 20 | 21 | LOCAL_MAP_RESOLUTION 5.0 22 | 23 | LOCAL_MAP_KERNEL_LENGTH 9 24 | 25 | LOCAL_MAP_NUM_CONVOLVE 1 26 | 27 | LOCAL_MAP_UNKNOWN_VAL 0.01 28 | 29 | LOCAL_MAP_HISTORY_LENGTH 6000 30 | 31 | LOCAL_MAP_MAX_USED_HISTORY 150 32 | 33 | 34 | 35 | POS_CORRECTIONL_FORWARD_STEP 7.5 36 | 37 | POS_CORRECTIONL_SIDEWARD_STEP 7.5 38 | 39 | POS_CORRECTIONL_ROTATION_STEP 0.125 40 | 41 | POS_CORRECTIONL_NUM_DECREASE_LOOP 10 42 | 43 | 44 | 45 | USE_PEOPLE_PROB 0 46 | 47 | PEOPLE_PROB_KERNEL_LENGTH 3 48 | 49 | PEOPLE_PROB_NUM_CONVOLVE 0 50 | 51 | 52 | 53 | OUTPUT_SCAN_PROBABILITIES 0 54 | 55 | OUTPUT_SCAN_PROB_FILENAME probs.dat 56 | 57 | 58 | OUTPUT_CORRECTED_SCRIPT 0 59 | 60 | OUTPUT_CORR_SCRIPT_FILENAME map2d-corrected.script 61 | 62 | 63 | 64 | OUTPUT_DATA_MAP 0 65 | 66 | OUTPUT_DATA_MAP_FILENAME map-data.dump 67 | 68 | 69 | 70 | GLOBAL_MAP_MAX_RANGE 1000.0 71 | 72 | GLOBAL_MAP_SIZE_X 1000 73 | 74 | GLOBAL_MAP_SIZE_Y 1000 75 | 76 | GLOBAL_MAP_RESOLUTION 5.0 77 | 78 | 79 | -------------------------------------------------------------------------------- /src/Makefile: -------------------------------------------------------------------------------- 1 | SILENT = @ 2 | ECHO = @echo 3 | 4 | PACKAGES = mapper fast-slam 5 | 6 | RECURSE = $(SILENT) \ 7 | for i in $(PACKAGES) xxxx ; do \ 8 | if [ -d $$i ] ; then \ 9 | if ! $(MAKE) -C $$i $@ ; then \ 10 | exit -1; \ 11 | fi; \ 12 | fi \ 13 | done 14 | 15 | ifndef CARMEN_HOME 16 | $(warning ) 17 | $(warning The CARMEN_HOME is not set.) 18 | $(warning ) 19 | $(warning This package needs CARMEN. Please install CARMEN) 20 | $(warning and set the CARMEN_HOME environment variable to) 21 | $(warning the CARMEN directory.) 22 | $(warning ) 23 | $(warning for more information about CARMEN) 24 | $(warning see http://carmen.sourceforge.net/) 25 | $(warning ) 26 | $(error ) 27 | endif 28 | 29 | 30 | all: phase1 phase2 31 | 32 | phase1: 33 | $(ECHO) 34 | $(ECHO) " *****************" 35 | $(ECHO) " L I B R A R I E S" 36 | $(ECHO) " *****************" 37 | $(ECHO) 38 | $(RECURSE) 39 | 40 | phase2: 41 | $(ECHO) 42 | $(ECHO) " ***************" 43 | $(ECHO) " B I N A R I E S" 44 | $(ECHO) " ***************" 45 | $(ECHO) 46 | $(RECURSE) 47 | $(ECHO) "Done making binaries..." 48 | 49 | clean: 50 | $(ECHO) 51 | $(ECHO) " *********" 52 | $(ECHO) " C L E A N" 53 | $(ECHO) " *********" 54 | $(ECHO) 55 | $(RECURSE) 56 | 57 | distclean: 58 | $(ECHO) 59 | $(ECHO) " *****************" 60 | $(ECHO) " D I S T C L E A N" 61 | $(ECHO) " *****************" 62 | $(ECHO) 63 | $(RECURSE) 64 | -------------------------------------------------------------------------------- /src/mapper/mapview.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | ** $Id: mapview.h,v 1.2 2004/06/11 13:57:56 haehnel Exp $ 3 | ** 4 | ** Copyright (C) 1992-2000 Trolltech AS. All rights reserved. 5 | ** 6 | ** This file is part of an example program for Qt. This example 7 | ** program may be used, distributed and modified without limitation. 8 | ** 9 | *****************************************************************************/ 10 | 11 | #ifndef MAPVIEW_H 12 | #define MAPVIEW_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #ifdef __cplusplus 32 | extern "C" { 33 | #endif 34 | 35 | #include 36 | #include 37 | 38 | #ifdef __cplusplus 39 | } 40 | #endif 41 | 42 | class MapView : public QScrollView { 43 | Q_OBJECT 44 | 45 | public: 46 | MapView(QWidget* parent, const char * = 0 ); 47 | 48 | void drawContents( QPainter *p, int cx, int cy, int cw, int ch ); 49 | void paintRobot( logtools_rpos2_t pos ); 50 | void plotRobotPosition( logtools_rpos2_t pos ); 51 | void centerRobot( void ); 52 | void updateMap( void ); 53 | void clearMap( void ); 54 | void setGlobalSize( int size_x, int size_y ); 55 | void setLocalSize( int size_x, int size_y ); 56 | void showRays(); 57 | int maptype; 58 | 59 | private: 60 | QImage * gimage; 61 | QImage * grimage; 62 | QImage * limage; 63 | QImage * rimage; 64 | QPainter * pt; 65 | logtools_rpos2_t rpos; 66 | 67 | }; 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /src/mapper/gui.ui.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | ** ui.h extension file, included from the uic-generated form implementation. 3 | ** 4 | ** If you wish to add, delete or rename slots use Qt Designer which will 5 | ** update this file, preserving your code. Create an init() slot in place of 6 | ** a constructor, and a destroy() slot in place of a destructor. 7 | *****************************************************************************/ 8 | 9 | #include 10 | 11 | #ifdef __cplusplus 12 | extern "C" { 13 | #endif 14 | 15 | #include "map2d.h" 16 | 17 | #ifdef __cplusplus 18 | } 19 | #endif 20 | 21 | void MapGUI::UpdateMapSlot() 22 | { 23 | Map->updateMap(); 24 | } 25 | 26 | void MapGUI::CenterRobotSlot() 27 | { 28 | Map->centerRobot(); 29 | } 30 | 31 | void MapGUI::ClearMapSlot() 32 | { 33 | Map->clearMap(); 34 | } 35 | 36 | void MapGUI::MenuNewSlot() 37 | { 38 | Map->clearMap(); 39 | fprintf( stderr, "pressed new button\n" ); 40 | } 41 | 42 | void MapGUI::MenuOpenSlot() 43 | { 44 | fprintf( stderr, "pressed open button\n" ); 45 | } 46 | 47 | void MapGUI::MenuPrintSlot() 48 | { 49 | fprintf( stderr, "pressed print button\n" ); 50 | } 51 | 52 | void MapGUI::MenuExitSlot() 53 | { 54 | exit(0); 55 | } 56 | 57 | void MapGUI::GlobalMapTypeSlot() 58 | { 59 | Map->maptype = GLOBAL_MAP; 60 | Map->updateMap(); 61 | Map->centerRobot(); 62 | } 63 | 64 | 65 | void MapGUI::LocalMapTypeSlot() 66 | { 67 | Map->maptype = LOCAL_MAP; 68 | Map->updateMap(); 69 | Map->centerRobot(); 70 | } 71 | 72 | 73 | void MapGUI::ShowBeamsTypeSlot() 74 | { 75 | Map->maptype = SHOW_RAYS; 76 | Map->updateMap(); 77 | Map->centerRobot(); 78 | } 79 | 80 | void MapGUI::ViewPathSlot() 81 | { 82 | fprintf( stderr, "x1\n" ); 83 | if (ViewPathAction->isOn()) { 84 | settings.view_path = TRUE; 85 | } else { 86 | settings.view_path = FALSE; 87 | } 88 | 89 | } 90 | 91 | void MapGUI::ChangeMapSlot() 92 | { 93 | change_map = ChangeMapAction->isOn(); 94 | } 95 | 96 | void MapGUI::ClearMapSlot( bool ) 97 | { 98 | 99 | } 100 | -------------------------------------------------------------------------------- /src/mapper/write-map.c: -------------------------------------------------------------------------------- 1 | #include "map2d.h" 2 | 3 | #define MINIMUM_MAP_SIZE 300 4 | #define MAP_SIZE_STEP 50 5 | 6 | void 7 | compute_map_probs( MAP2 * map ) 8 | { 9 | int x, y; 10 | for (x=0;xmapsize.x;x++) { 11 | for (y=0;ymapsize.y;y++) { 12 | if (map->mapsum[x][y]>0) { 13 | map->mapprob[x][y] = 14 | ( map->maphit[x][y] / (double) map->mapsum[x][y] ); 15 | } else { 16 | map->mapprob[x][y] = settings.global_map_std_val; 17 | } 18 | } 19 | } 20 | } 21 | 22 | int 23 | create_output_files( void ) 24 | { 25 | if (settings.log_output && !settings.use_correction) { 26 | settings.log_output = FALSE; 27 | } 28 | 29 | if ((settings.devNULL = 30 | fopen( "/dev/null", "w")) == 0){ 31 | fprintf(stderr, "ERROR: can't open /dev/null\n" ); 32 | exit(-1); 33 | } 34 | 35 | if (settings.log_output) { 36 | if (( settings.logF = 37 | fopen( settings.log_filename, "w")) == 0){ 38 | fprintf(stderr, "ERROR: can't write carmen log file %s\n", 39 | settings.log_filename ); 40 | exit(-1); 41 | } 42 | } 43 | return(0); 44 | } 45 | 46 | void 47 | write_log_entry( FILE * fp, logtools_lasersens2_data_t * lsens ) 48 | { 49 | static int firsttime = TRUE; 50 | static double logtime = 0.0; 51 | double t = lsens->laser.time.tv_sec + (lsens->laser.time.tv_usec/1000000.0 ); 52 | int i; 53 | if (firsttime) { 54 | logtime = t; 55 | firsttime = FALSE; 56 | } 57 | fprintf( fp, "ODOM %f %f %f %f %f %f %f %s %f\n", 58 | lsens->estpos.x/100.0, 59 | lsens->estpos.y/100.0, 60 | lsens->estpos.o, 61 | 0.0, 0.0, 0.0, 62 | t, "mapper", t-logtime ); 63 | fprintf( fp, "RAWLASER%d %d %f %f %f %f %f %d %d", lsens->id+1, 0, 64 | -lsens->laser.fov/2.0, lsens->laser.fov, 65 | lsens->laser.fov/(double)lsens->laser.numvalues, 66 | 80.95, 0.05, 0, lsens->laser.numvalues ); 67 | for (i=0; ilaser.numvalues; i++) { 68 | fprintf( fp, " %f", lsens->laser.val[i]/100.0 ); 69 | } 70 | fprintf( fp, " %d %f %s %f\n", 71 | 0, t, "mapper", t-logtime ); 72 | 73 | } 74 | 75 | void 76 | close_output_files( void ) 77 | { 78 | if (settings.log_output) { 79 | fclose(settings.logF); 80 | } 81 | } 82 | 83 | -------------------------------------------------------------------------------- /src/fast-slam/line.c: -------------------------------------------------------------------------------- 1 | #include "fast-slam.h" 2 | 3 | void 4 | grid_line_core( logtools_ivector2_t start, logtools_ivector2_t end, logtools_grid_line_t *line ) 5 | { 6 | int dx, dy, incr1, incr2, d, x, y, xend, yend, xdirflag, ydirflag; 7 | 8 | int cnt = 0; 9 | 10 | dx = abs(end.x-start.x); dy = abs(end.y-start.y); 11 | 12 | if (dy <= dx) { 13 | d = 2*dy - dx; incr1 = 2 * dy; incr2 = 2 * (dy - dx); 14 | if (start.x > end.x) { 15 | x = end.x; y = end.y; 16 | ydirflag = (-1); 17 | xend = start.x; 18 | } else { 19 | x = start.x; y = start.y; 20 | ydirflag = 1; 21 | xend = end.x; 22 | } 23 | line->grid[cnt].x=x; 24 | line->grid[cnt].y=y; 25 | cnt++; 26 | if (((end.y - start.y) * ydirflag) > 0) { 27 | while (x < xend) { 28 | x++; 29 | if (d <0) { 30 | d+=incr1; 31 | } else { 32 | y++; d+=incr2; 33 | } 34 | line->grid[cnt].x=x; 35 | line->grid[cnt].y=y; 36 | cnt++; 37 | } 38 | } else { 39 | while (x < xend) { 40 | x++; 41 | if (d <0) { 42 | d+=incr1; 43 | } else { 44 | y--; d+=incr2; 45 | } 46 | line->grid[cnt].x=x; 47 | line->grid[cnt].y=y; 48 | cnt++; 49 | } 50 | } 51 | } else { 52 | d = 2*dx - dy; 53 | incr1 = 2*dx; incr2 = 2 * (dx - dy); 54 | if (start.y > end.y) { 55 | y = end.y; x = end.x; 56 | yend = start.y; 57 | xdirflag = (-1); 58 | } else { 59 | y = start.y; x = start.x; 60 | yend = end.y; 61 | xdirflag = 1; 62 | } 63 | line->grid[cnt].x=x; 64 | line->grid[cnt].y=y; 65 | cnt++; 66 | if (((end.x - start.x) * xdirflag) > 0) { 67 | while (y < yend) { 68 | y++; 69 | if (d <0) { 70 | d+=incr1; 71 | } else { 72 | x++; d+=incr2; 73 | } 74 | line->grid[cnt].x=x; 75 | line->grid[cnt].y=y; 76 | cnt++; 77 | } 78 | } else { 79 | while (y < yend) { 80 | y++; 81 | if (d <0) { 82 | d+=incr1; 83 | } else { 84 | x--; d+=incr2; 85 | } 86 | line->grid[cnt].x=x; 87 | line->grid[cnt].y=y; 88 | cnt++; 89 | } 90 | } 91 | } 92 | line->numgrids = cnt; 93 | } 94 | 95 | void 96 | grid_line( logtools_ivector2_t start, logtools_ivector2_t end, logtools_grid_line_t *line ) { 97 | int i,j; 98 | int half; 99 | logtools_ivector2_t v; 100 | grid_line_core( start, end, line ); 101 | if ( start.x!=line->grid[0].x || 102 | start.y!=line->grid[0].y ) { 103 | half = line->numgrids/2; 104 | for (i=0,j=line->numgrids - 1;igrid[i]; 106 | line->grid[i] = line->grid[j]; 107 | line->grid[j] = v; 108 | } 109 | } 110 | } 111 | 112 | -------------------------------------------------------------------------------- /src/mapper/line.c: -------------------------------------------------------------------------------- 1 | #include "map2d.h" 2 | 3 | void 4 | grid_line_core( logtools_ivector2_t start, logtools_ivector2_t end, 5 | logtools_grid_line_t *line ) 6 | { 7 | int dx, dy, incr1, incr2, d, x, y, xend, yend, xdirflag, ydirflag; 8 | 9 | int cnt = 0; 10 | 11 | dx = abs(end.x-start.x); dy = abs(end.y-start.y); 12 | 13 | if (dy <= dx) { 14 | d = 2*dy - dx; incr1 = 2 * dy; incr2 = 2 * (dy - dx); 15 | if (start.x > end.x) { 16 | x = end.x; y = end.y; 17 | ydirflag = (-1); 18 | xend = start.x; 19 | } else { 20 | x = start.x; y = start.y; 21 | ydirflag = 1; 22 | xend = end.x; 23 | } 24 | line->grid[cnt].x=x; 25 | line->grid[cnt].y=y; 26 | cnt++; 27 | if (((end.y - start.y) * ydirflag) > 0) { 28 | while (x < xend) { 29 | x++; 30 | if (d <0) { 31 | d+=incr1; 32 | } else { 33 | y++; d+=incr2; 34 | } 35 | line->grid[cnt].x=x; 36 | line->grid[cnt].y=y; 37 | cnt++; 38 | } 39 | } else { 40 | while (x < xend) { 41 | x++; 42 | if (d <0) { 43 | d+=incr1; 44 | } else { 45 | y--; d+=incr2; 46 | } 47 | line->grid[cnt].x=x; 48 | line->grid[cnt].y=y; 49 | cnt++; 50 | } 51 | } 52 | } else { 53 | d = 2*dx - dy; 54 | incr1 = 2*dx; incr2 = 2 * (dx - dy); 55 | if (start.y > end.y) { 56 | y = end.y; x = end.x; 57 | yend = start.y; 58 | xdirflag = (-1); 59 | } else { 60 | y = start.y; x = start.x; 61 | yend = end.y; 62 | xdirflag = 1; 63 | } 64 | line->grid[cnt].x=x; 65 | line->grid[cnt].y=y; 66 | cnt++; 67 | if (((end.x - start.x) * xdirflag) > 0) { 68 | while (y < yend) { 69 | y++; 70 | if (d <0) { 71 | d+=incr1; 72 | } else { 73 | x++; d+=incr2; 74 | } 75 | line->grid[cnt].x=x; 76 | line->grid[cnt].y=y; 77 | cnt++; 78 | } 79 | } else { 80 | while (y < yend) { 81 | y++; 82 | if (d <0) { 83 | d+=incr1; 84 | } else { 85 | x--; d+=incr2; 86 | } 87 | line->grid[cnt].x=x; 88 | line->grid[cnt].y=y; 89 | cnt++; 90 | } 91 | } 92 | } 93 | line->numgrids = cnt; 94 | } 95 | 96 | void 97 | grid_line( logtools_ivector2_t start, logtools_ivector2_t end, 98 | logtools_grid_line_t *line ) { 99 | int i,j; 100 | int half; 101 | logtools_ivector2_t v; 102 | grid_line_core( start, end, line ); 103 | if ( start.x!=line->grid[0].x || 104 | start.y!=line->grid[0].y ) { 105 | half = line->numgrids/2; 106 | for (i=0,j=line->numgrids - 1;igrid[i]; 108 | line->grid[i] = line->grid[j]; 109 | line->grid[j] = v; 110 | } 111 | } 112 | } 113 | -------------------------------------------------------------------------------- /src/mapper/ipc.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "map2d.h" 6 | 7 | extern logtools_rpos2_t current_pos; 8 | extern int change_map; 9 | 10 | void convert_time( double tval, struct timeval *time ); 11 | 12 | static void 13 | ipc_odometry_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, 14 | void *clientData __attribute__ ((unused))) 15 | { 16 | static carmen_base_odometry_message data; 17 | IPC_RETURN_TYPE err = IPC_OK; 18 | FORMATTER_PTR formatter; 19 | 20 | formatter = IPC_msgInstanceFormatter(msgRef); 21 | err = IPC_unmarshallData(formatter, callData, &data, 22 | sizeof(carmen_base_odometry_message)); 23 | IPC_freeByteArray(callData); 24 | 25 | carmen_test_ipc_return(err, "Could not unmarshall data", 26 | IPC_msgInstanceName(msgRef)); 27 | current_pos.x = data.x * 100.0; 28 | current_pos.y = data.y * 100.0; 29 | current_pos.o = data.theta; 30 | 31 | online_odo_update = TRUE; 32 | } 33 | 34 | double 35 | apex_angle( int num ) 36 | { 37 | switch( num ) { 38 | case(180): 39 | return(180.0); 40 | break; 41 | case(181): 42 | return(180.0); 43 | break; 44 | case(360): 45 | return(180.0); 46 | break; 47 | case(361): 48 | return(180.0); 49 | break; 50 | case(401): 51 | return(100.0); 52 | break; 53 | default: 54 | return(180.0); 55 | break; 56 | } 57 | } 58 | 59 | static void 60 | ipc_carmen_front_laser_handler( MSG_INSTANCE msgRef, BYTE_ARRAY callData, 61 | void *clientData __attribute__ ((unused))) 62 | { 63 | IPC_RETURN_TYPE err = IPC_OK; 64 | FORMATTER_PTR formatter; 65 | static int allocated = FALSE; 66 | static carmen_laser_laser_message data; 67 | int i, r; 68 | double anglediff, rangestart, apxangle; 69 | 70 | if (!allocated) { 71 | data.range= (float *) malloc( MAX_NUM_LASER_VALUES * sizeof(float)); 72 | allocated = TRUE; 73 | } 74 | 75 | formatter = IPC_msgInstanceFormatter(msgRef); 76 | err = IPC_unmarshallData(formatter, callData, &data, 77 | sizeof(carmen_laser_laser_message)); 78 | IPC_freeByteArray(callData); 79 | carmen_test_ipc_return(err, "Could not unmarshall", 80 | IPC_msgInstanceName(msgRef)); 81 | 82 | 83 | apxangle = deg2rad(apex_angle( data.num_readings )); 84 | anglediff = (apxangle / (double) (data.num_readings-1)); 85 | rangestart = -(apxangle / 2.0); 86 | r = online_scan_ctr%settings.local_map_num_history; 87 | online_data.lsens[r].laser.numvalues = data.num_readings; 88 | for (i = 0; i < data.num_readings; i++){ 89 | online_data.lsens[r].laser.val[i] = data.range[i]*100.0; 90 | online_data.lsens[r].laser.angle[i] = rangestart+(i*anglediff); 91 | } 92 | convert_time( data.timestamp, &online_data.lsens[r].laser.time ); 93 | online_data.lsens[r].id = settings.laser_number; 94 | online_data.lsens[r].laser.fov = apxangle; 95 | online_data.lsens[r].estpos = current_pos; 96 | online_laserupdate = TRUE; 97 | 98 | } 99 | 100 | 101 | void 102 | ipc_initialize_messages( void ) 103 | { 104 | IPC_RETURN_TYPE err = IPC_OK; 105 | 106 | err = IPC_subscribe(CARMEN_BASE_ODOMETRY_NAME, ipc_odometry_handler, NULL); 107 | IPC_setMsgQueueLength(CARMEN_BASE_ODOMETRY_NAME, 1); 108 | carmen_test_ipc(err, "Could not subscribe", CARMEN_BASE_ODOMETRY_NAME); 109 | 110 | err = IPC_subscribe(CARMEN_LASER_FRONTLASER_NAME, 111 | ipc_carmen_front_laser_handler, NULL); 112 | IPC_setMsgQueueLength(CARMEN_LASER_FRONTLASER_NAME, 1); 113 | carmen_test_ipc(err, "Could not subscribe", CARMEN_LASER_FRONTLASER_NAME); 114 | 115 | } 116 | 117 | void 118 | ipc_update( void ) 119 | { 120 | IPC_listen(0); 121 | } 122 | 123 | void 124 | ipc_init( int argc, char *argv[] ) 125 | { 126 | carmen_ipc_initialize( argc, argv ); 127 | ipc_initialize_messages(); 128 | } 129 | 130 | void 131 | ipc_stop( void ) 132 | { 133 | fprintf( stderr, "INFO: close connection to CENTRAL\n" ); 134 | IPC_disconnect(); 135 | } 136 | 137 | -------------------------------------------------------------------------------- /src/fast-slam/fast-slam.h: -------------------------------------------------------------------------------- 1 | #ifndef GRID_FAST_SLAM_H 2 | #define GRID_FAST_SLAM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #define MAX_NAME_LENGTH 256 21 | 22 | typedef struct { 23 | 24 | int detect_size; 25 | 26 | double detect_size_border; 27 | double size_x; 28 | double size_y; 29 | double start_x; 30 | double start_y; 31 | 32 | double resolution; 33 | 34 | int dump_screen; 35 | char dump_filename[MAX_NAME_LENGTH]; 36 | 37 | char result_filename[MAX_NAME_LENGTH]; 38 | 39 | int num_samples; 40 | 41 | double min_step_distance; 42 | 43 | double max_range_length; 44 | double max_usable_length; 45 | 46 | double rotation_noise; 47 | double sideward_noise; 48 | double forward_noise; 49 | 50 | double min_likelihood; 51 | double max_likelihood; 52 | double unknown_likelihood; 53 | 54 | int show_graphics; 55 | int show_local_map; 56 | 57 | int kernel_size; 58 | 59 | int laser_id; 60 | } FASTSLAM_SETTINGS; 61 | 62 | typedef struct { 63 | logtools_rpos2_t offset; 64 | double resolution; 65 | float ** maphit; 66 | short ** mapsum; 67 | float ** mapprob; 68 | float ** calc; 69 | logtools_ivector2_t mapsize; 70 | logtools_vector2_t center; 71 | } MAP2; 72 | 73 | #define logtools_grid_object_t logtools_grid_line_t 74 | 75 | typedef struct { 76 | logtools_bounding_box_t bbox; 77 | logtools_rpos2_t pos; 78 | } HISTORY; 79 | 80 | typedef struct { 81 | logtools_rpos2_t pos; 82 | double val; 83 | double logsum; 84 | int histlen; 85 | HISTORY * hist; 86 | int outside; 87 | } PARTICLE; 88 | 89 | typedef struct { 90 | int numparticles; 91 | PARTICLE * particle; 92 | } SAMPLE_SET; 93 | 94 | typedef struct { 95 | double r; 96 | double g; 97 | double b; 98 | } RGB; 99 | 100 | extern FASTSLAM_SETTINGS settings; 101 | 102 | 103 | void 104 | map_initialize( MAP2 *map, int sx, int sy, int center_x, int center_y, 105 | double resolution, logtools_rpos2_t start ); 106 | 107 | void 108 | map_clear( MAP2 *map ); 109 | 110 | void 111 | update_map( MAP2 * map, int numvalues, float * val, float * angle, 112 | logtools_rpos2_t estpos, double max_range, double max_usable ); 113 | 114 | void 115 | compute_probs_of_map( MAP2 * map ); 116 | 117 | void 118 | compute_voronoi_of_map( MAP2 * map ); 119 | 120 | void 121 | compute_calc_of_map( MAP2 * map ); 122 | 123 | void 124 | compute_intersections_of_map( MAP2 * map ); 125 | 126 | void 127 | grid_circle( logtools_ivector2_t center, int radius, 128 | logtools_grid_object_t * circle ); 129 | 130 | void 131 | grid_line( logtools_ivector2_t start, logtools_ivector2_t end, 132 | logtools_grid_line_t *line ); 133 | 134 | void 135 | show_scan( MAP2 * map, logtools_lasersens2_data_t lsens, double max_range ); 136 | 137 | double 138 | compute_scan_probability( MAP2 * map, logtools_rpos2_t pos, logtools_lasersens2_data_t lsens, 139 | double max_range, double max_usable ); 140 | 141 | double 142 | compute_beam_prob( MAP2 * map, logtools_rpos2_t pos, double length, 143 | double max_range, double * prob1, double * prob2 ); 144 | 145 | int 146 | map_pos_from_rpos( logtools_rpos2_t rpos, MAP2 *map, logtools_ivector2_t *v ); 147 | 148 | double 149 | get_map_val( logtools_ivector2_t pos, MAP2 map ); 150 | 151 | void 152 | simple_convolve_map( MAP2 *map, logtools_gauss_kernel_t kernel ); 153 | 154 | void 155 | simple_convolve_map2( MAP2 *map ); 156 | 157 | double 158 | prob_unknown_space( double length, int endpoint ); 159 | 160 | int 161 | map_pos_from_rpos( logtools_rpos2_t rpos, MAP2 *map, logtools_ivector2_t *v ); 162 | 163 | int 164 | map_pos_from_vec2( logtools_vector2_t pos, MAP2 *map, logtools_ivector2_t *v ); 165 | 166 | void 167 | resample( SAMPLE_SET sample1, SAMPLE_SET * sample2 ); 168 | 169 | void 170 | copy_particle( PARTICLE src, PARTICLE *dest ); 171 | 172 | int 173 | intersect_bboxes( logtools_bounding_box_t box1, logtools_bounding_box_t box2 ); 174 | 175 | logtools_bounding_box_t 176 | compute_laser_bounding_box( logtools_rpos2_t pos, 177 | logtools_lasersens2_data_t lsens, 178 | double laser_max_range ); 179 | 180 | int 181 | find_best_particle_logsum( SAMPLE_SET pset ); 182 | 183 | int 184 | find_best_particle_value( SAMPLE_SET pset ); 185 | 186 | 187 | void 188 | set_default( void ); 189 | 190 | void 191 | read_ini_file( char *filename ); 192 | 193 | void 194 | write_map( MAP2 map, char *filename ); 195 | 196 | #endif 197 | 198 | -------------------------------------------------------------------------------- /src/mapper/global-map.c: -------------------------------------------------------------------------------- 1 | #include "map2d.h" 2 | 3 | extern MAP2D_SETTINGS settings; 4 | extern MAP2 global_map; 5 | 6 | typedef struct { 7 | double minx; 8 | double maxx; 9 | double miny; 10 | double maxy; 11 | } MINMAX2_VALUES; 12 | 13 | 14 | int 15 | map_pos_from_rpos( logtools_rpos2_t rpos, MAP2 *map, logtools_ivector2_t *v ) 16 | { 17 | v->x = (int) (map->center.x + ((rpos.x-map->offset.x)/ 18 | (double)map->resolution)); 19 | v->y = (int) (map->center.y + ((rpos.y-map->offset.y)/ 20 | (double)map->resolution)); 21 | if (v->x<0) { 22 | return(FALSE); 23 | } else if (v->x>map->mapsize.x-1) { 24 | return(FALSE); 25 | } 26 | if (v->y<0) { 27 | return(FALSE); 28 | } else if (v->y>map->mapsize.y-1) { 29 | return(FALSE); 30 | } 31 | return(TRUE); 32 | } 33 | 34 | void 35 | rpos_pos_from_map_pos( logtools_ivector2_t v, MAP2 *map, logtools_rpos2_t *rpos ) 36 | { 37 | rpos->x = ((v.x-map->center.x)*(double) map->resolution) + map->offset.x; 38 | rpos->y = ((v.y-map->center.y)*(double) map->resolution) + map->offset.y; 39 | } 40 | 41 | void 42 | vec2_from_map_pos( logtools_ivector2_t v, MAP2 *map, logtools_vector2_t *pos ) 43 | { 44 | pos->x = ((v.x-map->center.x)*(double) map->resolution) + map->offset.x; 45 | pos->y = ((v.y-map->center.y)*(double) map->resolution) + map->offset.y; 46 | } 47 | 48 | int 49 | map_pos_from_vec2( logtools_vector2_t pos, MAP2 *map, logtools_ivector2_t *v ) 50 | { 51 | v->x = (int) (map->center.x + 52 | ((pos.x-map->offset.x)/(double)map->resolution)); 53 | v->y = (int) (map->center.y + ((pos.y-map->offset.y)/ 54 | (double)map->resolution)); 55 | if (v->x<0) { 56 | return(FALSE); 57 | } else if (v->x>map->mapsize.x-1) { 58 | return(FALSE); 59 | } 60 | if (v->y<0) { 61 | return(FALSE); 62 | } else if (v->y>map->mapsize.y-1) { 63 | return(FALSE); 64 | } 65 | return(TRUE); 66 | } 67 | 68 | void 69 | initialize_global_map( MAP2 *map, int size_x, int size_y, 70 | int start_x, int start_y, double resolution, 71 | logtools_rpos2_t start, int convolve ) 72 | { 73 | int x, y; 74 | 75 | map->resolution = resolution; 76 | 77 | map->mapsize.x = size_x; 78 | map->mapsize.y = size_y; 79 | 80 | map->offset = start; 81 | 82 | map->maphit = (float **) mdalloc( 2, sizeof(float), size_x, size_y ); 83 | map->mapsum = (short **) mdalloc( 2, sizeof(short), size_x, size_y ); 84 | map->mapprob = (float **) mdalloc( 2, sizeof(float), size_x, size_y ); 85 | 86 | if(convolve) 87 | map->calc = (float **) mdalloc( 2, sizeof(float), size_x, size_y ); 88 | 89 | map->center.x = start_x; 90 | map->center.y = start_y; 91 | 92 | fprintf( stderr, "* INFO: global map: size: %7d - %7d\n", 93 | map->mapsize.x, map->mapsize.y ); 94 | fprintf( stderr, "* INFO: global map: center: %7.1f - %7.1f\n", 95 | map->center.x, map->center.y ); 96 | for (x=0;xmapsum[x][y] = 0; 99 | map->maphit[x][y] = 0; 100 | } 101 | } 102 | } 103 | 104 | void 105 | clear_global_map( MAP2 *map ) 106 | { 107 | int x, y; 108 | for (x=0;xmapsize.x;x++) { 109 | for (y=0;ymapsize.y;y++) { 110 | map->mapsum[x][y] = 0; 111 | map->maphit[x][y] = 0; 112 | map->mapprob[x][y] = 0.0; 113 | } 114 | } 115 | } 116 | 117 | void 118 | update_global_map( logtools_lasersens2_data_t lsens, MAP2 * map ) 119 | { 120 | static int first_time = TRUE; 121 | static logtools_grid_line_t line; 122 | int i, j, x, y; 123 | int max_num_linepoints; 124 | logtools_ivector2_t start, end; 125 | logtools_vector2_t abspt; 126 | logtools_rmove2_t nomove = {0.0, 0.0, 0.0}; 127 | 128 | if (first_time) { 129 | max_num_linepoints = 130 | (int) (4 * ( settings.global_map_max_range / 131 | settings.global_map_resolution )); 132 | line.grid = (logtools_ivector2_t *) malloc( max_num_linepoints * 133 | sizeof(logtools_ivector2_t) ); 134 | first_time = FALSE; 135 | } 136 | 137 | for (j=0;j settings.global_map_max_range ) { 140 | abspt = logtools_compute_laser_points( lsens.estpos, 141 | settings.global_map_max_range, 142 | nomove, 143 | lsens.laser.angle[j] ); 144 | map_pos_from_vec2( abspt, map, &end ); 145 | } else { 146 | abspt = logtools_compute_laser_points( lsens.estpos, 147 | lsens.laser.val[j], 148 | nomove, 149 | lsens.laser.angle[j] ); 150 | map_pos_from_vec2( abspt, map, &end ); 151 | if ( end.x>=0 && end.xmapsize.x && 152 | end.y>=0 && end.ymapsize.y ) { 153 | map->maphit[end.x][end.y]+=1; 154 | if (!settings.global_map_ray_model) { 155 | map->mapsum[end.x][end.y]++; 156 | } 157 | } 158 | } 159 | if (settings.global_map_ray_model) { 160 | map_pos_from_rpos( lsens.estpos, map, &start ); 161 | grid_line( start, end, &line ); 162 | for (i=0;i=0 && xmapsize.x && 166 | y>=0 && ymapsize.y ) { 167 | if (lsens.laser.val[j]<=settings.global_map_max_range ) { 168 | if (i>=line.numgrids-2) { 169 | map->maphit[x][y]++; 170 | } 171 | map->mapsum[x][y]++; 172 | } else { 173 | if (imapsum[x][y]++; 175 | } 176 | } 177 | } 178 | } 179 | } 180 | } 181 | } 182 | } 183 | 184 | -------------------------------------------------------------------------------- /src/mapper/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "gui.h" 5 | #include "mapview.h" 6 | 7 | QApplication * papp; 8 | MapGUI * wptr; 9 | 10 | #ifdef __cplusplus 11 | extern "C" { 12 | #endif 13 | 14 | #include 15 | #include 16 | #include "map2d.h" 17 | 18 | MAP2 * global_map; 19 | MAP2 * local_map; 20 | 21 | extern logtools_lasersens2_data_t * current_scan; 22 | extern logtools_rmove2_t * current_movement; 23 | 24 | void 25 | initialize_maps( MapGUI * window, MAP2 * local_map, MAP2 * global_map ) 26 | { 27 | int size_x, size_y; 28 | logtools_rpos2_t npos = { 0.0, 0.0, 0.0 }; 29 | 30 | fprintf( stderr, "***************************************\n" ); 31 | fprintf( stderr, "* MAPS\n" ); 32 | fprintf( stderr, "***************************************\n" ); 33 | 34 | if (settings.use_correction) { 35 | size_x = (int) ceil((ADDITIONAL_X_SIZE+settings.local_map_max_range)/ 36 | settings.local_map_resolution); 37 | size_y = (int) ceil((ADDITIONAL_Y_SIZE+settings.local_map_max_range)/ 38 | settings.local_map_resolution); 39 | fprintf( stderr, "* INFO: create -local- map: %d x %d\n", 2*size_x, size_y ); 40 | initialize_map( local_map, 2*size_x, size_y, 60, size_y/2, 41 | settings.local_map_resolution, npos ); 42 | fprintf( stderr, "***************************************\n" ); 43 | } 44 | 45 | if (settings.use_global_map) { 46 | fprintf( stderr, "* INFO: create -global- map: %d x %d\n", 47 | settings.global_map_size_x, settings.global_map_size_y ); 48 | initialize_global_map( global_map, 49 | settings.global_map_size_x, 50 | settings.global_map_size_y, 51 | settings.global_map_start_x, 52 | settings.global_map_start_y, 53 | settings.global_map_resolution, 54 | settings.global_map_start_pos, 0 ); 55 | fprintf( stderr, "***************************************\n" ); 56 | } 57 | 58 | if (settings.use_graphics) { 59 | fprintf( stderr, "* INFO: set graphics -global- map size\n" ); 60 | fprintf( stderr, "***************************************\n" ); 61 | window->Map->setGlobalSize( global_map->mapsize.x, global_map->mapsize.y ); 62 | if (settings.use_correction) { 63 | fprintf( stderr, "* INFO: set graphics -local- map size\n" ); 64 | fprintf( stderr, "***************************************\n" ); 65 | window->Map->setLocalSize( local_map->mapsize.x, local_map->mapsize.y ); 66 | } 67 | fprintf( stderr, "* INFO: show maps\n" ); 68 | fprintf( stderr, "***************************************\n" ); 69 | 70 | if (settings.use_correction) { 71 | window->LocalMapType->setEnabled( TRUE ); 72 | } else { 73 | window->LocalMapType->setEnabled( FALSE ); 74 | window->LocalMapType->hide(); 75 | } 76 | 77 | window->Map->maptype = GLOBAL_MAP; 78 | 79 | window->show(); 80 | window->Map->updateMap(); 81 | 82 | } 83 | } 84 | 85 | int 86 | window_maptype( void ) 87 | { 88 | return(wptr->Map->maptype); 89 | } 90 | 91 | void 92 | window_show_rays( void ) 93 | { 94 | wptr->Map->showRays(); 95 | } 96 | 97 | void 98 | app_process_events( void ) 99 | { 100 | papp->processEvents(); 101 | } 102 | 103 | void 104 | paint_robot( logtools_rpos2_t pos ) 105 | { 106 | wptr->Map->paintRobot( pos ); 107 | } 108 | 109 | void 110 | plot_robot( logtools_rpos2_t pos ) 111 | { 112 | wptr->Map->plotRobotPosition( pos ); 113 | } 114 | 115 | void 116 | update_map( void ) 117 | { 118 | wptr->Map->updateMap(); 119 | } 120 | 121 | void 122 | update_change_map( void ) 123 | { 124 | wptr->ChangeMapAction->setOn(change_map); 125 | } 126 | 127 | void 128 | center_robot( void ) 129 | { 130 | wptr->Map->centerRobot(); 131 | } 132 | 133 | #ifdef __cplusplus 134 | } 135 | #endif 136 | 137 | void 138 | print_usage( void ) 139 | { 140 | fprintf( stderr, 141 | "usage: map2d [-ini ] [-file ]\n" ); 142 | } 143 | 144 | int 145 | main( int argc, char *argv[] ) 146 | { 147 | QApplication app( argc, argv ); 148 | MapGUI window; 149 | 150 | int i; 151 | 152 | MAP2 lmap; 153 | MAP2 gmap; 154 | 155 | 156 | int ini_file = FALSE; 157 | char ini_filename[MAX_NAME_LENGTH]; 158 | 159 | set_default(); 160 | 161 | local_map = &lmap; 162 | global_map = &gmap; 163 | 164 | wptr = &window; 165 | papp = &app; 166 | 167 | settings.mode = ONLINE; 168 | 169 | if (argc>5) { 170 | print_usage(); 171 | exit(1); 172 | } 173 | 174 | for (i=1; ii+1)) { 176 | strncpy( ini_filename, argv[++i], MAX_NAME_LENGTH ); 177 | ini_file = TRUE; 178 | } else if (!strcmp(argv[i],"-file") && (argc>i+1)) { 179 | strncpy( settings.data_filename, argv[++i], MAX_NAME_LENGTH ); 180 | settings.mode = READ_FILE; 181 | } else { 182 | print_usage(); 183 | exit(1); 184 | } 185 | } 186 | 187 | if (ini_file) { 188 | read_ini_file( ini_filename ); 189 | } 190 | 191 | check_settings(); 192 | 193 | fprintf( stderr, "INFO: global map max range: %.1f\n", 194 | settings.global_map_max_range ); 195 | fprintf( stderr, "INFO: global map size: %d %d\n", 196 | settings.global_map_size_x, 197 | settings.global_map_size_y ); 198 | fprintf( stderr, "INFO: global map start: %d %d\n", 199 | settings.global_map_start_x, 200 | settings.global_map_start_y ); 201 | fprintf( stderr, "INFO: global map resolution: %.1f\n", 202 | settings.global_map_resolution ); 203 | 204 | if (create_output_files() != 0) { 205 | exit(1); 206 | } 207 | 208 | initialize_maps( &window, &lmap, &gmap ); 209 | 210 | switch(settings.mode) { 211 | case READ_FILE: 212 | run_data_file( settings, &lmap, &gmap ); 213 | break; 214 | case ONLINE: 215 | run_online( settings, &lmap, &gmap, argc, argv ); 216 | break; 217 | } 218 | 219 | close_output_files(); 220 | 221 | return(0); 222 | 223 | } 224 | -------------------------------------------------------------------------------- /src/fast-slam/read-ini.c: -------------------------------------------------------------------------------- 1 | #include "fast-slam.h" 2 | 3 | FASTSLAM_SETTINGS settings; 4 | 5 | void 6 | set_default( void ) 7 | { 8 | 9 | settings.num_samples = 150; 10 | 11 | // settings.detect_size = FALSE; 12 | settings.detect_size = TRUE; 13 | 14 | settings.detect_size_border = 500.0; 15 | 16 | 17 | settings.size_x = 8000.0; 18 | settings.size_y = 8000.0; 19 | settings.start_x = 4000.0; 20 | settings.start_y = 4000.0; 21 | 22 | settings.resolution = 7.5; 23 | 24 | settings.dump_screen = FALSE; 25 | strncpy( settings.dump_filename, "dump_", MAX_NAME_LENGTH ); 26 | strncpy( settings.result_filename, "fastslam-result.rec", MAX_NAME_LENGTH ); 27 | 28 | settings.max_range_length = 450.0; 29 | settings.max_usable_length = 8000.0; 30 | 31 | settings.show_graphics = TRUE; 32 | 33 | settings.rotation_noise = 0.02; 34 | settings.sideward_noise = deg2rad(0.001); 35 | settings.forward_noise = 0.05; 36 | 37 | settings.min_likelihood = 0.8; 38 | settings.max_likelihood = 1.0; 39 | settings.unknown_likelihood = 0.83; 40 | 41 | settings.min_step_distance = 500.0; 42 | 43 | settings.laser_id = 0; 44 | } 45 | 46 | #define MAX_COMMAND_LENGTH 1024 47 | 48 | void 49 | read_ini_file( char *filename ) 50 | { 51 | int FEnd; 52 | FILE *iop; 53 | char command[MAX_COMMAND_LENGTH]; 54 | char inp[MAX_COMMAND_LENGTH]; 55 | 56 | if ((iop = fopen( filename, "r")) == 0){ 57 | fprintf(stderr, "ERROR: could not open ini file %s\n", filename ); 58 | exit(0); 59 | } 60 | fprintf(stderr, "* INFO: read ini file %s\n", filename ); 61 | FEnd=0; 62 | do{ 63 | if (fscanf(iop, "%s", command) == EOF) 64 | FEnd=1; 65 | else{ 66 | if (!strcmp( command, "DETECT_SIZE") ){ 67 | if (fscanf(iop, "%d", &settings.detect_size) == EOF) 68 | FEnd=1; 69 | } else if (!strcmp( command, "DETECT_SIZE_BORDER") ){ 70 | if (fscanf(iop, "%s", inp) == EOF) { 71 | FEnd=1; 72 | } else { 73 | settings.detect_size_border = atof(inp); 74 | } 75 | } else if (!strcmp( command, "SIZE_X") ){ 76 | if (fscanf(iop, "%s", inp) == EOF) { 77 | FEnd=1; 78 | } else { 79 | settings.size_x = atof(inp); 80 | } 81 | } else if (!strcmp( command, "SIZE_Y") ){ 82 | if (fscanf(iop, "%s", inp) == EOF) { 83 | FEnd=1; 84 | } else { 85 | settings.size_y = atof(inp); 86 | } 87 | } else if (!strcmp( command, "START_X") ){ 88 | if (fscanf(iop, "%s", inp) == EOF) { 89 | FEnd=1; 90 | } else { 91 | settings.start_x = atof(inp); 92 | } 93 | } else if (!strcmp( command, "START_Y") ){ 94 | if (fscanf(iop, "%s", inp) == EOF) { 95 | FEnd=1; 96 | } else { 97 | settings.start_y = atof(inp); 98 | } 99 | } else if (!strcmp( command, "MAX_RANGE_LENGTH") ){ 100 | if (fscanf(iop, "%s", inp) == EOF) { 101 | FEnd=1; 102 | } else { 103 | settings.max_range_length = atof(inp); 104 | } 105 | 106 | } else if (!strcmp( command, "MAX_USABLE_LENGTH") ){ 107 | if (fscanf(iop, "%s", inp) == EOF) { 108 | FEnd=1; 109 | } else { 110 | settings.max_usable_length = atof(inp); 111 | } 112 | } else if (!strcmp( command, "ROTATION_NOISE") ){ 113 | if (fscanf(iop, "%s", inp) == EOF) { 114 | FEnd=1; 115 | } else { 116 | settings.rotation_noise = atof(inp); 117 | } 118 | } else if (!strcmp( command, "SIDEWARD_NOISE") ){ 119 | if (fscanf(iop, "%s", inp) == EOF) { 120 | FEnd=1; 121 | } else { 122 | settings.sideward_noise = deg2rad(atof(inp)); 123 | } 124 | } else if (!strcmp( command, "FORWARD_NOISE") ){ 125 | if (fscanf(iop, "%s", inp) == EOF) { 126 | FEnd=1; 127 | } else { 128 | settings.forward_noise = atof(inp); 129 | } 130 | } else if (!strcmp( command, "MIN_LIKELIHOOD") ){ 131 | if (fscanf(iop, "%s", inp) == EOF) { 132 | FEnd=1; 133 | } else { 134 | settings.min_likelihood = atof(inp); 135 | } 136 | } else if (!strcmp( command, "MAX_LIKELIHOOD") ){ 137 | if (fscanf(iop, "%s", inp) == EOF) 138 | FEnd=1; 139 | else 140 | settings.max_likelihood = atof(inp); 141 | } else if (!strcmp( command, "UNKNOWN_LIKELIHOOD") ){ 142 | if (fscanf(iop, "%s", inp) == EOF) { 143 | FEnd=1; 144 | } else { 145 | settings.unknown_likelihood = atof(inp); 146 | } 147 | } else if (!strcmp( command, "NUM_SAMPLES") ){ 148 | if (fscanf(iop, "%s", inp) == EOF) { 149 | FEnd=1; 150 | } else { 151 | settings.num_samples = atoi(inp); 152 | } 153 | } else if (!strcmp( command, "MIN_STEP_DISTANCE") ){ 154 | if (fscanf(iop, "%s", inp) == EOF) { 155 | FEnd=1; 156 | } else { 157 | settings.min_step_distance = atof(inp); 158 | } 159 | } else if (!strcmp( command, "RESOLUTION") ){ 160 | if (fscanf(iop, "%s", inp) == EOF) { 161 | FEnd=1; 162 | } else { 163 | settings.resolution = atof(inp); 164 | } 165 | 166 | } else if (!strcmp( command, "DUMP_SCREEN") ){ 167 | if (fscanf(iop, "%s", inp) == EOF) { 168 | FEnd=1; 169 | } else { 170 | settings.dump_screen = atoi(inp); 171 | } 172 | 173 | } else if (!strcmp( command, "SHOW_GRAPHICS") ){ 174 | if (fscanf(iop, "%s", inp) == EOF) { 175 | FEnd=1; 176 | } else { 177 | settings.show_graphics = atoi(inp); 178 | } 179 | 180 | } else if (!strcmp( command, "SHOW_LOCAL_MAP") ){ 181 | if (fscanf(iop, "%s", inp) == EOF) { 182 | FEnd=1; 183 | } else { 184 | settings.show_local_map = atoi(inp); 185 | } 186 | 187 | } else if (!strcmp( command, "KERNEL_SIZE") ){ 188 | if (fscanf(iop, "%s", inp) == EOF) { 189 | FEnd=1; 190 | } else { 191 | settings.kernel_size = atoi(inp); 192 | } 193 | 194 | } else if (!strcmp( command, "DUMP_FILENAME_PREFIX") ){ 195 | if (fscanf(iop, "%s", inp) == EOF) { 196 | FEnd=1; 197 | } else { 198 | strncpy( settings.dump_filename, inp, MAX_NAME_LENGTH ); 199 | } 200 | 201 | } else { 202 | if (!(command[0]=='#')){ 203 | fprintf( stderr, "ERROR unknown keyword %s\n", command ); 204 | fclose(iop); 205 | exit(0); 206 | } else 207 | fgets(command,sizeof(command),iop); 208 | } 209 | } 210 | } while (!FEnd); 211 | fclose(iop); 212 | 213 | } 214 | 215 | 216 | 217 | 218 | 219 | -------------------------------------------------------------------------------- /src/mapper/mapview.cpp: -------------------------------------------------------------------------------- 1 | #include "mapview.h" 2 | 3 | #ifdef __cplusplus 4 | extern "C" { 5 | #endif 6 | 7 | #include "map2d.h" 8 | 9 | extern MAP2 * global_map; 10 | extern MAP2 * local_map; 11 | extern logtools_lasersens2_data_t * current_scan; 12 | extern logtools_rmove2_t * current_movement; 13 | 14 | #ifdef __cplusplus 15 | } 16 | #endif 17 | 18 | int scale = 1; 19 | 20 | double beam_factor = 5.0; 21 | 22 | 23 | MapView::MapView( QWidget* parent, const char * ) 24 | : QScrollView( parent ) 25 | { 26 | #if QT_VERSION >= 300 27 | setSizePolicy( QSizePolicy( (QSizePolicy::SizeType)7, 28 | (QSizePolicy::SizeType)7, 29 | 1, 1, 30 | sizePolicy().hasHeightForWidth() ) ); 31 | #else 32 | setSizePolicy( QSizePolicy( (QSizePolicy::SizeType)7, 33 | (QSizePolicy::SizeType)7, 34 | sizePolicy().hasHeightForWidth() ) ); 35 | #endif 36 | viewport()->setBackgroundMode( PaletteBase ); 37 | pt = new QPainter( viewport() ); 38 | } 39 | 40 | 41 | void 42 | MapView::setGlobalSize( int size_x, int size_y ) 43 | { 44 | if (settings.use_global_map) { 45 | gimage = new QImage( size_x, size_y, 8, 256 ); 46 | 47 | for (int i=0; i<256; i++) { 48 | gimage->setColor( i, qRgb( i, i, i ) ); 49 | } 50 | gimage->setColor( 0, qRgb( 200, 200, 255 ) ); 51 | gimage->setColor( 1, qRgb( 0, 255, 0 ) ); 52 | } 53 | } 54 | 55 | void 56 | MapView::setLocalSize( int size_x, int size_y ) 57 | { 58 | if (settings.use_correction) { 59 | limage = new QImage( size_x*scale, size_y*scale, 8, 256 ); 60 | 61 | for (int i=0; i<250; i++) { 62 | limage->setColor( i, qRgb( (int) (255-(i*2.55)), 63 | (int) (255-(i*2.55)), 64 | (int) (255-(i*2.55)) ) ); 65 | } 66 | limage->setColor( 255, qRgb( 255, 0, 0 ) ); 67 | limage->setColor( 254, qRgb( 0, 0, 255 ) ); 68 | } 69 | } 70 | 71 | QString 72 | number2str( int num ) 73 | { 74 | if (num<10) { 75 | return("000"+QString::number( num )); 76 | } else if (num<100) { 77 | return("00"+QString::number( num )); 78 | } else if (num<1000) { 79 | return("0"+QString::number( num )); 80 | } 81 | return(QString::number( num )); 82 | } 83 | 84 | char * 85 | dumpMapName( char * prefix ) 86 | { 87 | static int dumpCtr = 0; 88 | static char name[MAX_NAME_LENGTH]; 89 | 90 | QString str = "/dev/null"; 91 | QFileInfo fi; 92 | 93 | do { 94 | str = QString( QString( prefix ) + number2str( dumpCtr ) + ".png" ); 95 | fi = str; 96 | dumpCtr++; 97 | } while( fi.exists()); 98 | strncpy( name, str.ascii(), MAX_NAME_LENGTH ); 99 | 100 | return( name ); 101 | } 102 | 103 | void 104 | MapView::drawContents( QPainter *p, int cx, int cy, int cw, int ch ) 105 | { 106 | p->fillRect( cx, cy, cw, ch, colorGroup().brush( QColorGroup::Base ) ); 107 | switch(maptype) { 108 | case GLOBAL_MAP: 109 | if (settings.use_global_map) { 110 | p->drawImage( 0, 0, *gimage ); 111 | } 112 | break; 113 | case LOCAL_MAP: 114 | if (settings.use_correction) { 115 | p->drawImage( 0, 0, *limage ); 116 | } 117 | break; 118 | case SHOW_RAYS: 119 | break; 120 | } 121 | } 122 | 123 | void 124 | MapView::plotRobotPosition( logtools_rpos2_t pos ) 125 | { 126 | logtools_ivector2_t mpos; 127 | map_pos_from_rpos( pos, global_map, &mpos ); 128 | gimage->setPixel( global_map->mapsize.x-1-mpos.x, 129 | mpos.y, 1 ); 130 | fprintf( stderr, "(%d:%d)", mpos.x, mpos.y ); 131 | } 132 | 133 | void 134 | MapView::updateMap( ) 135 | { 136 | int x, y, i, j; 137 | 138 | switch (maptype) { 139 | case GLOBAL_MAP: 140 | if (settings.use_global_map) { 141 | compute_probs_of_global_map( global_map ); 142 | for (x=0;xmapsize.x;x++) { 143 | for (y=0;ymapsize.y;y++) { 144 | if (global_map->mapsum[x][y]>0) { 145 | gimage->setPixel( (int) (global_map->mapsize.x-1-x), (int) y, 146 | (int) (255-253*global_map->mapprob[x][y]) ); 147 | } else { 148 | global_map->mapprob[x][y] = settings.global_map_std_val; 149 | gimage->setPixel( (int) (global_map->mapsize.x-1-x), (int) y, 0 ); 150 | } 151 | } 152 | } 153 | resizeContents( gimage->size().width(), gimage->size().height() ); 154 | } 155 | break; 156 | case LOCAL_MAP: 157 | if (settings.use_correction) { 158 | for (x=0;xmapsize.x;x++) { 159 | for (y=0;ymapsize.y;y++) { 160 | for (i=0;isetPixel( (int) (x*scale+i), 163 | (int) ((local_map->mapsize.y-y-1)*scale+j), 164 | (int) (200.0 * local_map->mapprob[x][y]) ); 165 | } 166 | } 167 | } 168 | } 169 | resizeContents( limage->size().width(), 170 | limage->size().height() ); 171 | } 172 | break; 173 | case SHOW_RAYS: 174 | resizeContents( viewport()->size().width(), 175 | viewport()->size().height() ); 176 | break; 177 | } 178 | 179 | viewport()->repaint( FALSE ); 180 | // plot_robot_path(); 181 | } 182 | 183 | void 184 | MapView::clearMap( void ) 185 | { 186 | int x, y; 187 | for (x=0;xmapsize.x;x++) { 188 | for (y=0;ymapsize.y;y++) { 189 | global_map->mapsum[x][y] = 0; 190 | global_map->maphit[x][y] = 0; 191 | global_map->mapprob[x][y] = settings.global_map_std_val; 192 | } 193 | } 194 | global_map->offset = rpos; 195 | updateMap(); 196 | } 197 | 198 | void 199 | MapView::paintRobot( logtools_rpos2_t pos ) 200 | { 201 | int rsize; 202 | logtools_ivector2_t mpos; 203 | QBrush brush( yellow ); 204 | int vx, vy; 205 | switch (maptype) { 206 | case GLOBAL_MAP: 207 | if (map_pos_from_rpos( pos, global_map, &mpos )) { 208 | contentsToViewport( global_map->mapsize.x-1-mpos.x, mpos.y, vx, vy ); 209 | if ( vx>=0 && vxmapsize.x && 210 | vy>=0 && vymapsize.y) { 211 | pt->setBrush( brush ); 212 | pt->setPen(red); 213 | pt->drawEllipse( vx, vy, 214 | settings.display_pixel_robot_size, 215 | settings.display_pixel_robot_size ); 216 | } 217 | rpos = pos; 218 | } 219 | break; 220 | case LOCAL_MAP: 221 | rsize = (int) ( ( settings.display_robot_size * scale ) / 222 | settings.local_map_resolution ); 223 | if (map_pos_from_rpos( pos, local_map, &mpos )) { 224 | contentsToViewport( mpos.x, local_map->mapsize.y*scale-mpos.y-1, 225 | vx, vy ); 226 | if ( vx>=0 && vxmapsize.x && 227 | vy>=0 && vymapsize.y) { 228 | pt->setBrush( brush ); 229 | pt->setPen(red); 230 | pt->drawEllipse( vx, vy, rsize, rsize ); 231 | } 232 | } 233 | break; 234 | } 235 | } 236 | 237 | void 238 | MapView::showRays( void ) 239 | { 240 | int c, i, sy; 241 | 242 | 243 | pt->eraseRect ( 0, 0, 244 | viewport()->size().width(), 245 | viewport()->size().height() ); 246 | sy = (int) (viewport()->size().height() / 2.0); 247 | if (current_scan != NULL) { 248 | for (i=0; ilaser.numvalues; i++) { 249 | c = 255; 250 | pt->setPen( QPen( qRgb( c, c, c ), 3 ) ); 251 | pt->drawLine( 5, sy, 252 | (int) (5+( current_scan->coord[i].relpt.x ) / beam_factor), 253 | (int) (sy-( current_scan->coord[i].relpt.y ) / beam_factor) ); 254 | } 255 | } 256 | } 257 | 258 | 259 | void 260 | MapView::centerRobot( void ) 261 | { 262 | logtools_ivector2_t mpos; 263 | switch(maptype) { 264 | case GLOBAL_MAP: 265 | if (map_pos_from_rpos( rpos, global_map, &mpos )) { 266 | center( (int) (global_map->mapsize.x-1-mpos.x), (int) mpos.y ); 267 | } 268 | break; 269 | case LOCAL_MAP: 270 | center( (int) ( local_map->center.x * scale ), 271 | (int) ( local_map->center.y * scale ) ); 272 | 273 | break; 274 | } 275 | } 276 | 277 | -------------------------------------------------------------------------------- /src/fast-slam/graphics.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "fast-slam.h" 9 | 10 | #ifdef __cplusplus 11 | } 12 | #endif 13 | 14 | #include "graphics.h" 15 | 16 | QString 17 | number2str( int num ) 18 | { 19 | if (num<10) { 20 | return("000"+QString::number( num )); 21 | } else if (num<100) { 22 | return("00"+QString::number( num )); 23 | } else if (num<1000) { 24 | return("0"+QString::number( num )); 25 | } 26 | return(QString::number( num )); 27 | } 28 | 29 | QString 30 | dumpMapName( char * prefix ) 31 | { 32 | static int dumpCtr = 0; 33 | 34 | QString str = "/dev/null"; 35 | QFileInfo fi; 36 | 37 | do { 38 | str = QString( QString( prefix ) + number2str( dumpCtr ) + ".png" ); 39 | fi = str; 40 | dumpCtr++; 41 | } while( fi.exists()); 42 | 43 | return( str ); 44 | } 45 | 46 | MapPainter::MapPainter( QWidget* parent, const char * ) 47 | : QScrollView( parent ) 48 | { 49 | #if QT_VERSION >= 300 50 | setSizePolicy( QSizePolicy( (QSizePolicy::SizeType)7, 51 | (QSizePolicy::SizeType)7, 52 | 1, 1, 53 | sizePolicy().hasHeightForWidth() ) ); 54 | #else 55 | setSizePolicy( QSizePolicy( (QSizePolicy::SizeType)7, 56 | (QSizePolicy::SizeType)7, 57 | sizePolicy().hasHeightForWidth() ) ); 58 | #endif 59 | 60 | // 4000 pixel is the maximum size a qpainter can paint 61 | pm = new QPixmap(4000,4000); 62 | pm->fill( QColor( 200, 200, 255 ) ); 63 | 64 | viewport()->setBackgroundMode( PaletteBase ); 65 | pt = new QPainter( pm ); 66 | } 67 | 68 | void 69 | MapPainter::setSize( int size_x, int size_y ) 70 | { 71 | if (settings.dump_screen) { 72 | pix->resize( size_x, size_y ); 73 | } 74 | image = new QImage( size_x, size_y, 8, 256 ); 75 | for (int i=0; i<256; i++) { 76 | image->setColor( i, qRgb( i, i, i ) ); 77 | } 78 | image->setColor( 0, qRgb( 200, 200, 255 ) ); 79 | image->setColor( 1, qRgb( 255, 0, 0 ) ); 80 | } 81 | 82 | 83 | void 84 | MapPainter::drawContents( QPainter * p, int , int , int , int ) 85 | { 86 | p->drawPixmap( 0, 0, *pm ); 87 | } 88 | 89 | void 90 | MapPainter::update( MAP2 map ) 91 | { 92 | int maxx = map.mapsize.x; 93 | int maxy = map.mapsize.y; 94 | 95 | // 4000 pixel is the maximum size a qpainter can paint 96 | if (maxx > 4000) 97 | maxx=4000; 98 | if (maxy > 4000) 99 | maxy=4000; 100 | 101 | for (int x=0;xsetPixel( (int) (maxx-1-x), (int) y, 1 ); 105 | } else { 106 | if (map.mapsum[x][y]>0) { 107 | image->setPixel( (int) (maxx-1-x), (int) y, 108 | (int) (255-253*map.mapprob[x][y]) ); 109 | } else { 110 | image->setPixel( (int) (maxx-1-x), (int) y, 0 ); 111 | } 112 | } 113 | } 114 | } 115 | if (settings.dump_screen) 116 | pix->convertFromImage( *image ); 117 | pt->drawImage( 0, 0, *image ); 118 | doPaint(); 119 | if (settings.dump_screen) { 120 | QPixmap pixmap = QPixmap::grabWindow( this->winId() ); 121 | fprintf( stderr, "dump picture\n" ); 122 | pixmap.save( dumpMapName(settings.dump_filename), "PNG" ); 123 | } 124 | } 125 | 126 | void 127 | MapPainter::refresh( MAP2 ) 128 | { 129 | viewport()->repaint( TRUE ); 130 | } 131 | 132 | void 133 | MapPainter::showscan( MAP2 map, logtools_lasersens2_data_t lsens, double maxrange ) 134 | { 135 | int i; 136 | logtools_rmove2_t nomove = {0.0, 0.0, 0.0}; 137 | logtools_ivector2_t start, end; 138 | logtools_vector2_t abspt; 139 | QPen usedpen = QPen( qRgb( 255, 0, 0 ) ); 140 | QPen maxrpen = QPen( qRgb( 255, 255, 0 ) ); 141 | 142 | if (map_pos_from_rpos( lsens.estpos, &map, &start ) ) { 143 | for (i=0; isetPen( usedpen ); 146 | abspt = logtools_compute_laser_points( lsens.estpos, lsens.laser.val[i], 147 | nomove, lsens.laser.angle[i] ); 148 | } else { 149 | pt->setPen( maxrpen ); 150 | abspt = logtools_compute_laser_points( lsens.estpos, maxrange, 151 | nomove, lsens.laser.angle[i] ); 152 | } 153 | if ( map_pos_from_vec2( abspt, &map, &end ) ) { 154 | pt->drawLine( (map.mapsize.x-1-start.x), start.y, 155 | (map.mapsize.x-1-end.x), end.y ); 156 | } 157 | } 158 | } 159 | } 160 | 161 | #define NUM_COLORS 4 162 | 163 | RGB colors[] = { { 255, 0, 0 }, 164 | { 0, 255, 0 }, 165 | { 255, 255, 0 }, 166 | { 0, 0, 255 } }; 167 | 168 | void 169 | MapPainter::drawrobot( MAP2 map, logtools_rpos2_t pos, int color ) 170 | { 171 | 172 | logtools_ivector2_t vec; 173 | int cidx = color % NUM_COLORS; 174 | QBrush brush = QBrush( qRgb( (int) colors[cidx].r, 175 | (int) colors[cidx].g, 176 | (int) colors[cidx].b ) ); 177 | 178 | if (map_pos_from_rpos( pos, &map, &vec ) ) { 179 | 180 | pt->setBrush( brush ); 181 | pt->drawEllipse( (map.mapsize.x-1-vec.x)-4, vec.y-4, 8, 8 ); 182 | if (settings.dump_screen) { 183 | QPainter dump; 184 | dump.begin(pix); 185 | dump.setBrush( brush ); 186 | dump.drawEllipse( (map.mapsize.x-1-vec.x)-4, vec.y-4, 8, 8 ); 187 | dump.end(); 188 | } 189 | } 190 | 191 | } 192 | 193 | void 194 | MapPainter::doPaint( ) { 195 | resizeContents( image->size().width(), image->size().height() ); 196 | viewport()->repaint( FALSE ); 197 | } 198 | 199 | 200 | void 201 | MapPainter::centerView( MAP2 map, logtools_rpos2_t pos ) 202 | { 203 | logtools_ivector2_t vec; 204 | if (map_pos_from_rpos( pos, &map, &vec ) ) { 205 | //center( (map.mapsize.x-1-vec.x)-4, vec.y-4); 206 | ensureVisible( (map.mapsize.x-1-vec.x)-4, vec.y-4); 207 | 208 | } 209 | } 210 | 211 | 212 | void 213 | MapPainter::drawparticles( MAP2 map, SAMPLE_SET pset, int, int showpath ) 214 | { 215 | 216 | int hist, i, j; 217 | logtools_ivector2_t vec, vec1, vec2; 218 | QPen pen1 = QPen( qRgb( 0, 0, 0 ) ); /* black */ 219 | QPainter dump; 220 | 221 | if (0) { 222 | pt->setBrush( NoBrush ); 223 | hist = pset.particle[0].histlen; 224 | for (j=0;jdrawRect( (map.mapsize.x-1-vec2.x)-1, vec1.y-1, 230 | vec2.x-vec1.x, vec2.y-vec1.y ); 231 | } 232 | } 233 | } 234 | 235 | pt->setPen( pen1 ); 236 | if (settings.dump_screen) { 237 | dump.begin(pix); 238 | dump.setPen( pen1 ); 239 | } 240 | 241 | for (i=0;idrawEllipse( (map.mapsize.x-1-vec.x)-1, vec.y-1, 2, 2 ); 244 | } 245 | if (showpath) { 246 | for (j=1;jdrawLine( (map.mapsize.x-1-vec1.x)-1, vec1.y-1, 250 | (map.mapsize.x-1-vec2.x)-1, vec2.y-1 ); 251 | if (settings.dump_screen) { 252 | dump.drawLine( (map.mapsize.x-1-vec1.x)-1, vec1.y-1, 253 | (map.mapsize.x-1-vec2.x)-1, vec2.y-1 ); 254 | } 255 | } 256 | } 257 | } 258 | } 259 | 260 | if (settings.dump_screen) { 261 | dump.end(); 262 | } 263 | 264 | } 265 | 266 | void 267 | MapPainter::dumpscreen( void ) 268 | { 269 | QString filename; 270 | if (settings.dump_screen) { 271 | filename = dumpMapName(settings.dump_filename); 272 | fprintf( stderr, "dump picture %s\n", filename.ascii() ); 273 | pix->save( filename, "PNG" ); 274 | } else { 275 | fprintf( stderr, "can't dump picture\n" ); 276 | } 277 | } 278 | 279 | 280 | -------------------------------------------------------------------------------- /src/mapper/match-map.c: -------------------------------------------------------------------------------- 1 | #include "map2d.h" 2 | 3 | double compute_orientation_diff( double start, double end ); 4 | 5 | double 6 | get_map_val( logtools_ivector2_t pos, MAP2 map ) 7 | { 8 | #ifdef SLOW 9 | fprintf( settings.devNULL, "%d %d -> %d %d\n", pos.x, pos.y, 10 | map.mapsize.x, 11 | map.mapsize.y ); 12 | #endif 13 | if ( pos.x>=0 && pos.x=0 && pos.y=0 && pos.x=0 && pos.y=0 && pos.x=0 && pos.ymval1) { 64 | val1 = mval1; 65 | } 66 | 67 | if (val2>mval2) { 68 | val2 = mval2; 69 | } 70 | 71 | ret = ( ((mval1-val1)/mval1) + ((mval2-val2)/mval2) )/2.0; 72 | 73 | return( ret ); 74 | } 75 | 76 | double 77 | probability_between_moves( logtools_rmove2_t move1, logtools_rmove2_t move2 ) 78 | { 79 | double sum = 0.0; 80 | sum += log( EPSILON + 81 | logtools_gauss_function( fabs( move1.forward-move2.forward ), 82 | 0, settings.motion_model.forward )); 83 | sum += log( EPSILON + 84 | logtools_gauss_function( fabs( move1.sideward-move2.sideward ), 85 | 0, settings.motion_model.sideward)); 86 | sum += log( EPSILON + 87 | logtools_gauss_function( fabs( compute_orientation_diff( move1.rotation, 88 | move2.rotation )), 89 | 0, settings.motion_model.rotation )); 90 | return( sum ); 91 | } 92 | 93 | int 94 | compute_rmap_pos_from_vec2( logtools_vector2_t vec, MAP2 map, logtools_ivector2_t *v ) 95 | { 96 | v->x = (int) (map.center.x + (vec.x/(double)map.resolution)); 97 | v->y = (int) (map.center.y + (vec.y/(double)map.resolution)); 98 | if (v->x<0) { 99 | return(FALSE); 100 | } else if (v->x>map.mapsize.x-1) { 101 | return(FALSE); 102 | } 103 | if (v->y<0) { 104 | return(FALSE); 105 | } else if (v->y>map.mapsize.y-1) { 106 | return(FALSE); 107 | } 108 | return(TRUE); 109 | } 110 | 111 | double 112 | error( double val, double expect ) 113 | { 114 | double sigma1=40.0; 115 | double sigma2=80.0; 116 | if ( fabs(val-expect)expect) { 134 | return( EPSILON ); 135 | } else { 136 | return( EPSILON+(val-start)*approx ); 137 | } 138 | } 139 | 140 | double 141 | ivector2_distance( logtools_ivector2_t p1, logtools_ivector2_t p2 ) 142 | { 143 | return sqrt( (p1.x-p2.x)*(p1.x-p2.x) + 144 | (p1.y-p2.y)*(p1.y-p2.y) ); 145 | } 146 | 147 | double 148 | compute_beam_log_prob( double expected, double measured ) 149 | { 150 | double val, d = fabs(expected-measured); /* dist in cm */ 151 | 152 | if (measured>0.95*settings.local_map_max_range) 153 | return(log(0.01)); 154 | if (d>settings.local_map_max_range) 155 | d = settings.local_map_max_range; 156 | if (d<200.0) { 157 | val = (220.0-d)/220.0; 158 | } else { 159 | val = 0.01; 160 | } 161 | return(log(val)); 162 | 163 | } 164 | 165 | 166 | double 167 | probability_with_move( MAP2 map, 168 | logtools_lasersens2_data_t data, 169 | logtools_rmove2_t move, 170 | logtools_rmove2_t odo_move, 171 | double *laserprob ) 172 | { 173 | int i; 174 | logtools_vector2_t pt; 175 | logtools_ivector2_t mvec; 176 | logtools_rpos2_t rpos; 177 | logtools_rpos2_t npos = {0.0, 0.0, 0.0}; 178 | static logtools_rmove2_t nomove = {0.0, 0.0, 0.0}; 179 | 180 | double bprob, prob = 0.0; 181 | 182 | rpos = logtools_rpos2_with_movement2( npos, move ); 183 | 184 | for (i=0;ipprob) { 297 | pmove = tmove; 298 | pprob = prob; 299 | } 300 | } 301 | 302 | if (pprob-bprob>EPSILON) { 303 | bmove = pmove; 304 | bprob = pprob; 305 | } else if (loop0) 317 | adjusting = FALSE; 318 | 319 | } /* end while( adjust ) */ 320 | 321 | return(bmove); 322 | } 323 | -------------------------------------------------------------------------------- /src/mapper/run_file.c: -------------------------------------------------------------------------------- 1 | #include "map2d.h" 2 | 3 | 4 | extern logtools_lasersens2_data_t * current_scan; 5 | extern logtools_rmove2_t * current_movement; 6 | 7 | logtools_log_data_t rec; 8 | 9 | int histcnt; 10 | int * history; 11 | 12 | double 13 | rpos2_dist( logtools_rpos2_t pos1, logtools_rpos2_t pos2 ) 14 | { 15 | return( 16 | sqrt( (pos1.x - pos2.x) * (pos1.x - pos2.x) + 17 | (pos1.y - pos2.y) * (pos1.y - pos2.y) ) 18 | ); 19 | } 20 | 21 | 22 | double 23 | rmove2_length( logtools_rmove2_t move ) 24 | { 25 | return( 26 | sqrt( (move.forward * move.forward) + 27 | (move.sideward * move.sideward) ) 28 | ); 29 | } 30 | 31 | void 32 | plot_robot_path( void ) 33 | { 34 | int i; 35 | for (i=0; iqtree), lmap, hk ); 146 | 147 | ctr = cnt; tctr = 0; 148 | for (h=cnt-1; 149 | h>=0 && 150 | h>cnt-settings.local_map_num_history && 151 | tctr 157 | settings.local_map_min_bbox_distance || 158 | (cnt-1-h<15) ) ) { 159 | lmove = 160 | logtools_movement2_between_rpos2( rec.lsens[history[h]].estpos, 161 | rec.lsens[history[cnt]].estpos ); 162 | create_local_map( lmap, rec.lsens[history[h]], lmove ); 163 | ctr = h; 164 | tctr++; 165 | } 166 | } 167 | printf( " - using %d scans for local map in history\n", tctr ); 168 | 169 | histcnt = cnt; 170 | ct = 0; 171 | 172 | /* convolve maps */ 173 | convolve_map( lmap ); 174 | 175 | bestmove = fit_data_in_local_map( *lmap, &(rec.lsens[i]), move ); 176 | 177 | printf( "best movment %.4f %.4f %.4f\n", 178 | bestmove.forward, bestmove.sideward, 179 | rad2deg(bestmove.rotation) ); 180 | 181 | rec.lsens[i].estpos = 182 | logtools_rpos2_with_movement2( rec.lsens[history[cnt]].estpos, 183 | bestmove ); 184 | 185 | if ( settings.use_error_analyze && 186 | rpos2_dist( rec.lsens[i].estpos, last_error_pos ) > 187 | settings.error_min_distance ) { 188 | 189 | e_ctr++; 190 | 191 | for (ef=0;ef<2*efs+1;ef++) { 192 | for (es=0;es<2*ess+1;es++) { 193 | for (er=0;er<2*ers+1;er++) { 194 | testmove = bestmove; 195 | testmove.forward += 196 | (ef-efs) * settings.error_forward_step; 197 | testmove.sideward += 198 | (es-efs) * settings.error_sideward_step; 199 | testmove.rotation += 200 | (er-ers) * settings.error_rotation_step; 201 | probability_with_move( *lmap, rec.lsens[i], testmove, 202 | bestmove, &(error[ef][es][er]) ); 203 | } 204 | } 205 | } 206 | for (ef=0;ef<2*efs+1;ef++) { 207 | for (es=0;es<2*ess+1;es++) { 208 | for (er=0;er<2*ers+1;er++) { 209 | } 210 | } 211 | } 212 | 213 | last_error_pos = rec.lsens[i].estpos; 214 | } 215 | 216 | 217 | for (l=0;l0?nh:1); 30 | 31 | data->lsens = 32 | (logtools_lasersens2_data_t *) 33 | malloc( nh * sizeof(logtools_lasersens2_data_t) ); 34 | 35 | for (i=0; ilsens[i].laser.val = 37 | (float *) malloc( MAX_NUM_LASER_BEAMS * sizeof(double) ); 38 | data->lsens[i].laser.angle = 39 | (float *) malloc( MAX_NUM_LASER_BEAMS * sizeof(double) ); 40 | data->lsens[i].coord = 41 | (logtools_laser_coord2_t *) malloc( MAX_NUM_LASER_BEAMS * 42 | sizeof(logtools_laser_coord2_t) ); 43 | /* 44 | data->lsens[i].poly.pt = 45 | (VECTOR2 *) malloc( MAX_NUM_LASER_BEAMS * sizeof(VECTOR2) ); 46 | */ 47 | } 48 | 49 | data->lsens[0].estpos = settings.global_map_start_pos; 50 | 51 | movements->nummovements = nh; 52 | movements->pos = (logtools_rpos2_t *) malloc( nh * sizeof(logtools_rpos2_t) ); 53 | movements->move = (logtools_rmove2_t *) malloc( nh * sizeof(logtools_rmove2_t) ); 54 | } 55 | 56 | 57 | /**************************************************************** 58 | * ONLINE 59 | ****************************************************************/ 60 | 61 | void 62 | run_online( MAP2D_SETTINGS settings, MAP2 * lmap, MAP2 * gmap, 63 | int argc, char *argv[] ) 64 | { 65 | static int update_ctr = 0; 66 | 67 | int h, l, hk, tctr, r, r_new, r_old, ctr=0; 68 | 69 | REC2_MOVEMENTS movements; 70 | logtools_rpos2_t npos = {0.0, 0.0, 0.0}; 71 | 72 | logtools_rmove2_t lmove, move, bestmove; 73 | logtools_rmove2_t nomove = { 0.0, 0.0, 0.0 }; 74 | logtools_laser_coord2_t coord[MAX_NUM_LASER_VALUES]; 75 | 76 | int moctr, loctr, nhist; 77 | 78 | hk = (settings.local_map_kernel_len-1)/2; 79 | nhist = settings.local_map_num_history; 80 | current_scan = NULL; 81 | 82 | alloc_online_structures( &online_data, &movements ); 83 | 84 | signal(SIGINT, abort_signal); 85 | 86 | ipc_init( argc, argv ); 87 | 88 | fprintf( stderr, "***************************************\n" ); 89 | fprintf( stderr, "* START\n" ); 90 | fprintf( stderr, "***************************************\n" ); 91 | 92 | loctr = 0; 93 | while (loop) { 94 | 95 | ipc_update(); 96 | 97 | if (online_laserupdate) { 98 | 99 | if (1 || online_odo_update) { 100 | 101 | r = online_scan_ctr%nhist; 102 | 103 | online_data.lsens[r].estpos = current_pos; 104 | 105 | online_laserupdate = FALSE; 106 | update_ctr++; 107 | 108 | /* 109 | compute_forward_correction( online_data.lsens[r].estpos, 110 | center, &pos ); 111 | online_data.lsens[r].estpos = pos; 112 | */ 113 | movements.pos[r] = 114 | online_data.lsens[r].estpos; 115 | 116 | if (settings.use_correction) { 117 | 118 | if (online_scan_ctr>0) { 119 | 120 | r_old = (nhist+online_scan_ctr-1)%nhist; 121 | r_new = (online_scan_ctr)%nhist; 122 | 123 | move = 124 | logtools_movement2_between_rpos2( movements.pos[r_old], 125 | current_pos ); 126 | 127 | if (rmove2_length(move)>settings.pos_diff_min_dist) { 128 | 129 | clear_local_treemap( &(lmap->qtree), lmap, hk ); 130 | create_local_map( lmap, 131 | online_data.lsens[r_old], nomove ); 132 | tctr = 0; 133 | moctr = r_old; 134 | for (h=online_scan_ctr-2; 135 | h>=0 && 136 | h>online_scan_ctr-settings.local_map_num_history && 137 | tctr 143 | settings.local_map_min_bbox_distance ) { 144 | lmove = 145 | logtools_movement2_between_rpos2( online_data.lsens[h%nhist].estpos, 146 | online_data.lsens[r_old].estpos ); 147 | moctr = h%nhist; 148 | create_local_map( lmap, online_data.lsens[h%nhist], lmove ); 149 | tctr++; 150 | } 151 | } 152 | convolve_map( lmap ); 153 | bestmove = fit_data_in_local_map( *lmap, 154 | &(online_data.lsens[r]), 155 | move ); 156 | 157 | if ( minimal_rmove_diff( bestmove, 158 | settings.pos_diff_min_dist, 159 | settings.pos_diff_min_rot )) { 160 | #ifdef VERBOSE 161 | printf( "using %d scans in history\n", tctr ); 162 | #endif 163 | 164 | /* position of last scan */ 165 | online_data.lsens[r].estpos = 166 | logtools_rpos2_with_movement2( online_data.lsens[r_old].estpos, 167 | bestmove ); 168 | for (l=0;l Starting make" 185 | $(install_includes) 186 | $(install_manpages) 187 | ifneq (Makefile.depend, $(wildcard Makefile.depend)) 188 | $(SILENT) $(MAKE) depend 189 | endif 190 | $(SILENT) $(MAKE) libraries 191 | $(install_libraries) 192 | $(ECHO) " <-- Make done" 193 | 194 | phase2: 195 | $(banner) 196 | $(ECHO) "" 197 | $(ECHO) " --> Starting make" 198 | $(SILENT) $(MAKE) binaries 199 | $(install_binaries) 200 | $(ECHO) " <-- Make done" 201 | 202 | all: 203 | $(banner) 204 | $(ECHO) "" 205 | $(ECHO) " --> Starting make" 206 | $(install_includes) 207 | $(install_manpages) 208 | ifneq (Makefile.depend, $(wildcard Makefile.depend)) 209 | $(SILENT) $(MAKE) depend 210 | endif 211 | $(SILENT) $(MAKE) libraries 212 | $(install_libraries) 213 | $(SILENT) $(MAKE) binaries 214 | $(install_binaries) 215 | $(ECHO) " <-- Make done" 216 | 217 | install: 218 | $(banner) 219 | $(global_install_libraries) 220 | $(global_install_binaries) 221 | $(global_install_includes) 222 | 223 | clean: 224 | $(ECHO) " ---- Cleaning up "$(MODULE_NAME) 225 | $(SILENT) $(RM) *.o *.a *.so *.exe core a.out Makefile.depend Makefile.depend.bak $(TARGETS) $(ADD_CLEAN) $(patsubst %.class,'%.class',$(CLASS_FILES)) 226 | $(subdir_recurse) 227 | 228 | relink: 229 | $(SILENT) $(RM) $(filter-out %.a, $(TARGETS)) 230 | $(SILENT) for i in $(filter-out %.a, $(TARGETS)) ; do \ 231 | $(MAKE) $$i ; \ 232 | done 233 | $(subdir_recurse) 234 | 235 | distclean: 236 | $(ECHO) " ---- Cleaning up "$(MODULE_NAME) 237 | $(SILENT) $(RM) *.o *.a *.so *.exe *~ core a.out 238 | $(SILENT) $(RM) Makefile.depend Makefile.depend.bak $(TARGETS) $(ADD_CLEAN) $(patsubst %.class,'%.class',$(CLASS_FILES)) 239 | $(remove_includes) 240 | $(remove_manpages) 241 | $(remove_libraries) 242 | $(remove_binaries) 243 | $(RM) core gmon.out *~ .\#* \#* 244 | $(subdir_recurse) 245 | 246 | export: 247 | $(install_includes) 248 | $(subdir_recurse) 249 | 250 | dep depend: 251 | $(ECHO) " ---- Assigning dependencies in "$(MODULE_NAME) 252 | $(SILENT) $(TOUCH) Makefile.depend 253 | $(SILENT) if test ! "$(SOURCES)x" = x ; then \ 254 | $(CC) -M $(SOURCES) $(IFLAGS) $(filter -D%, $(CFLAGS)) > Makefile.depend ; \ 255 | fi 256 | $(SILENT) $(RM) Makefile.depend.bak 257 | $(subdir_recurse) 258 | 259 | debug: 260 | $(SILENT) $(MAKE) MSP_DEBUG=1 all 261 | 262 | loud: 263 | $(SILENT) $(MAKE) MAKEFLAGS= LOUD=1 all 264 | 265 | .SUFFIXES: .c .o .a .so .C 266 | 267 | .PHONY: all clean dep debug 268 | 269 | %.o: %.c 270 | $(ECHO) " ---- Compiling $< to $@ (C)" 271 | $(SILENT) $(CC) $(CFLAGS) $(IFLAGS) -c $< -o $@ 272 | 273 | %.a: 274 | $(ECHO) " ---- Archiving $^ into $@ (C)" 275 | $(SILENT) $(AR) $@ $^ 276 | $(SILENT) $(RANLIB) $@ 277 | 278 | %.o: %.C 279 | $(ECHO) " ---- Compiling $< to $@ (C++)" 280 | $(SILENT) $(CXX) $(CXXFLAGS) $(IFLAGS) -c $< -o $@ 281 | 282 | %.o: %.cpp 283 | $(ECHO) " ---- Compiling $< to $@ (C++)" 284 | $(SILENT) $(CXX) $(CXXFLAGS) $(IFLAGS) -c $< 285 | 286 | 287 | %.o: %.cc 288 | $(ECHO) " ---- Compiling $< to $@ (C++)" 289 | $(SILENT) $(CXX) $(CXXFLAGS) $(IFLAGS) -c $< 290 | 291 | %.so: 292 | $(ECHO) " ---- Archiving $^ into $@ (C)" 293 | $(SILENT) $(CXX) -shared -o $@ $^ 294 | 295 | %.doxygen: 296 | $(ECHO) " ---- Compiling $< (Doxygen)" 297 | $(SILENT) $(DOXYGEN) $< 298 | 299 | %: 300 | $(ECHO) " ---- Linking $^ to $@ (C)" 301 | $(SILENT) $(LINK) $(filter %.o, $^) $(filter %.a, $^) -o $@ -L. $(patsubst lib%.a,-l%,$(filter %.a, $^)) $(LFLAGS) $(LFLAGS_POST) 302 | ifdef __CROSS_COMPILE 303 | $(ECHO) " ---- Stripping $@ (arm)" 304 | $(SILENT) $(STRIP) $@ 305 | endif 306 | 307 | 308 | .SECONDARY: 309 | 310 | ./%.h: %.ui 311 | $(ECHO) " ---- Creating Header file for $< (uic)" 312 | $(SILENT) $(QT3_DIR)/bin/uic $< -o $@ 313 | 314 | ./%.cpp: %.ui 315 | $(ECHO) " ---- Creating C++ file for $< (uic)" 316 | $(SILENT) $(QT3_DIR)/bin/uic -impl $*.h $< -o $@ 317 | 318 | ./%-moc.cpp %-moc.cpp: %.h 319 | $(ECHO) " ---- Creating MOC C++ file for $< (moc)" 320 | $(SILENT) $(QT3_DIR)/bin/moc -o $@ $< 321 | 322 | 323 | ifeq (Makefile.depend, $(wildcard Makefile.depend)) 324 | include Makefile.depend 325 | endif 326 | -------------------------------------------------------------------------------- /src/mapper/map2d.c: -------------------------------------------------------------------------------- 1 | #include "map2d.h" 2 | 3 | logtools_vector2_t 4 | map2d_compute_laser_abs_point( logtools_rpos2_t rpos, double val, 5 | logtools_rmove2_t offset, double angle ) 6 | { 7 | logtools_vector2_t abspt; 8 | abspt.x = 9 | rpos.x + 10 | cos( angle+offset.rotation+rpos.o ) * val; 11 | abspt.y = 12 | rpos.y + 13 | sin( angle+offset.rotation+rpos.o ) * val; 14 | return(abspt); 15 | } 16 | 17 | logtools_laser_coord2_t 18 | map2d_compute_laser2d_coord_with_offset( logtools_lasersens2_data_t lsens, 19 | int i ) 20 | { 21 | double val; 22 | logtools_laser_coord2_t coord = { {0.0,0.0},{0.0,0.0},0,0 }; 23 | logtools_rpos2_t rpos, npos; 24 | 25 | rpos = logtools_rpos2_with_movement2( lsens.estpos, 26 | lsens.laser.offset ); 27 | npos.x = 0.0; 28 | npos.y = 0.0; 29 | npos.o = 0.0; 30 | 31 | val = lsens.laser.val[i]; 32 | coord.relpt = logtools_compute_laser_points( npos, val, 33 | lsens.laser.offset, 34 | lsens.laser.angle[i] ); 35 | coord.abspt = logtools_compute_laser_points( rpos, val, 36 | lsens.laser.offset, 37 | lsens.laser.angle[i] ); 38 | return(coord); 39 | } 40 | 41 | logtools_laser_coord2_t 42 | map2d_compute_laser2d_coord( logtools_lasersens2_data_t lsens, int i ) 43 | { 44 | static double val; 45 | static logtools_laser_coord2_t coord; 46 | static logtools_rpos2_t npos; 47 | static logtools_rmove2_t nomove = {0.0, 0.0, 0.0}; 48 | 49 | npos.x = 0.0; 50 | npos.y = 0.0; 51 | npos.o = 0.0; 52 | 53 | val = lsens.laser.val[i]; 54 | coord.relpt = logtools_compute_laser_points( npos, val, 55 | nomove, 56 | lsens.laser.angle[i] ); 57 | coord.abspt = logtools_compute_laser_points( lsens.estpos, val, 58 | nomove, 59 | lsens.laser.angle[i] ); 60 | return(coord); 61 | } 62 | 63 | void 64 | compute_laser2d_points( logtools_log_data_t *rec, int laserID ) 65 | { 66 | int i, j; 67 | fprintf( stderr, "compute points ...\n" ); 68 | for (j=0;jnumlaserscans;j++) { 69 | if (rec->lsens[j].id == laserID) { 70 | if (rec->lsens[j].coord==NULL) { 71 | rec->lsens[j].coord = 72 | (logtools_laser_coord2_t *) malloc( rec->lsens[j].laser.numvalues * 73 | sizeof(logtools_laser_coord2_t) ); 74 | for (i=0;ilsens[j].laser.numvalues;i++) { 75 | rec->lsens[j].coord[i] = map2d_compute_laser2d_coord(rec->lsens[j], i); 76 | } 77 | } 78 | } 79 | } 80 | } 81 | 82 | logtools_vector2_t 83 | compute_rel_coord2_with_offset( logtools_lasersens2_data_t lsens, int i, 84 | logtools_rmove2_t offset ) 85 | { 86 | double angle, val, rot; 87 | logtools_vector2_t origin; 88 | logtools_vector2_t relpt; 89 | angle = lsens.laser.angle[i]; 90 | val = lsens.laser.val[i]; 91 | origin.x = lsens.laser.offset.forward + offset.forward; 92 | origin.y = lsens.laser.offset.sideward + offset.sideward; 93 | rot = lsens.laser.offset.rotation + offset.rotation; 94 | if (val>settings.local_map_max_range) 95 | val = settings.local_map_max_range; 96 | relpt.x = 97 | origin.x + cos( (angle+rot) ) * val; 98 | relpt.y = 99 | origin.y + sin( (angle+rot) ) * val; 100 | return(relpt); 101 | } 102 | 103 | int 104 | minimal_rpos_diff( logtools_rpos2_t pos1, logtools_rpos2_t pos2, 105 | double pos_diff_min_dist, 106 | double pos_diff_min_rot ) 107 | { 108 | logtools_vector2_t v1, v2; 109 | v1.x = pos1.x; v1.y = pos1.y; 110 | v2.x = pos2.x; v2.y = pos2.y; 111 | if ( logtools_vector2_distance(v1,v2) > pos_diff_min_dist ) 112 | return(TRUE); 113 | if ( compute_orientation_diff(pos1.o,pos2.o) > pos_diff_min_rot ) 114 | return(TRUE); 115 | return(FALSE); 116 | } 117 | 118 | int 119 | minimal_rmove_diff( logtools_rmove2_t move, 120 | double pos_diff_min_dist, 121 | double pos_diff_min_rot ) 122 | { 123 | logtools_vector2_t v1; 124 | v1.x = move.forward; v1.y = move.sideward; 125 | if ( logtools_vector2_length(v1) > pos_diff_min_dist ) 126 | return(TRUE); 127 | if ( move.rotation > pos_diff_min_rot ) 128 | return(TRUE); 129 | return(FALSE); 130 | } 131 | 132 | #define BOX_MAX_SIZE 600.0 133 | #define BOX_ADD_SIZE 0.0 134 | #define TIME_HIST 200 135 | #define MIN_VEL 20.0 136 | 137 | int 138 | isInBox( logtools_vector2_t p, logtools_vector2_t ll, logtools_vector2_t ur ) 139 | { 140 | double w, h, cx, cy; 141 | w = ur.x - ll.x; 142 | h = ur.y - ll.y; 143 | if (w>BOX_MAX_SIZE || h>BOX_MAX_SIZE) { 144 | cx = ll.x + w/2.0; 145 | cy = ll.y + h/2.0; 146 | if ( p.x > cx-(BOX_MAX_SIZE/2.0) && 147 | p.x < cx+(BOX_MAX_SIZE/2.0) && 148 | p.y > cy-(BOX_MAX_SIZE/2.0) && 149 | p.y < cy+(BOX_MAX_SIZE/2.0) ) 150 | return(1); 151 | else 152 | return(0); 153 | } else { 154 | if ( p.x > ll.x-BOX_ADD_SIZE && 155 | p.x < ur.x+BOX_ADD_SIZE && 156 | p.y > ll.y-BOX_ADD_SIZE && 157 | p.y < ur.y+BOX_ADD_SIZE ) 158 | return(1); 159 | else 160 | return(0); 161 | } 162 | } 163 | 164 | void 165 | remove_rear_scans( logtools_log_data_t * rec, int laserID ) 166 | { 167 | int i, fstart = 0, remove = 0; 168 | 169 | fprintf( stderr, "remove all rear scans\n" ); 170 | for (i=0; inumlaserscans;i++) { 171 | if (rec->lsens[i].id == laserID) { 172 | if (i!=fstart) { 173 | rec->lsens[fstart] = rec->lsens[i]; 174 | } 175 | fstart++; 176 | } else { 177 | remove++; 178 | } 179 | } 180 | fprintf( stderr, "found %d front scans\n", fstart ); 181 | fprintf( stderr, "removed %d rear scans\n", remove ); 182 | rec->numlaserscans = fstart; 183 | } 184 | 185 | void 186 | save_rec2_movements( logtools_log_data_t rec, REC2_MOVEMENTS * save, int laserID ) 187 | { 188 | int i, cnt; 189 | logtools_rmove2_t nomove; 190 | save->nummovements = rec.numlaserscans; 191 | save->pos = (logtools_rpos2_t *) malloc( rec.numlaserscans * sizeof(logtools_rpos2_t) ); 192 | save->move = (logtools_rmove2_t *) malloc( rec.numlaserscans * sizeof(logtools_rmove2_t) ); 193 | cnt = -1; 194 | for (i=0; ipos[i] = rec.lsens[i].estpos; 196 | } 197 | nomove.forward = 0.0; 198 | nomove.sideward = 0.0; 199 | nomove.rotation = 0.0; 200 | for (i=0; imove[i] = logtools_movement2_between_rpos2( save->pos[cnt], 204 | save->pos[i] ); 205 | } else { 206 | save->move[i] = nomove; 207 | } 208 | cnt = i; 209 | } 210 | } 211 | } 212 | 213 | void 214 | add_noise( logtools_log_data_t * rec, REC2_MOVEMENTS orig, int laserID ) 215 | { 216 | int i; 217 | logtools_rmove2_t move, noise, nmove; 218 | double trans_factor, rot_factor; 219 | 220 | for (i=1; inumlaserscans;i++) { 221 | if (rec->lsens[i].id == laserID) { 222 | srand(settings.random_number); 223 | 224 | move = orig.move[i]; 225 | 226 | if (settings.noise_type == GAUSS_NOISE) { 227 | noise.forward = random_gauss(); 228 | noise.sideward = random_gauss(); 229 | noise.rotation = deg2rad(random_gauss()); 230 | trans_factor = settings.add_noise_val; 231 | rot_factor = settings.add_noise_val; 232 | } else { 233 | noise.forward = rand()/(double) RAND_MAX; 234 | if (rand()%2==1) 235 | noise.forward *= -1.0; 236 | noise.sideward = rand()/(double) RAND_MAX; 237 | if (rand()%2==1) 238 | noise.sideward *= -1.0; 239 | noise.rotation = rand()/(double) RAND_MAX; 240 | if (rand()%2==1) 241 | noise.rotation *= -1.0; 242 | trans_factor = 243 | ( settings.add_noise_val * 244 | sqrt( move.forward*move.forward + move.sideward*move.sideward ) ) / 245 | sqrt( noise.forward*noise.forward + noise.sideward*noise.sideward ); 246 | rot_factor = 247 | ( settings.add_noise_val * fabs( move.rotation ) ) / 248 | fabs( noise.rotation ); 249 | } 250 | 251 | nmove.forward = move.forward + trans_factor * noise.forward; 252 | nmove.sideward = move.sideward + trans_factor * noise.sideward; 253 | nmove.rotation = move.rotation + rot_factor * noise.rotation; 254 | 255 | rec->lsens[i].estpos = 256 | logtools_rpos2_with_movement2( rec->lsens[i-1].estpos, nmove ); 257 | } 258 | } 259 | } 260 | 261 | void 262 | compute_laser2d_bbox( logtools_lasersens2_data_t *lsens ) 263 | { 264 | int i; 265 | 266 | logtools_vector2_t min,max; 267 | min.x = MAXDOUBLE; 268 | min.y = MAXDOUBLE; 269 | max.x = -MAXDOUBLE; 270 | max.y = -MAXDOUBLE; 271 | for (i=0;ilaser.numvalues;i++) { 272 | if (lsens->laser.val[i]coord[i].abspt.xcoord[i].abspt.x; 275 | if (lsens->coord[i].abspt.ycoord[i].abspt.y; 277 | if (lsens->coord[i].abspt.x>max.x) 278 | max.x = lsens->coord[i].abspt.x; 279 | if (lsens->coord[i].abspt.y>max.y) 280 | max.y = lsens->coord[i].abspt.y; 281 | } 282 | } 283 | lsens->bbox.min = min; 284 | lsens->bbox.max = max; 285 | } 286 | 287 | logtools_bounding_box_t 288 | laser2d_bbox( int numvalues, float *val, logtools_laser_coord2_t *coord ) 289 | { 290 | static logtools_bounding_box_t bbox; 291 | static logtools_vector2_t min, max; 292 | int i; 293 | 294 | min.x = MAXDOUBLE; min.y = MAXDOUBLE; 295 | max.x = -MAXDOUBLE; max.y = -MAXDOUBLE; 296 | 297 | for (i=0;imax.x) 304 | max.x = coord[i].abspt.x; 305 | if (coord[i].abspt.y>max.y) 306 | max.y = coord[i].abspt.y; 307 | } 308 | } 309 | bbox.min = min; 310 | bbox.max = max; 311 | return(bbox); 312 | } 313 | 314 | int 315 | intersect_bboxes( logtools_bounding_box_t box1, logtools_bounding_box_t box2 ) 316 | { 317 | if (box1.min.x<=box2.min.x) { 318 | /* box1.min.x is smaller that box2 */ 319 | if (box1.max.x>box2.min.x) { 320 | /* intersection in x */ 321 | if (box1.min.y<=box2.min.y) { 322 | /* box1.min.y is smaller that box2 */ 323 | if (box1.max.y>box2.min.y) { 324 | /* intersection in y */ 325 | return(1); 326 | } else { 327 | return(0); 328 | } 329 | } else { 330 | /* box2.min.y is smaller that box1 */ 331 | if (box2.max.y>=box1.min.y) { 332 | /* intersection in y */ 333 | return(1); 334 | } else { 335 | return(0); 336 | } 337 | } 338 | } else { 339 | return(0); 340 | } 341 | } else { 342 | /* box2.min.x is smaller that box1 */ 343 | if (box2.max.x>=box1.min.x) { 344 | /* intersection in x */ 345 | if (box1.min.y<=box2.min.y) { 346 | /* box1.min.y is smaller that box2 */ 347 | if (box1.max.y>box2.min.y) { 348 | /* intersection in y */ 349 | return(1); 350 | } else { 351 | return(0); 352 | } 353 | } else { 354 | /* box2.min.y is smaller that box1 */ 355 | if (box2.max.y>=box1.min.y) { 356 | /* intersection in y */ 357 | return(1); 358 | } else { 359 | return(0); 360 | } 361 | } 362 | } else { 363 | return(0); 364 | } 365 | } 366 | return(0); 367 | } 368 | 369 | -------------------------------------------------------------------------------- /src/Makefile.rules: -------------------------------------------------------------------------------- 1 | banner = $(SILENT) \ 2 | echo ; \ 3 | echo "****************************************************************" ; \ 4 | echo "* Module : "$(MODULE_NAME) ; \ 5 | echo "* Comment : "$(MODULE_COMMENT) ; \ 6 | echo "****************************************************************" ; \ 7 | 8 | subdir_recurse = $(SILENT) \ 9 | for i in $(SUBDIRS) xxxx ; do \ 10 | if [ -d $$i ] ; then \ 11 | if ! $(MAKE) -C $$i $@ ; then \ 12 | exit -1; \ 13 | fi; \ 14 | fi \ 15 | done 16 | 17 | install_includes = $(SILENT) \ 18 | for i in $(PUBLIC_INCLUDES) xxxx ; do \ 19 | if test -f $$i && ! $(NEWER) $(INC_DIR)/carmen/$$i $$i ; then \ 20 | echo " ---- Copying $$i to $(INC_DIR)/carmen" ; \ 21 | export dir=`pwd` ; \ 22 | $(RM) -f $(INC_DIR)/carmen/$$i ; \ 23 | $(LN) -s $$dir/$$i $(INC_DIR)/carmen/$$i ; \ 24 | fi; \ 25 | done 26 | 27 | install_libraries = $(SILENT) \ 28 | for i in $(PUBLIC_LIBRARIES) xxxx ; do \ 29 | if test -f $$i && ! $(NEWER) $(LIB_DIR)/$$i $$i ; then \ 30 | echo " ---- Copying $$i to $(LIB_DIR)" ; \ 31 | export dir=`pwd` ; \ 32 | $(RM) -f $(LIB_DIR)/$$i ; \ 33 | $(LN) -s $$dir/$$i $(LIB_DIR)/$$i ; \ 34 | fi; \ 35 | done 36 | 37 | install_libraries_so = \ 38 | for i in $(PUBLIC_LIBRARIES_SO) xxxx ; do \ 39 | if test -f $$i.1 && ! $(NEWER) $(SHARED_DIR)/$$i $$i.1 ; then \ 40 | echo " ---- Copying $$i to $(SHARED_DIR)" ; \ 41 | export dir=`pwd` ; \ 42 | $(RM) -f $(SHARED_DIR)/$$i ; \ 43 | $(LN) -s $$dir/$$i.1 $(SHARED_DIR)/$$i ; \ 44 | fi; \ 45 | done 46 | 47 | install_binaries = $(SILENT) \ 48 | for i in $(PUBLIC_BINARIES) xxxx ; do \ 49 | if test -f $$i && ! $(NEWER) $(BIN_DIR)/$$i $$i ; then \ 50 | echo " ---- Copying $$i to $(BIN_DIR)" ; \ 51 | export dir=`pwd` ; \ 52 | $(RM) -f $(BIN_DIR)/$$i ; \ 53 | $(LN) -s $$dir/$$i $(BIN_DIR)/$$i ; \ 54 | fi; \ 55 | done 56 | 57 | 58 | 59 | install_manpages = $(SILENT) \ 60 | for i in $(MAN_PAGES) xxxx ; do \ 61 | if test -f $$i && ! $(NEWER) $(MAN_DIR)/$$i $$i ; then \ 62 | echo " ---- Copying $$i to $(MAN_DIR)" ; \ 63 | export dir=`pwd` ; \ 64 | $(RM) -f $(MAN_DIR)/$$i ; \ 65 | $(LN) -s $$dir/$$i $(MAN_DIR)/$$i ; \ 66 | fi; \ 67 | done 68 | 69 | global_install_includes = $(SILENT) \ 70 | for i in $(PUBLIC_INCLUDES) xxxx ; do \ 71 | if test -f $$i && \ 72 | ! $(NEWER) $(GLOBAL_INC_DIR)/carmen/$$i $$i ; then \ 73 | echo \ 74 | " ---- Copying $$i to $(GLOBAL_INC_DIR)/carmen" ; \ 75 | export dir=`pwd` ; \ 76 | $(RM) -f $(GLOBAL_INC_DIR)/carmen/$$i ; \ 77 | $(CP) $$dir/$$i $(GLOBAL_INC_DIR)/carmen/$$i ; \ 78 | fi; \ 79 | done 80 | 81 | global_install_libraries = $(SILENT) \ 82 | for i in $(PUBLIC_LIBRARIES) xxxx ; do \ 83 | if test -f $$i && ! $(NEWER) $(GLOBAL_LIB_DIR)/$$i $$i ; then \ 84 | echo " ---- Copying $$i to $(GLOBAL_LIB_DIR)" ; \ 85 | export dir=`pwd` ; \ 86 | $(RM) -f $(GLOBAL_LIB_DIR)/$$i ; \ 87 | $(CP) $$dir/$$i $(GLOBAL_LIB_DIR)/$$i ; \ 88 | fi; \ 89 | done 90 | 91 | global_install_libraries_so = $(SILENT) \ 92 | for i in $(PUBLIC_LIBRARIES_SO) xxxx ; do \ 93 | if test -f $$i && ! $(NEWER) $(GLOBAL_SHARED_DIR)/$$i $$i ; then \ 94 | echo " ---- Copying $$i to $(GLOBAL_SHARED_DIR)" ; \ 95 | export dir=`pwd` ; \ 96 | $(RM) -f $(GLOBAL_SHARED_DIR)/$$i ; \ 97 | $(CP) $$dir/$$i $(GLOBAL_SHARED_DIR)/$$i ; \ 98 | fi; \ 99 | done 100 | 101 | global_install_binaries = $(SILENT) \ 102 | for i in $(PUBLIC_BINARIES) xxxx ; do \ 103 | if test -f $$i && ! $(NEWER) $(GLOBAL_BIN_DIR)/$$i $$i ; then \ 104 | echo " ---- Copying $$i to $(GLOBAL_BIN_DIR)" ; \ 105 | export dir=`pwd` ; \ 106 | $(RM) -f $(GLOBAL_BIN_DIR)/$$i ; \ 107 | $(CP) $$dir/$$i $(GLOBAL_BIN_DIR)/$$i ; \ 108 | fi; \ 109 | done 110 | 111 | global_install_manpages = $(SILENT) \ 112 | for i in $(MAN_PAGES) xxxx ; do \ 113 | if test -f $$i && ! $(NEWER) $(GLOBAL_MAN_DIR)/$$i $$i ; then \ 114 | echo " ---- Copying $$i to $(GLOBAL_MAN_DIR)" ; \ 115 | export dir=`pwd` ; \ 116 | $(RM) -f $(GLOBAL_MAN_DIR)/$$i ; \ 117 | $(CP) $$dir/$$i $(GLOBAL_MAN_DIR)/$$i ; \ 118 | fi; \ 119 | done 120 | 121 | remove_includes = $(SILENT) \ 122 | for i in $(PUBLIC_INCLUDES) xxxx ; do \ 123 | if test ! "$$i" = "xxxx" ; then \ 124 | echo " ---- Removing $$i from $(INC_DIR)/carmen" ; \ 125 | $(RM) -f $(INC_DIR)/carmen/$$i ; \ 126 | fi ; \ 127 | done 128 | 129 | remove_libraries = $(SILENT) \ 130 | for i in $(PUBLIC_LIBRARIES) xxxx ; do \ 131 | if test ! "$$i" = "xxxx" ; then \ 132 | echo " ---- Removing $$i from $(LIB_DIR)" ; \ 133 | $(RM) -f $(LIB_DIR)/$$i ; \ 134 | fi ; \ 135 | done 136 | 137 | remove_libraries_so = $(SILENT) \ 138 | for i in $(PUBLIC_LIBRARIES_SO) xxxx ; do \ 139 | if test ! "$$i" = "xxxx" ; then \ 140 | echo " ---- Removing $$i from $(SHARED_DIR)" ; \ 141 | $(RM) -f $(SHARED_DIR)/$$i ; \ 142 | fi ; \ 143 | done 144 | 145 | remove_binaries = $(SILENT) \ 146 | for i in $(PUBLIC_BINARIES) xxxx ; do \ 147 | if test ! "$$i" = "xxxx" ; then \ 148 | echo " ---- Removing $$i from $(BIN_DIR)" ; \ 149 | $(RM) -f $(BIN_DIR)/$$i ; \ 150 | fi ; \ 151 | done 152 | 153 | remove_manpages = $(SILENT) \ 154 | for i in $(MAN_PAGES) xxxx ; do \ 155 | if test ! "$$i" = "xxxx" ; then \ 156 | echo " ---- Removing $$i from $(MAN_DIR)" ; \ 157 | $(RM) -f $(MAN_DIR)/$$i ; \ 158 | fi ; \ 159 | done 160 | 161 | global_remove_includes = $(SILENT) \ 162 | for i in $(PUBLIC_INCLUDES) xxxx ; do \ 163 | if test ! "$$i" = "xxxx" ; then \ 164 | echo " ---- Removing $$i from $(GLOBAL_INC_DIR)/carmen" ; \ 165 | $(RM) -f $(GLOBAL_INC_DIR)/carmen/$$i ; \ 166 | fi ; \ 167 | done 168 | 169 | global_remove_libraries = $(SILENT) \ 170 | for i in $(PUBLIC_LIBRARIES) xxxx ; do \ 171 | if test ! "$$i" = "xxxx" ; then \ 172 | echo " ---- Removing $$i from $(GLOBAL_LIB_DIR)" ; \ 173 | $(RM) -f $(GLOBAL_LIB_DIR)/$$i ; \ 174 | fi ; \ 175 | done 176 | 177 | global_remove_binaries = $(SILENT) \ 178 | for i in $(PUBLIC_BINARIES) xxxx ; do \ 179 | if test ! "$$i" = "xxxx" ; then \ 180 | echo " ---- Removing $$i from $(GLOBAL_BIN_DIR)" ; \ 181 | $(RM) -f $(GLOBAL_BIN_DIR)/$$i ; \ 182 | fi ; \ 183 | done 184 | 185 | global_remove_manpages = $(SILENT) \ 186 | for i in $(MAN_PAGES) xxxx ; do \ 187 | if test ! "$$i" = "xxxx" ; then \ 188 | echo " ---- Removing $$i from $(GLOBAL_MAN_DIR)" ; \ 189 | $(RM) -f $(GLOBAL_MAN_DIR)/$$i ; \ 190 | fi ; \ 191 | done 192 | 193 | libraries: 194 | $(SILENT) for i in $(filter %.a, $(TARGETS)) $(filter %.so, $(TARGETS)) xxxx ; do \ 195 | if test ! "$$i" = "xxxx" ; then \ 196 | if ! $(MAKE) $$i ; then \ 197 | exit -1; \ 198 | fi; \ 199 | fi ; \ 200 | done 201 | 202 | binaries: 203 | $(SILENT) for i in $(filter-out %.a, $(TARGETS)) xxxx ; do \ 204 | if test ! "$$i" = "xxxx" ; then \ 205 | if ! $(MAKE) $$i ; then \ 206 | exit -1; \ 207 | fi; \ 208 | fi ; \ 209 | done 210 | 211 | phase1: 212 | $(banner) 213 | $(ECHO) "" 214 | $(ECHO) " --> Starting make" 215 | $(install_includes) 216 | $(install_manpages) 217 | ifneq (Makefile.depend, $(wildcard Makefile.depend)) 218 | $(SILENT) $(MAKE) depend 219 | endif 220 | $(SILENT) $(MAKE) libraries 221 | $(install_libraries) 222 | $(ECHO) " <-- Make done" 223 | 224 | phase2: 225 | $(banner) 226 | $(ECHO) "" 227 | $(ECHO) " --> Starting make" 228 | $(SILENT) $(MAKE) binaries 229 | $(install_binaries) 230 | $(ECHO) " <-- Make done" 231 | 232 | phase3: 233 | $(install_libraries_so) 234 | 235 | phase4: 236 | 237 | 238 | all: 239 | $(banner) 240 | $(ECHO) "" 241 | $(ECHO) " --> Starting make" 242 | $(install_includes) 243 | $(install_manpages) 244 | ifneq (Makefile.depend, $(wildcard Makefile.depend)) 245 | $(SILENT) $(MAKE) depend 246 | endif 247 | $(SILENT) $(MAKE) libraries 248 | $(install_libraries) 249 | $(install_libraries_so) 250 | $(SILENT) $(MAKE) binaries 251 | $(install_binaries) 252 | $(ECHO) " <-- Make done" 253 | 254 | install: 255 | $(banner) 256 | $(global_install_libraries) 257 | $(global_install_binaries) 258 | $(global_install_includes) 259 | 260 | clean: 261 | $(ECHO) " ---- Cleaning up "$(MODULE_NAME) 262 | $(SILENT) $(RM) *.o *.a *.so *.exe core a.out Makefile.depend Makefile.depend.bak $(TARGETS) $(ADD_CLEAN) $(patsubst %.class,'%.class',$(CLASS_FILES)) 263 | $(subdir_recurse) 264 | 265 | relink: 266 | $(SILENT) $(RM) $(filter-out %.a, $(TARGETS)) 267 | $(SILENT) for i in $(filter-out %.a, $(TARGETS)) ; do \ 268 | $(MAKE) $$i ; \ 269 | done 270 | $(subdir_recurse) 271 | 272 | distclean: 273 | $(ECHO) " ---- Cleaning up "$(MODULE_NAME) 274 | $(SILENT) $(RM) *.o *.a *.so *.exe *~ core a.out 275 | $(SILENT) $(RM) Makefile.depend Makefile.depend.bak $(TARGETS) $(patsubst %.class,'%.class',$(CLASS_FILES)) 276 | $(SILENT) $(RM) $(ADD_CLEAN) 277 | $(remove_includes) 278 | $(remove_manpages) 279 | $(remove_libraries_so) 280 | $(remove_libraries) 281 | $(remove_binaries) 282 | $(RM) core gmon.out *~ .\#* \#* 283 | $(subdir_recurse) 284 | 285 | export: 286 | $(install_includes) 287 | $(subdir_recurse) 288 | 289 | dep depend: 290 | $(ECHO) " ---- Assigning dependencies in "$(MODULE_NAME) 291 | $(SILENT) $(TOUCH) Makefile.depend 292 | $(SILENT) if test ! "$(SOURCES)x" = x ; then \ 293 | $(CC) -M $(SOURCES) $(IFLAGS) $(filter -D%, $(CFLAGS)) > Makefile.depend ; \ 294 | fi 295 | $(SILENT) $(RM) Makefile.depend.bak 296 | $(subdir_recurse) 297 | 298 | debug: 299 | $(SILENT) $(MAKE) CARMEN_DEBUG=1 all 300 | 301 | loud: 302 | $(SILENT) $(MAKE) MAKEFLAGS= LOUD=1 all 303 | 304 | 305 | # This isn't the rule that adds a directory to the tar ball. 306 | # That happens in the top-level tar rule using recursion. 307 | # All this does is remove the unwanted CVS and .cvsignore files. 308 | 309 | tar-clean: 310 | $(ECHO) " Adding "$(MODULE_NAME)"..." 311 | dir=$${PWD/#*carmen/}; \ 312 | cd $(CARMEN_HOME)/..; \ 313 | tar --delete -f $(CARMEN_HOME)/../carmen.tar carmen$$dir/CVS ; \ 314 | if [ -f carmen/$$dir/.cvsignore ] ; then \ 315 | tar --delete -f $(CARMEN_HOME)/../carmen.tar \ 316 | carmen$$dir/.cvsignore ; \ 317 | fi 318 | $(subdir_recurse) 319 | 320 | .SUFFIXES: .c .o .a .so .C .cpp .cc 321 | 322 | .PHONY: all clean dep debug 323 | 324 | %.o: %.c 325 | $(ECHO) " ---- Compiling $< to $@ (C)" 326 | $(SILENT) $(CC) $(CFLAGS) $(IFLAGS) -c $< -o $@ 327 | 328 | %.a: 329 | $(ECHO) " ---- Archiving $^ into $@ (C)" 330 | $(SILENT) $(AR) $@ $^ 331 | $(SILENT) $(RANLIB) $@ 332 | 333 | %.o: %.C 334 | $(ECHO) " ---- Compiling $< to $@ (C++)" 335 | $(SILENT) $(CXX) $(CXXFLAGS) $(IFLAGS) -c $< -o $@ 336 | 337 | %.o: %.cpp 338 | $(ECHO) " ---- Compiling $< to $@ (C++)" 339 | $(SILENT) $(CXX) $(CXXFLAGS) $(IFLAGS) -c $< 340 | 341 | %.o: %.cc 342 | $(ECHO) " ---- Compiling $< to $@ (C++)" 343 | $(SILENT) $(CXX) $(CXXFLAGS) $(IFLAGS) -c $< 344 | 345 | %.so.1: 346 | $(ECHO) " ---- Archiving $^ into $@ (C)" 347 | $(SILENT) $(CXX) -shared -o $@ $^ 348 | 349 | %.so: 350 | $(ECHO) " ---- Archiving $^ into $@ (C)" 351 | $(SILENT) $(CXX) -shared -o $@ $^ 352 | 353 | %.doxygen: 354 | $(ECHO) " ---- Compiling $< (Doxygen)" 355 | $(SILENT) $(DOXYGEN) $< 356 | 357 | %: 358 | $(ECHO) " ---- Linking $^ to $@ (C)" 359 | $(SILENT) $(LINK) $(filter %.o, $^) $(filter %.a, $^) -o $@ -L. $(patsubst lib%.a,-l%,$(filter %.a, $^)) $(LFLAGS) $(LFLAGS_POST) 360 | 361 | 362 | .SECONDARY: 363 | 364 | ./%.h: %.ui 365 | $(ECHO) " ---- Creating Header file for $< (uic)" 366 | $(SILENT) $(QT3_DIR)/bin/uic $< -o $@ 367 | 368 | ./%.cpp: %.ui 369 | $(ECHO) " ---- Creating C++ file for $< (uic)" 370 | $(SILENT) $(QT3_DIR)/bin/uic -impl $*.h $< -o $@ 371 | 372 | ./%-moc.cpp %-moc.cpp: %.h 373 | $(ECHO) " ---- Creating MOC C++ file for $< (moc)" 374 | $(SILENT) $(QT3_DIR)/bin/moc -o $@ $< 375 | 376 | 377 | ifeq (Makefile.depend, $(wildcard Makefile.depend)) 378 | include Makefile.depend 379 | endif 380 | -------------------------------------------------------------------------------- /src/mapper/map2d.h: -------------------------------------------------------------------------------- 1 | #ifndef MAP2D_H 2 | #define MAP2D_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #define MAX_NAME_LENGTH 256 19 | #define EPSILON 0.00000001 20 | 21 | #define MAX_NUM_LASER_VALUES 401 22 | #define GAUSS_NOISE 0 23 | 24 | #define MAX_LINE_LENGTH 4096 25 | #define LOCAL_MAP_OBSTACLE_PROB 0.1 26 | 27 | enum LASER_TYPE { PLS, LMS }; 28 | 29 | #define ADDITIONAL_X_SIZE 0.0 30 | #define ADDITIONAL_Y_SIZE 0.0 31 | #define MAX_NUM_LASER_BEAMS 401 32 | 33 | #define ODO_NOTHING 0 34 | #define ODO_STD 1 35 | #define ODO_LAST 2 36 | 37 | #define GLOBAL_MAP 0 38 | #define LOCAL_MAP 1 39 | #define SHOW_RAYS 4 40 | 41 | #define UPDATE_TCX_LOOP 5 42 | 43 | typedef struct { 44 | int nummovements; 45 | logtools_rpos2_t * pos; 46 | logtools_rmove2_t * move; 47 | } REC2_MOVEMENTS; 48 | 49 | typedef struct { 50 | short x; 51 | short y; 52 | } logtools_svector2_t; 53 | 54 | typedef struct QUAD_TREE { 55 | struct QUAD_TREE * elem[4]; 56 | logtools_svector2_t center; 57 | unsigned char level; 58 | unsigned char inuse; 59 | } QUAD_TREE; 60 | 61 | #define UPDT_NOT 0 62 | #define UPDT_X 1 63 | #define UPDT_Y 2 64 | 65 | typedef struct { 66 | QUAD_TREE qtree; 67 | logtools_rpos2_t offset; 68 | double resolution; 69 | unsigned char ** updated; 70 | float ** maphit; 71 | short ** mapsum; 72 | float ** mapprob; 73 | float ** calc; 74 | logtools_ivector2_t mapsize; 75 | logtools_vector2_t center; 76 | } MAP2; 77 | 78 | enum MODES { ONLINE, READ_FILE }; 79 | 80 | typedef struct { 81 | int use_graphics; 82 | 83 | int use_correction; 84 | 85 | char robot_name[MAX_NAME_LENGTH]; 86 | int using_multiple_laser; 87 | int laser_number; 88 | int laser_direction; 89 | char laser_char_type[MAX_NAME_LENGTH]; 90 | enum LASER_TYPE laser_type; 91 | 92 | enum MODES mode; 93 | char data_filename[MAX_NAME_LENGTH]; 94 | FILE * dataF; 95 | 96 | int dump_maps; 97 | char dump_mapnames[MAX_NAME_LENGTH]; 98 | 99 | double max_usable_laser_range; 100 | 101 | double local_map_max_range; 102 | double local_map_resolution; 103 | int local_map_kernel_len; 104 | int local_map_use_odometry; 105 | int local_map_num_convolve; 106 | double local_map_std_val; 107 | int local_map_num_history; 108 | int local_map_max_used_history; 109 | double local_map_min_bbox_distance; 110 | int local_map_history_skip; 111 | double local_map_object_prob; 112 | 113 | double max_dynprob_change; 114 | double max_dynamic_prob; 115 | 116 | logtools_rmove2_t motion_model; 117 | 118 | int add_noise; 119 | double add_noise_val; 120 | int noise_type; 121 | int random_number; 122 | 123 | double pos_diff_min_dist; 124 | double pos_diff_max_dist; 125 | double pos_diff_min_rot; 126 | double pos_diff_max_rot; 127 | 128 | double pos_corr_step_size_forward; 129 | double pos_corr_step_size_sideward; 130 | double pos_corr_step_size_rotation; 131 | int pos_corr_step_size_loop; 132 | 133 | int use_error_analyze; 134 | double error_max_forward; 135 | double error_forward_step; 136 | double error_max_sideward; 137 | double error_sideward_step; 138 | double error_max_rotation; 139 | double error_rotation_step; 140 | double error_min_distance; 141 | 142 | int use_global_map; 143 | int global_map_ray_model; 144 | double global_map_max_range; 145 | int global_map_size_x; 146 | int global_map_size_y; 147 | int global_map_start_x; 148 | int global_map_start_y; 149 | double global_map_resolution; 150 | double global_map_std_val; 151 | logtools_rpos2_t global_map_start_pos; 152 | char global_map_filename[MAX_NAME_LENGTH]; 153 | 154 | double display_robot_size; 155 | int display_pixel_robot_size; 156 | 157 | FILE * devNULL; 158 | 159 | int log_output; 160 | char log_filename[MAX_NAME_LENGTH]; 161 | FILE * logF; 162 | 163 | int view_path; 164 | int show_updates; 165 | int change_map; 166 | 167 | int loop_sleep; 168 | 169 | } MAP2D_SETTINGS; 170 | 171 | typedef struct { 172 | int numlaserscans; 173 | logtools_lasersens2_data_t * lsens; 174 | } ONLINE_LASER_DATA; 175 | 176 | extern MAP2D_SETTINGS settings; 177 | extern ONLINE_LASER_DATA online_data; 178 | extern int online_dptr; 179 | extern int online_laserupdate; 180 | extern int online_scan_ctr; 181 | extern int online_odo_update; 182 | 183 | void grid_line( logtools_ivector2_t start, 184 | logtools_ivector2_t end, 185 | logtools_grid_line_t *line ); 186 | 187 | 188 | void initialize_map( MAP2 *map, int sx, int sy, int center_x, int center_y, 189 | double resolution, logtools_rpos2_t start ); 190 | 191 | void clear_local_map( MAP2 *map ); 192 | 193 | void create_local_map( MAP2 *map, 194 | logtools_lasersens2_data_t data, 195 | logtools_rmove2_t move ); 196 | 197 | logtools_vector2_t 198 | compute_rel_coord2_with_offset( logtools_lasersens2_data_t lsens, int i, 199 | logtools_rmove2_t offset ); 200 | 201 | logtools_rmove2_t fit_data_in_local_map( MAP2 map, 202 | logtools_lasersens2_data_t *data, 203 | logtools_rmove2_t movement ); 204 | 205 | void set_default( void ); 206 | 207 | void read_ini_file( char *filename ); 208 | 209 | void check_settings( void ); 210 | 211 | int compute_map_pos_from_rpos( logtools_rpos2_t rpos, MAP2 *map, logtools_ivector2_t *v ); 212 | 213 | int map_pos_from_vector2( logtools_vector2_t pos, MAP2 *map, logtools_ivector2_t *v ); 214 | 215 | int compute_map_pos_from_vector2( logtools_vector2_t pos, MAP2 *map, logtools_ivector2_t *v ); 216 | 217 | void compute_map( MAP2 * map ); 218 | 219 | void compute_map_probs( MAP2 * map ); 220 | 221 | void clear_global_map( MAP2 *map ); 222 | 223 | double compute_orientation_diff( double start, double end ); 224 | 225 | void initialize_global_map( MAP2 *map, int size_x, int size_y, 226 | int start_x, int start_y, double resolution, 227 | logtools_rpos2_t start, int convolve ); 228 | 229 | void update_global_map( logtools_lasersens2_data_t lsens, MAP2 * map ); 230 | 231 | void update_global_ray_map( logtools_lasersens2_data_t lsens, MAP2 * map, int loop ); 232 | 233 | logtools_laser_coord2_t map2d_compute_laser2d_coord( logtools_lasersens2_data_t lsens, int i ); 234 | 235 | void compute_laser2d_points( logtools_log_data_t *rec, int laserID ); 236 | 237 | logtools_vector2_t compute_rel_coord2_with_offset( logtools_lasersens2_data_t lsens, int i, 238 | logtools_rmove2_t offset ); 239 | 240 | int minimal_rpos_diff( logtools_rpos2_t pos1, logtools_rpos2_t pos2, 241 | double pos_diff_min_dist, 242 | double pos_diff_min_rot ); 243 | 244 | int minimal_rmove_diff( logtools_rmove2_t move, 245 | double pos_diff_min_dist, 246 | double pos_diff_min_rot ); 247 | 248 | void write_log_entry( FILE * fp, logtools_lasersens2_data_t * lsens ); 249 | 250 | int isInBox( logtools_vector2_t p, logtools_vector2_t ll, logtools_vector2_t ur ); 251 | 252 | void write_data_entry( FILE *fp, logtools_lasersens2_data_t lsens, int laserID ); 253 | 254 | void remove_rear_scans( logtools_log_data_t * rec, int laserID ); 255 | 256 | void save_rec2_movements( logtools_log_data_t rec, REC2_MOVEMENTS * save, 257 | int laserID ); 258 | 259 | void add_noise( logtools_log_data_t * rec, REC2_MOVEMENTS orig, int laserID ); 260 | 261 | void tree_list( QUAD_TREE *tree, int *ct ); 262 | 263 | void clear_local_treemap( QUAD_TREE *tree, MAP2 *map, int hk ); 264 | 265 | void convolve_map( MAP2 *map ); 266 | 267 | void compute_laser2d_bbox( logtools_lasersens2_data_t *lsens ); 268 | 269 | int intersect_bboxes( logtools_bounding_box_t box1, 270 | logtools_bounding_box_t box2 ); 271 | 272 | logtools_vector2_t map2d_compute_laser_abs_point( logtools_rpos2_t rpos, double val, 273 | logtools_rmove2_t offset, double angle ); 274 | 275 | void conncect_tcx( char* robotName ); 276 | 277 | void init_modul_names( char *extension ); 278 | 279 | void check_tcx_connections( void ); 280 | 281 | void simple_compute_map( MAP2 *map ); 282 | 283 | void simple_clear_map( MAP2 *map ); 284 | 285 | void compute_ray_map( MAP2 *map ); 286 | 287 | int compute_rmap_pos_from_vector2( logtools_vector2_t vec, MAP2 map, logtools_ivector2_t *v ); 288 | 289 | void dynamic_prob_from_global_map( logtools_lasersens2_data_t * data, MAP2 map ); 290 | 291 | void compute_probs_of_global_map( MAP2 * global_map ); 292 | 293 | void simple_convolve_map( MAP2 *map, logtools_gauss_kernel_t kernel ); 294 | 295 | 296 | int map_pos_from_rpos( logtools_rpos2_t rpos, MAP2 *map, logtools_ivector2_t *v ); 297 | 298 | void rpos_pos_from_map_pos( logtools_ivector2_t v, MAP2 *map, logtools_rpos2_t *rpos ); 299 | 300 | void vector2_from_map_pos( logtools_ivector2_t v, MAP2 *map, logtools_vector2_t *pos ); 301 | 302 | int map_pos_from_vector2( logtools_vector2_t pos, MAP2 *map, logtools_ivector2_t *v ); 303 | 304 | double probability_with_pos( MAP2 map, logtools_lasersens2_data_t data, 305 | logtools_rpos2_t rpos, logtools_rmove2_t move, logtools_rmove2_t odo_move ); 306 | 307 | void app_process_events( void ); 308 | 309 | void update_screen( logtools_lasersens2_data_t lsens ); 310 | 311 | void paint_robot( logtools_rpos2_t pos ); 312 | 313 | void plot_robot( logtools_rpos2_t pos ); 314 | 315 | void plot_robot_path( void ); 316 | 317 | void update_map( void ); 318 | 319 | void center_robot( void ); 320 | 321 | int window_maptype( void ); 322 | 323 | void window_show_rays( void ); 324 | 325 | double rpos2_dist( logtools_rpos2_t pos1, logtools_rpos2_t pos2 ); 326 | 327 | double rmove2_length( logtools_rmove2_t move ); 328 | 329 | void TCX_send_update_status( logtools_rpos2_t pos, struct timeval time ); 330 | 331 | int create_output_files( void ); 332 | 333 | void close_output_files( void ); 334 | 335 | void run_data_file( MAP2D_SETTINGS settings, 336 | MAP2 * lmap, MAP2 * gmap ); 337 | 338 | void run_online( MAP2D_SETTINGS settings, 339 | MAP2 * lmap, MAP2 * gmap, 340 | int argc, char *argv[] ); 341 | 342 | void create_script_file( int number ); 343 | 344 | double get_mapval( int pos_x, int pos_y, MAP2 map ); 345 | 346 | logtools_bounding_box_t laser2d_bbox( int numvalues, float *val, 347 | logtools_laser_coord2_t *coord ); 348 | 349 | void ipc_update( void ); 350 | 351 | void ipc_init( int argc, char *argv[] ); 352 | 353 | void ipc_stop( void ); 354 | 355 | void ipc_publish_status( logtools_rpos2_t pos, struct timeval time ); 356 | 357 | void map_change_settings( int set ); 358 | 359 | void update_change_map( void ); 360 | 361 | extern int change_map; 362 | 363 | double probability_with_move( MAP2 map, logtools_lasersens2_data_t data, 364 | logtools_rmove2_t move, logtools_rmove2_t odo_move, 365 | double *laserprob ); 366 | 367 | #endif 368 | -------------------------------------------------------------------------------- /src/fast-slam/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifdef __cplusplus 4 | extern "C" { 5 | #endif 6 | 7 | #include "fast-slam.h" 8 | 9 | #ifdef __cplusplus 10 | } 11 | #endif 12 | 13 | #include "graphics.h" 14 | 15 | #define MAX_STRING_LENGTH 80 16 | #define NUM_TESTS 200000 17 | #define EPSILON 0.000000000001 18 | 19 | int loop = TRUE; 20 | 21 | void 22 | print_usage( void ) 23 | { 24 | fprintf(stderr, "\nusage: fast-slam [-ini ] [-id NUM_LASER] \n" ); 25 | } 26 | 27 | void 28 | shutdown( int sig ) { 29 | loop = FALSE; 30 | exit(sig); 31 | } 32 | 33 | typedef struct { 34 | logtools_rpos2_t pos; 35 | struct timeval time; 36 | } logtools_rpos2_hist_t; 37 | 38 | void 39 | clear_map( MAP2 * map, logtools_rpos2_t pos ) 40 | { 41 | int x, y; 42 | for (x=0;xmapsize.x;x++) { 43 | for (y=0;ymapsize.y;y++) { 44 | map->maphit[x][y] = 0.0; 45 | map->mapsum[x][y] = 0; 46 | map->calc[x][y] = 0.0; 47 | } 48 | } 49 | map->offset = pos; 50 | } 51 | 52 | 53 | void 54 | clear_scans( MAP2 * map ) 55 | { 56 | int x, y; 57 | for (x=0;xmapsize.x;x++) { 58 | for (y=0;ymapsize.y;y++) { 59 | map->calc[x][y] = 0.0; 60 | } 61 | } 62 | } 63 | 64 | double 65 | rpos2_length( logtools_rpos2_t pos1, logtools_rpos2_t pos2 ) 66 | { 67 | return( sqrt( ( (pos1.x-pos2.x) * (pos1.x-pos2.x) ) + 68 | ( (pos1.y-pos2.y) * (pos1.y-pos2.y) ) ) ); 69 | } 70 | 71 | int 72 | main( int argc, char** argv) 73 | { 74 | 75 | QApplication app( argc, argv ); 76 | MapPainter win; 77 | 78 | logtools_log_data_t rec; 79 | MAP2 map, localmap; 80 | 81 | char rec_filename[MAX_STRING_LENGTH]; 82 | char ini_filename[MAX_STRING_LENGTH]; 83 | 84 | logtools_ivector2_t rmappos; 85 | logtools_vector2_t min, max, abspt; 86 | logtools_bounding_box_t bbox; 87 | logtools_rmove2_t particlemove, odomove, move; 88 | logtools_rmove2_t lmove, nomove = {0.0, 0.0, 0.0}; 89 | logtools_rpos2_t lastpartpos, lastpos, pos, newpos; 90 | logtools_rpos2_t addpos, ucpos, nullpos = {0.0, 0.0, 0.0}; 91 | 92 | double length = 0.0; 93 | 94 | int h, hist, i, j, p; 95 | int idxctr = 0, ctr = 0, * idx; 96 | int read_ini = FALSE; 97 | 98 | int bestvalidx = 0; 99 | int check = FALSE; 100 | 101 | SAMPLE_SET pset, tset; 102 | double samplesum; 103 | 104 | /* 105 | double sizex = 6000.0, sizey = 3000.0; 106 | double startx = 3000.0, starty = 1300.0; 107 | int resolution = 5; 108 | 109 | double sizex = 2000.0, sizey = 2000.0; 110 | double startx = 1000.0, starty = 1700.0; 111 | */ 112 | 113 | 114 | double v; 115 | logtools_gauss_kernel_t kernel = logtools_compute_gauss_kernel( 9 ); 116 | logtools_gauss_kernel_t nokernel = logtools_compute_gauss_kernel( 1 ); 117 | 118 | double lsizex, lsizey, lstartx, lstarty; 119 | int laser_id = 0; 120 | FILE * iop; 121 | 122 | if (argc<2) { 123 | print_usage(); 124 | exit(1); 125 | } 126 | 127 | for (i=1; ii+1)) { 129 | strncpy( ini_filename, argv[++i], MAX_STRING_LENGTH ); 130 | read_ini = TRUE; 131 | } else if (!strcmp(argv[i],"-id") && (argc>i+1)) { 132 | laser_id = atoi( argv[++i] ); 133 | } else { 134 | print_usage(); 135 | exit(1); 136 | } 137 | } 138 | 139 | fprintf( stderr, "# INFO: laser id = %d\n", settings.laser_id ); 140 | strncpy( rec_filename, argv[argc-1], MAX_STRING_LENGTH ); 141 | 142 | set_default(); 143 | if (laser_id!=0) { 144 | settings.laser_id = laser_id; 145 | } 146 | if (read_ini) 147 | read_ini_file( ini_filename ); 148 | 149 | lsizex = 50.0 + 1.0 * settings.max_range_length; 150 | lsizey = 50.0 + 2.0 * settings.max_range_length; 151 | lstartx = lsizex / 2.0; 152 | lstarty = 50.0; 153 | 154 | logtools_read_logfile( &rec, rec_filename ); 155 | 156 | pset.numparticles = settings.num_samples; 157 | pset.particle = (PARTICLE *) malloc( pset.numparticles * sizeof(PARTICLE) ); 158 | 159 | tset.numparticles = settings.num_samples; 160 | tset.particle = (PARTICLE *) malloc( tset.numparticles * sizeof(PARTICLE) ); 161 | 162 | for (i=0; i settings.max_range_length ) { 184 | abspt=logtools_compute_laser_points( rec.lsens[i].estpos, 185 | settings.max_range_length, 186 | nomove, rec.lsens[i].laser.angle[j] ); 187 | } else { 188 | abspt=logtools_compute_laser_points( rec.lsens[i].estpos, 189 | rec.lsens[i].laser.val[j], 190 | nomove, rec.lsens[i].laser.angle[j] ); 191 | } 192 | if (abspt.x < min.x) { 193 | min.x = abspt.x; 194 | } 195 | if (abspt.x > max.x) { 196 | max.x = abspt.x; 197 | } 198 | if (abspt.y < min.y) { 199 | min.y = abspt.y; 200 | } 201 | if (abspt.y > max.y) { 202 | max.y = abspt.y; 203 | } 204 | } 205 | // update_map( &map, rec.lsens[i].laser.numvalues, rec.lsens[i].laser.val, 206 | // rec.lsens[i].laser.angle, rec.lsens[i].estpos, 207 | // settings.max_range_length, settings.max_usable_length ); 208 | } 209 | 210 | if (settings.detect_size) { 211 | 212 | settings.size_x = max.x - min.x + 2.0 * settings.detect_size_border; 213 | settings.size_y = max.y - min.y + 2.0 * settings.detect_size_border; 214 | 215 | settings.start_x = -(min.x - settings.detect_size_border); 216 | settings.start_y = -(min.y - settings.detect_size_border); 217 | 218 | fprintf(stderr,"***************************************\nEstimating map size:\n"); 219 | fprintf(stderr," start_x: %.1f\n", settings.start_x); 220 | fprintf(stderr," start_y: %.1f\n", settings.start_y); 221 | fprintf(stderr," size_x : %.1f\n", settings.size_x); 222 | fprintf(stderr," size_y : %.1f\n", settings.size_y); 223 | fprintf(stderr,"***************************************\n"); 224 | } 225 | 226 | map_initialize( &map, 227 | (int) (settings.size_x/settings.resolution), 228 | (int) (settings.size_y/settings.resolution), 229 | (int) (settings.start_x/settings.resolution), 230 | (int) (settings.start_y/settings.resolution), 231 | settings.resolution, nullpos ); 232 | 233 | map_initialize( &localmap, 234 | (int) (lsizex/settings.resolution), 235 | (int) (lsizey/settings.resolution), 236 | (int) (0), 237 | (int) (lsizey/(2.0*settings.resolution)), 238 | settings.resolution, nullpos ); 239 | 240 | if (settings.show_graphics) { 241 | 242 | win.setMinimumSize( 640, 480 ); 243 | 244 | int window_size_x = 100 + (int) (settings.size_x/settings.resolution); 245 | if (window_size_x > 800) 246 | window_size_x = 800; 247 | 248 | int window_size_y = 100 + (int) (settings.size_y/settings.resolution); 249 | if (window_size_y > 600) 250 | window_size_y = 600; 251 | 252 | win.setSize( window_size_x, window_size_y) ; 253 | 254 | win.show(); 255 | } 256 | 257 | 258 | signal(SIGINT,shutdown); 259 | 260 | 261 | /* init all positions (including the particles) */ 262 | lastpos = lastpartpos = rec.lsens[0].estpos; 263 | for (p=0; p%d %d\n", rmappos.x, rmappos.y ); 272 | 273 | for (i=0; i0 && psettings.min_step_distance) { 289 | 290 | length = 0.0; 291 | move = logtools_movement2_between_rpos2( lastpartpos, 292 | rec.lsens[i].estpos ); 293 | 294 | for (p=0; p0) 343 | pset.particle[p].outside = FALSE; 344 | else 345 | pset.particle[p].outside = TRUE; 346 | check = TRUE; 347 | } 348 | compute_probs_of_map( &localmap ); 349 | if (settings.kernel_size==3) 350 | simple_convolve_map2( &localmap ); 351 | else 352 | simple_convolve_map( &localmap, kernel ); 353 | 354 | v = compute_scan_probability( &localmap, nullpos, 355 | rec.lsens[i], 356 | settings.max_range_length, 357 | settings.max_usable_length ); 358 | pset.particle[p].val = v; 359 | if (settings.show_graphics && settings.show_local_map && p==bestvalidx) 360 | win.update( localmap ); 361 | } 362 | //bestvalidx = find_best_particle_value( pset ); 363 | bestvalidx = find_best_particle_logsum( pset ); 364 | fprintf( stderr, "best idx = %d value %f -> scan %d\n", bestvalidx, pset.particle[bestvalidx].val, i ); 365 | lastpartpos = rec.lsens[i].estpos; 366 | 367 | if (settings.show_graphics) { 368 | map_clear( &map ); 369 | for (h=idxctr-1;h>=0;h--) { 370 | j = idx[h]; 371 | update_map( &map, 372 | rec.lsens[j].laser.numvalues, 373 | rec.lsens[j].laser.val, 374 | rec.lsens[j].laser.angle, 375 | pset.particle[bestvalidx].hist[j].pos, 376 | settings.max_range_length, settings.max_usable_length ); 377 | } 378 | compute_probs_of_map( &map ); 379 | simple_convolve_map( &map, nokernel ); 380 | win.update( map ); 381 | } 382 | 383 | /* normalization of the weights */ 384 | samplesum = 0; 385 | for (p=0; p xxx */ 410 | idx[idxctr] = i; 411 | idxctr++; 412 | } else { 413 | for (p=0; p=0;h--) { 461 | j = idx[h]; 462 | update_map( &map, 463 | rec.lsens[j].laser.numvalues, 464 | rec.lsens[j].laser.val, 465 | rec.lsens[j].laser.angle, 466 | pset.particle[bestvalidx].hist[j].pos, 467 | settings.max_range_length, settings.max_usable_length ); 468 | } 469 | compute_probs_of_map( &map ); 470 | simple_convolve_map( &map, nokernel ); 471 | win.update( map ); 472 | 473 | while (1) { 474 | app.processEvents(); 475 | usleep(500); 476 | } 477 | } 478 | 479 | 480 | exit(0); 481 | } 482 | 483 | -------------------------------------------------------------------------------- /src/mapper/gui.ui: -------------------------------------------------------------------------------- 1 | 2 | MapGUI 3 | 4 | 5 | MapGUI 6 | 7 | 8 | 9 | 0 10 | 0 11 | 557 12 | 479 13 | 14 | 15 | 16 | 17 | 7 18 | 7 19 | 0 20 | 0 21 | 22 | 23 | 24 | 25 | 540 26 | 449 27 | 28 | 29 | 30 | Map 2D 31 | 32 | 33 | 34 | unnamed 35 | 36 | 37 | 0 38 | 39 | 40 | 0 41 | 42 | 43 | 44 | Layout5 45 | 46 | 47 | 48 | unnamed 49 | 50 | 51 | 0 52 | 53 | 54 | 6 55 | 56 | 57 | 58 | Map 59 | 60 | 61 | 62 | 7 63 | 7 64 | 1 65 | 1 66 | 67 | 68 | 69 | 70 | 400 71 | 400 72 | 73 | 74 | 75 | 76 | 77 | Actions 78 | 79 | 80 | 81 | 7 82 | 7 83 | 0 84 | 0 85 | 86 | 87 | 88 | 89 | 130 90 | 400 91 | 92 | 93 | 94 | 95 | 130 96 | 32767 97 | 98 | 99 | 100 | 1 101 | 102 | 103 | 0 104 | 105 | 106 | 0 107 | 108 | 109 | Actions 110 | 111 | 112 | 113 | Layout3 114 | 115 | 116 | 117 | 10 118 | 21 119 | 109 120 | 288 121 | 122 | 123 | 124 | 125 | unnamed 126 | 127 | 128 | 0 129 | 130 | 131 | 6 132 | 133 | 134 | 135 | UpdateMap 136 | 137 | 138 | Update Map 139 | 140 | 141 | 142 | 143 | CenterRobot 144 | 145 | 146 | Center Robot 147 | 148 | 149 | 150 | 151 | Frame3_2 152 | 153 | 154 | 155 | 7 156 | 0 157 | 0 158 | 0 159 | 160 | 161 | 162 | 163 | 0 164 | 20 165 | 166 | 167 | 168 | StyledPanel 169 | 170 | 171 | Raised 172 | 173 | 174 | 0 175 | 176 | 177 | 178 | 179 | GlobalMapType 180 | 181 | 182 | Global Map 183 | 184 | 185 | 186 | 187 | LocalMapType 188 | 189 | 190 | Local Map 191 | 192 | 193 | 194 | 195 | ShowBeamsType 196 | 197 | 198 | Show Beams 199 | 200 | 201 | 202 | 203 | Frame3_2_2 204 | 205 | 206 | 207 | 7 208 | 0 209 | 0 210 | 0 211 | 212 | 213 | 214 | 215 | 0 216 | 20 217 | 218 | 219 | 220 | StyledPanel 221 | 222 | 223 | Raised 224 | 225 | 226 | 0 227 | 228 | 229 | 230 | 231 | ClearMap 232 | 233 | 234 | Clear Map 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | menubar 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | MapView 266 |
mapview.h
267 | 268 | -1 269 | -1 270 | 271 | 1 272 | 273 | 7 274 | 7 275 | 0 276 | 0 277 | 278 | image0 279 | slot() 280 | 281 | 282 | 283 | 284 | 285 | fileNewAction 286 | 287 | 288 | 289 | 290 | 291 | New 292 | 293 | 294 | &New 295 | 296 | 297 | Ctrl+N 298 | 299 | 300 | 301 | 302 | fileOpenAction 303 | 304 | 305 | 306 | 307 | 308 | Open 309 | 310 | 311 | &Open... 312 | 313 | 314 | Ctrl+O 315 | 316 | 317 | 318 | 319 | fileSaveAction 320 | 321 | 322 | 323 | 324 | 325 | Save 326 | 327 | 328 | &Save 329 | 330 | 331 | Ctrl+S 332 | 333 | 334 | 335 | 336 | fileSaveAsAction 337 | 338 | 339 | Save As 340 | 341 | 342 | Save &As... 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | filePrintAction 351 | 352 | 353 | 354 | 355 | 356 | Print 357 | 358 | 359 | &Print... 360 | 361 | 362 | Ctrl+P 363 | 364 | 365 | 366 | 367 | fileExitAction 368 | 369 | 370 | Exit 371 | 372 | 373 | E&xit 374 | 375 | 376 | Exit2 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | ChangeMapAction 385 | 386 | 387 | true 388 | 389 | 390 | true 391 | 392 | 393 | 394 | 395 | 396 | Open 397 | 398 | 399 | Change Map 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | ViewPathAction 408 | 409 | 410 | true 411 | 412 | 413 | 414 | 415 | 416 | View Path 417 | 418 | 419 | View Path 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 789c534e494dcbcc4b554829cdcdad8c2fcf4c29c95030e0524611cd48cd4ccf28010a1797249664262b2467241641a592324b8aa363156c15aab914146aadb90067111b1f 429 | 430 | 431 | 432 | 433 | fileNewAction 434 | activated() 435 | MapGUI 436 | MenuNewSlot() 437 | 438 | 439 | fileOpenAction 440 | activated() 441 | MapGUI 442 | MenuOpenSlot() 443 | 444 | 445 | filePrintAction 446 | activated() 447 | MapGUI 448 | MenuPrintSlot() 449 | 450 | 451 | fileExitAction 452 | activated() 453 | MapGUI 454 | MenuExitSlot() 455 | 456 | 457 | UpdateMap 458 | clicked() 459 | MapGUI 460 | UpdateMapSlot() 461 | 462 | 463 | CenterRobot 464 | clicked() 465 | MapGUI 466 | CenterRobotSlot() 467 | 468 | 469 | GlobalMapType 470 | clicked() 471 | MapGUI 472 | GlobalMapTypeSlot() 473 | 474 | 475 | LocalMapType 476 | clicked() 477 | MapGUI 478 | LocalMapTypeSlot() 479 | 480 | 481 | ShowBeamsType 482 | clicked() 483 | MapGUI 484 | ShowBeamsTypeSlot() 485 | 486 | 487 | ClearMap 488 | clicked() 489 | MapGUI 490 | ClearMapSlot() 491 | 492 | 493 | ViewPathAction 494 | activated() 495 | MapGUI 496 | ViewPathSlot() 497 | 498 | 499 | ChangeMapAction 500 | activated() 501 | MapGUI 502 | ChangeMapSlot() 503 | 504 | 505 | 506 | UpdateMap 507 | CenterRobot 508 | GlobalMapType 509 | LocalMapType 510 | ShowBeamsType 511 | ClearMap 512 | 513 | 514 | carmen/logtools.h 515 | gui.ui.h 516 | 517 | 518 | UpdateMapSlot() 519 | CenterRobotSlot() 520 | ClearMapSlot() 521 | MenuNewSlot() 522 | MenuOpenSlot() 523 | MenuPrintSlot() 524 | MenuExitSlot() 525 | GlobalMapTypeSlot() 526 | LocalMapTypeSlot() 527 | ShowBeamsTypeSlot() 528 | ViewPathSlot() 529 | ChangeMapSlot() 530 | ClearMapSlot( bool ) 531 | 532 | 533 | 534 | 535 | mapview.h 536 | 537 | 538 | -------------------------------------------------------------------------------- /src/mapper/map.c: -------------------------------------------------------------------------------- 1 | #include "map2d.h" 2 | 3 | double 4 | get_mapval( int pos_x, int pos_y, MAP2 map ) 5 | { 6 | #ifdef SLOW 7 | fprintf( settings.devNULL, "%d %d -> %d %d\n", pos_x, pos_y, 8 | map.mapsize.x, 9 | map.mapsize.y ); 10 | #endif 11 | if ( pos_x>=0 && pos_x=0 && pos_y %d %d\n", pos_x, pos_y, 25 | map.mapsize.x, 26 | map.mapsize.y ); 27 | #endif 28 | if ( pos_x>=0 && pos_x=0 && pos_y %d %d\n", pos_x, pos_y, 42 | map.mapsize.x, 43 | map.mapsize.y ); 44 | #endif 45 | if ( pos_x>=0 && pos_x=0 && pos_ya?b:a ); 58 | } 59 | 60 | int 61 | find_quadrant( logtools_svector2_t center, int x, int y ) 62 | { 63 | if (x>(center.x)) { 64 | if (y>(center.y)) { 65 | return(0); 66 | } else { 67 | return(3); 68 | } 69 | } else { 70 | if (y>(center.y)) { 71 | return(1); 72 | } else { 73 | return(2); 74 | } 75 | } 76 | } 77 | 78 | logtools_svector2_t 79 | newcenter( logtools_svector2_t center, int i, short stepsize ) 80 | { 81 | logtools_svector2_t ncenter = center; 82 | switch(i) { 83 | case 0: 84 | ncenter.x += stepsize; 85 | ncenter.y += stepsize; 86 | break; 87 | case 1: 88 | ncenter.x -= stepsize; 89 | ncenter.y += stepsize; 90 | break; 91 | case 2: 92 | ncenter.x -= stepsize; 93 | ncenter.y -= stepsize; 94 | break; 95 | case 3: 96 | ncenter.x += stepsize; 97 | ncenter.y -= stepsize; 98 | break; 99 | } 100 | return(ncenter); 101 | } 102 | 103 | void 104 | alloc_tree( QUAD_TREE * tree, int level, logtools_svector2_t center, short stepsize ) 105 | { 106 | int i; 107 | short nstepsize = stepsize/2; 108 | tree->center = center; 109 | tree->level = level; 110 | tree->inuse = FALSE; 111 | if (level>0) { 112 | for( i=0; i<4; i++) { 113 | tree->elem[i] = (QUAD_TREE *) malloc( sizeof(QUAD_TREE) ); 114 | alloc_tree( tree->elem[i], level-1, 115 | newcenter( center, i, nstepsize ), nstepsize ); 116 | } 117 | } 118 | } 119 | 120 | void 121 | initialize_qtree( QUAD_TREE * tree, int size_x, int size_y) 122 | { 123 | int i,v,nlevel = max( (int) ceil(log10(size_x)/log10(2)), 124 | (int) ceil(log10(size_y)/log10(2)) ); 125 | logtools_svector2_t center; 126 | fprintf( stderr, "* INFO: num levels = %d\n", nlevel ); 127 | v = 1; 128 | for (i=0;imapsize.x = sx; 151 | map->mapsize.y = sy; 152 | map->resolution = resolution; 153 | map->offset = start; 154 | 155 | fprintf( stderr, "* INFO: allocating memory ... " ); 156 | 157 | if (1) { 158 | map->updated = 159 | (unsigned char **) mdalloc( 2, sizeof(unsigned char), sx, sy ); 160 | map->maphit = (float **) mdalloc( 2, sizeof(float), sx, sy ); 161 | map->mapsum = (short **) mdalloc( 2, sizeof(short), sx, sy ); 162 | map->mapprob = (float **) mdalloc( 2, sizeof(float), sx, sy ); 163 | map->calc = (float **) mdalloc( 2, sizeof(float), sx, sy ); 164 | } else { 165 | map->updated = 166 | (unsigned char **) malloc( sx * sizeof(unsigned char *) ); 167 | map->maphit = 168 | (float **) malloc( sx * sizeof(float *) ); 169 | map->mapsum = 170 | (short **) malloc( sx * sizeof(short *) ); 171 | map->mapprob = 172 | (float **) malloc( sx * sizeof(float *) ); 173 | map->calc = 174 | (float **) malloc( sx * sizeof(float *) ); 175 | for (j=0; jupdated[j] = 177 | (unsigned char *) malloc( sy * sizeof(unsigned char) ); 178 | map->maphit[j] = 179 | (float *) malloc( sy * sizeof(float) ); 180 | map->mapsum[j] = 181 | (short *) malloc( sy * sizeof(short) ); 182 | map->mapprob[j] = 183 | (float *) malloc( sy * sizeof(float) ); 184 | map->calc[j] = 185 | (float *) malloc( sy * sizeof(float) ); 186 | } 187 | } 188 | fprintf( stderr, "done\n" ); 189 | map->center.x = center_x; 190 | map->center.y = center_y; 191 | 192 | fprintf( stderr, "* INFO: map: %d %d\n", 193 | map->mapsize.x, map->mapsize.y ); 194 | fprintf( stderr, "* INFO: center: %.1f %.1f\n", 195 | map->center.x, map->center.y ); 196 | fprintf( stderr, "* INFO: resolution: %.2f\n", 197 | map->resolution ); 198 | fprintf( stderr, "* INFO: real-size: [%.1f %.1f] [%.1f %.1f]\n", 199 | -sx*map->resolution, sx*map->resolution, 200 | -sy*map->resolution, sy*map->resolution ); 201 | fprintf( stderr, "***************************************\n" ); 202 | fprintf( stderr, "* INFO: clearing map ... " ); 203 | 204 | for (x=0;xmapprob[x][y] = settings.local_map_std_val; 207 | map->calc[x][y] = settings.local_map_std_val; 208 | map->maphit[x][y] = 0.0; 209 | map->mapsum[x][y] = 0; 210 | map->updated[x][y] = UPDT_NOT; 211 | } 212 | } 213 | initialize_qtree( &(map->qtree), sx, sy ); 214 | fprintf( stderr, "***************************************\n" ); 215 | } 216 | 217 | void 218 | initialize_ray_map( MAP2 *map, int sx, int sy, 219 | double resolution, logtools_rpos2_t start ) 220 | { 221 | int x, y; 222 | 223 | map->mapsize.x = sx; 224 | map->mapsize.y = sy; 225 | map->resolution = resolution; 226 | map->offset = start; 227 | 228 | fprintf( stderr, "allocating memory ..." ); 229 | map->maphit = (float **) mdalloc( 2, sizeof(float), sx, sy ); 230 | map->mapsum = (short **) mdalloc( 2, sizeof(short), sx, sy ); 231 | fprintf( stderr, "done\n" ); 232 | map->center.x = 10; /* map->mapsize.x / 2.0; */ 233 | map->center.y = map->mapsize.y / 2.0; 234 | 235 | fprintf( stderr, "map: %5d %5d\n", map->mapsize.x, map->mapsize.y ); 236 | fprintf( stderr, "center: %5.1f %5.1f\n", map->center.x, map->center.y ); 237 | fprintf( stderr, "resolutio: %5.2f\n", map->resolution ); 238 | fprintf( stderr, "real-size: [%.2f %.2f] [%.2f %.2f]\n", 239 | -sx*map->resolution, sx*map->resolution, 240 | -sy*map->resolution, sy*map->resolution ); 241 | 242 | for (x=0;xmaphit[x][y] = 0.0; 245 | map->mapsum[x][y] = 0; 246 | } 247 | } 248 | } 249 | 250 | void 251 | simple_compute_map( MAP2 *map ) 252 | { 253 | int x, y; 254 | double odds, logodds; 255 | double dynamic_prob = (0.51); 256 | double static_prob = (0.49); 257 | 258 | dynamic_prob = log(dynamic_prob/(1.0-dynamic_prob)); 259 | static_prob = log(static_prob/(1.0-static_prob)); 260 | for (x=0;xmapsize.x;x++) { 261 | for (y=0;ymapsize.y;y++) { 262 | if (map->mapsum[x][y]>0) { 263 | logodds = 264 | dynamic_prob * map->mapsum[x][y] + 265 | static_prob * map->maphit[x][y]; 266 | odds = exp(logodds); 267 | map->mapprob[x][y] = (odds / (1+odds)); 268 | } else { 269 | map->mapprob[x][y] = 0.0; 270 | } 271 | } 272 | } 273 | } 274 | 275 | void 276 | simple_convolve_map( MAP2 *map, logtools_gauss_kernel_t kernel ) 277 | { 278 | int x, y, k, hk; 279 | double ksum; 280 | 281 | hk = ( kernel.len - 1 ) / 2; 282 | for (x=hk;xmapsize.x-hk;x++) { 283 | for (y=0;ymapsize.y;y++) { 284 | ksum = 0.0; 285 | for (k=0;kmapprob[x+k-hk][y] ); 287 | } 288 | map->calc[x][y] = ksum; 289 | if (map->calc[x][y]>1.0) 290 | map->calc[x][y]=1.0; 291 | } 292 | } 293 | for (x=0;xmapsize.x;x++) { 294 | for (y=hk;ymapsize.y-hk;y++) { 295 | ksum = 0.0; 296 | for (k=0;kcalc[x][y+k-hk] ); 298 | } 299 | map->mapprob[x][y] = ksum; 300 | if (map->mapprob[x][y]>1.0) 301 | map->mapprob[x][y]=1.0; 302 | } 303 | } 304 | } 305 | 306 | 307 | void 308 | simple_inverse_convolve_map( MAP2 *map, logtools_gauss_kernel_t kernel ) 309 | { 310 | int x, y, k, hk; 311 | double ksum; 312 | 313 | hk = ( kernel.len - 1 ) / 2; 314 | for (x=hk;xmapsize.x-hk;x++) { 315 | for (y=0;ymapsize.y;y++) { 316 | ksum = 0.0; 317 | for (k=0;kmapprob[x+k-hk][y]) ); 319 | } 320 | map->calc[x][y] = ksum; 321 | if (map->calc[x][y]>1.0) 322 | map->calc[x][y]=1.0; 323 | } 324 | } 325 | for (x=0;xmapsize.x;x++) { 326 | for (y=hk;ymapsize.y-hk;y++) { 327 | ksum = 0.0; 328 | for (k=0;kcalc[x][y+k-hk] ); 330 | } 331 | if (map->mapprob[x][y]>1.0) 332 | map->mapprob[x][y]=1.0; 333 | map->mapprob[x][y] = 1.0-ksum; 334 | } 335 | } 336 | } 337 | 338 | 339 | void 340 | simple_clear_map( MAP2 *map ) 341 | { 342 | int x, y; 343 | for (x=0;xmapsize.x;x++) { 344 | for (y=0;ymapsize.y;y++) { 345 | map->mapprob[x][y] = 0.0; 346 | map->maphit[x][y] = 0.0; 347 | map->mapsum[x][y] = 0; 348 | } 349 | } 350 | } 351 | 352 | void 353 | compute_prob_point( MAP2 *map, int x, int y ) 354 | { 355 | 356 | double pobj; 357 | 358 | pobj = log(settings.local_map_object_prob/ 359 | (1.0-settings.local_map_object_prob)); 360 | 361 | if ( x>=0 && xmapsize.x && 362 | y>=0 && ymapsize.y ) { 363 | if (get_mapsum(x,y,*map)>0) { 364 | if (0) { 365 | map->mapprob[x][y] = map->maphit[x][y] / map->mapsum[x][y]; 366 | } else { 367 | map->mapprob[x][y] = 1.0; 368 | } 369 | } else { 370 | map->mapprob[x][y] = settings.local_map_std_val; 371 | } 372 | } 373 | 374 | } 375 | 376 | void 377 | convolve_calc_point( MAP2 *map, logtools_gauss_kernel_t kernel, int hk, 378 | int x, int y, double std_val ) 379 | { 380 | int k; 381 | double ksum; 382 | 383 | ksum = 0.0; 384 | 385 | if (x-hk>=0 && x+hkmapsize.x) { 386 | for (k=0;k<2*hk+1;k++) { 387 | if (get_mapsum(x+k-hk,y,*map)>0) { 388 | ksum += ( kernel.val[k] * map->mapprob[x+k-hk][y] ); 389 | } else { 390 | ksum += ( kernel.val[k] * std_val ); 391 | } 392 | } 393 | map->calc[x][y] = ksum; 394 | map->updated[x][y] = UPDT_X; 395 | } 396 | } 397 | 398 | void 399 | convolve_prob_point( MAP2 *map, logtools_gauss_kernel_t kernel, 400 | int hk, int x, int y ) 401 | { 402 | int k; 403 | double ksum; 404 | 405 | ksum = 0.0; 406 | 407 | for (k=0;k<2*hk+1;k++) { 408 | if ( x>=0 && xmapsize.x && 409 | y>=0 && ymapsize.y) 410 | ksum += ( kernel.val[k] * map->calc[x][y+k-hk] ); 411 | } 412 | map->mapprob[x][y] = ksum; 413 | map->updated[x][y] = UPDT_Y; 414 | 415 | } 416 | 417 | void 418 | compute_prob_treemap( QUAD_TREE *tree, MAP2 *map ) 419 | { 420 | if ((tree->level)>0 ) { 421 | if (tree->elem[0]->inuse) 422 | compute_prob_treemap( tree->elem[0], map ); 423 | if (tree->elem[1]->inuse) 424 | compute_prob_treemap( tree->elem[1], map ); 425 | if (tree->elem[2]->inuse) 426 | compute_prob_treemap( tree->elem[2], map ); 427 | if (tree->elem[3]->inuse) 428 | compute_prob_treemap( tree->elem[3], map ); 429 | } else { 430 | compute_prob_point( map, (tree->center.x/2), (tree->center.y/2) ); 431 | } 432 | } 433 | 434 | /* 435 | void 436 | compute_ray_prob_treemap( QUAD_TREE *tree, MAP2 *map ) 437 | { 438 | if ((tree->level)>0 ) { 439 | if (tree->elem[0]->inuse) 440 | compute_ray_prob_treemap( tree->elem[0], map ); 441 | if (tree->elem[1]->inuse) 442 | compute_ray_prob_treemap( tree->elem[1], map ); 443 | if (tree->elem[2]->inuse) 444 | compute_ray_prob_treemap( tree->elem[2], map ); 445 | if (tree->elem[3]->inuse) 446 | compute_ray_prob_treemap( tree->elem[3], map ); 447 | } else { 448 | compute_ray_prob_point( map, (tree->center.x/2), (tree->center.y/2) ); 449 | } 450 | } 451 | */ 452 | 453 | void 454 | convolve_calc_treemap( QUAD_TREE *tree, MAP2 *map, 455 | logtools_gauss_kernel_t kernel, int hk, double std ) 456 | { 457 | int i, j; 458 | if ((tree->level)>0 ) { 459 | if (tree->elem[0]->inuse) 460 | convolve_calc_treemap( tree->elem[0], map, kernel, hk, std ); 461 | if (tree->elem[1]->inuse) 462 | convolve_calc_treemap( tree->elem[1], map, kernel, hk, std ); 463 | if (tree->elem[2]->inuse) 464 | convolve_calc_treemap( tree->elem[2], map, kernel, hk, std ); 465 | if (tree->elem[3]->inuse) 466 | convolve_calc_treemap( tree->elem[3], map, kernel, hk, std ); 467 | } else { 468 | if ( (tree->center.x/2)>hk+1 && (tree->center.x/2)mapsize.x-hk-1 && 469 | (tree->center.y/2)>hk+1 && (tree->center.y/2)mapsize.y-hk-1 ) { 470 | for (i=(tree->center.x/2)-hk;i<(tree->center.x/2)+hk;i++) { 471 | for (j=(tree->center.y/2)-hk;j<(tree->center.y/2)+hk;j++) { 472 | if (map->updated[i][j] != UPDT_X) 473 | convolve_calc_point( map, kernel, hk, i, j, std ); 474 | } 475 | } 476 | } 477 | 478 | } 479 | } 480 | 481 | void 482 | convolve_prob_treemap( QUAD_TREE *tree, MAP2 *map, 483 | logtools_gauss_kernel_t kernel, int hk ) 484 | { 485 | int i, j; 486 | if ((tree->level)>0 ) { 487 | if (tree->elem[0]->inuse) 488 | convolve_prob_treemap( tree->elem[0], map, kernel, hk ); 489 | if (tree->elem[1]->inuse) 490 | convolve_prob_treemap( tree->elem[1], map, kernel, hk ); 491 | if (tree->elem[2]->inuse) 492 | convolve_prob_treemap( tree->elem[2], map, kernel, hk ); 493 | if (tree->elem[3]->inuse) 494 | convolve_prob_treemap( tree->elem[3], map, kernel, hk ); 495 | } else { 496 | if ( (tree->center.x/2)>hk+1 && (tree->center.x/2)mapsize.x-hk-1 && 497 | (tree->center.y/2)>hk+1 && (tree->center.y/2)mapsize.y-hk-1 ) { 498 | for (i=(tree->center.x/2)-hk;i<(tree->center.x/2)+hk;i++) { 499 | for (j=(tree->center.y/2)-hk;j<(tree->center.y/2)+hk;j++) { 500 | if (map->updated[i][j] != UPDT_Y) 501 | convolve_prob_point( map, kernel, hk, i, j ); 502 | } 503 | } 504 | } 505 | } 506 | } 507 | 508 | void 509 | convolve_map( MAP2 *map ) 510 | { 511 | int i; 512 | static int hk, first_time = TRUE; 513 | static logtools_gauss_kernel_t kernel; 514 | 515 | if (first_time) { 516 | hk = (settings.local_map_kernel_len-1)/2; 517 | kernel = logtools_compute_gauss_kernel( settings.local_map_kernel_len ); 518 | first_time = FALSE; 519 | } 520 | 521 | compute_prob_treemap( &(map->qtree), map ); 522 | for (i=0;iqtree), map, kernel, hk, 524 | settings.local_map_std_val ); 525 | convolve_prob_treemap( &(map->qtree), map, kernel, hk ); 526 | } 527 | 528 | } 529 | 530 | int 531 | compute_map_pos_from_rpos( logtools_rpos2_t rpos, MAP2 *map, 532 | logtools_ivector2_t *v ) 533 | { 534 | v->x = (int) (map->center.x + (rpos.x/(double)map->resolution)); 535 | v->y = (int) (map->center.y + (rpos.y/(double)map->resolution)); 536 | if (v->x<0 || v->x>map->mapsize.x-1) { 537 | return(FALSE); 538 | } 539 | if (v->y<0 || v->y>map->mapsize.y-1) { 540 | return(FALSE); 541 | } 542 | return(TRUE); 543 | } 544 | 545 | int 546 | compute_map_pos_from_vec2( logtools_vector2_t pos, MAP2 *map, 547 | logtools_ivector2_t *v ) 548 | { 549 | v->x = (int) (map->center.x + (pos.x/(double)(map->resolution))); 550 | v->y = (int) (map->center.y + (pos.y/(double)(map->resolution))); 551 | if (v->x<0 || v->x>map->mapsize.x-1) { 552 | return(FALSE); 553 | } 554 | if (v->y<0 || v->y>map->mapsize.y-1) { 555 | return(FALSE); 556 | } 557 | return(TRUE); 558 | } 559 | 560 | void 561 | mark_maphitpoint( QUAD_TREE *tree, MAP2 *map, int x, int y, float value ) 562 | { 563 | tree->inuse=TRUE; 564 | if ((tree->level)>0) { 565 | mark_maphitpoint( tree->elem[find_quadrant( tree->center, x, y )], 566 | map, x, y, value ); 567 | } else { 568 | map->maphit[tree->center.x/2][tree->center.y/2] += value; 569 | } 570 | } 571 | 572 | void 573 | mark_mapsumpoint( QUAD_TREE *tree, MAP2 *map, int x, int y ) 574 | { 575 | tree->inuse=TRUE; 576 | if ((tree->level)>0) { 577 | mark_mapsumpoint( tree->elem[find_quadrant( tree->center, x, y )], 578 | map, x, y ); 579 | } else { 580 | map->mapsum[tree->center.x/2][tree->center.y/2]++; 581 | } 582 | } 583 | 584 | void 585 | set_maphitpoint( MAP2 *map, int x, int y, float value ) 586 | { 587 | mark_maphitpoint( &(map->qtree), map, 2*x, 2*y, value ); 588 | } 589 | 590 | void 591 | set_mapsumpoint( MAP2 *map, int x, int y ) 592 | { 593 | mark_mapsumpoint( &(map->qtree), map, 2*x, 2*y ); 594 | } 595 | 596 | void 597 | tree_list( QUAD_TREE *tree , int *ct ) 598 | { 599 | if ((tree->level)>0 ) { 600 | if (tree->elem[0]->inuse) 601 | tree_list( tree->elem[0], ct ); 602 | if (tree->elem[1]->inuse) 603 | tree_list( tree->elem[1], ct ); 604 | if (tree->elem[2]->inuse) 605 | tree_list( tree->elem[2], ct ); 606 | if (tree->elem[3]->inuse) 607 | tree_list( tree->elem[3], ct ); 608 | } else { 609 | // fprintf( stderr, "%d %d\n", tree->center.x, tree->center.y ); 610 | (*ct)++; 611 | } 612 | } 613 | 614 | void 615 | clear_local_treemap( QUAD_TREE *tree, MAP2 *map, int hk ) 616 | { 617 | int i,j; 618 | if ((tree->level)>0 ) { 619 | if (tree->elem[0]->inuse) 620 | clear_local_treemap( tree->elem[0], map, hk ); 621 | if (tree->elem[1]->inuse) 622 | clear_local_treemap( tree->elem[1], map, hk ); 623 | if (tree->elem[2]->inuse) 624 | clear_local_treemap( tree->elem[2], map, hk ); 625 | if (tree->elem[3]->inuse) 626 | clear_local_treemap( tree->elem[3], map, hk ); 627 | } else { 628 | if ( (tree->center.x/2)>hk-1 && (tree->center.x/2)mapsize.x-hk && 629 | (tree->center.y/2)>hk-1 && (tree->center.y/2)mapsize.y-hk ) { 630 | for (i=(tree->center.x/2)-hk;i<=(tree->center.x/2)+hk;i++) { 631 | for (j=(tree->center.y/2)-hk;j<=(tree->center.y/2)+hk;j++) { 632 | map->maphit[i][j] = 0; 633 | map->mapsum[i][j] = 0; 634 | map->mapprob[i][j] = settings.local_map_std_val; 635 | map->calc[i][j] = settings.local_map_std_val; 636 | map->updated[i][j] = UPDT_NOT; 637 | } 638 | } 639 | } 640 | } 641 | tree->inuse = FALSE; 642 | } 643 | 644 | void 645 | create_local_map( MAP2 *map, logtools_lasersens2_data_t data, 646 | logtools_rmove2_t movement ) 647 | { 648 | int i, max_num_linepoints; 649 | static logtools_rpos2_t npos = {0.0, 0.0, 0.0 }; 650 | static logtools_rmove2_t nmove = {0.0, 0.0, 0.0 }; 651 | logtools_rpos2_t rpos; 652 | logtools_ivector2_t start, end; 653 | logtools_vector2_t lpos; 654 | static int first_time = TRUE; 655 | static logtools_grid_line_t line; 656 | logtools_rmove2_t offset; 657 | 658 | if (first_time) { 659 | max_num_linepoints = (int) 660 | (2 * ( settings.local_map_max_range / settings.local_map_resolution )); 661 | line.grid = (logtools_ivector2_t *) malloc( max_num_linepoints * sizeof(logtools_ivector2_t) ); 662 | first_time = FALSE; 663 | } 664 | 665 | offset.forward = movement.forward; // + data.laser.offset[i].forward; 666 | offset.sideward = movement.sideward; // + data.laser.offset[i].sideward; 667 | offset.rotation = movement.rotation; // + data.laser.offset[i].rotation; 668 | 669 | rpos = logtools_rpos2_backwards_with_movement2( npos, offset ); 670 | 671 | compute_map_pos_from_rpos( rpos, map, &start ); 672 | 673 | for (i=0;i=0 && end.xmapsize.x && 682 | end.y>=0 && end.ymapsize.y ) { 683 | set_mapsumpoint( map, end.x, end.y ); 684 | set_maphitpoint( map, end.x, end.y, 1.0 ); 685 | } 686 | } 687 | } 688 | } 689 | 690 | void 691 | compute_probs_of_global_map( MAP2 * global_map ) 692 | { 693 | int x, y; 694 | 695 | for (x=0;xmapsize.x;x++) { 696 | for (y=0;ymapsize.y;y++) { 697 | if (global_map->mapsum[x][y]>0) { 698 | global_map->mapprob[x][y] = 699 | ( global_map->maphit[x][y] / 700 | (double) ( global_map->maphit[x][y] + global_map->mapsum[x][y] ) ); 701 | } else { 702 | global_map->mapprob[x][y] = 703 | settings.global_map_std_val; 704 | } 705 | } 706 | } 707 | 708 | } 709 | 710 | void 711 | map_change_settings( int set ) 712 | { 713 | fprintf( stderr, "xxx= %d\n", set ); 714 | settings.change_map = set; 715 | settings.view_path = FALSE; 716 | } 717 | -------------------------------------------------------------------------------- /src/fast-slam/utils.c: -------------------------------------------------------------------------------- 1 | #include "fast-slam.h" 2 | #include 3 | 4 | #define UNKNOWN 0 5 | #define IN 1 6 | #define OUT 2 7 | 8 | #define EPSILON 0.000000001 9 | #define MAP_STD_VAL 0.00000 10 | 11 | double round(double x); 12 | 13 | int 14 | find_best_particle_logsum( SAMPLE_SET pset ) 15 | { 16 | int i, idx = 0; 17 | double val = -MAXDOUBLE; 18 | for (i=0;ival) { 20 | idx = i; 21 | val = pset.particle[i].logsum; 22 | } 23 | } 24 | return(idx); 25 | } 26 | 27 | int 28 | find_best_particle_value( SAMPLE_SET pset ) 29 | { 30 | int i, idx = 0; 31 | double val = -MAXDOUBLE; 32 | for (i=0;ival) { 34 | idx = i; 35 | val = pset.particle[i].val; 36 | } 37 | } 38 | return(idx); 39 | } 40 | 41 | double 42 | prob_unknown_space( double length, int endpoint ) 43 | { 44 | int idx; 45 | double frac; 46 | /* probs computed with seattle (intel research lab) map 47 | every 10 cm from 0.1 to 9m */ 48 | static double table1[] = { 0.027407, 0.023016, 0.021626, 0.020518, 0.019673, 0.018884, 0.017912, 0.017986, 0.016391, 0.016052, 0.015289, 0.014647, 0.013758, 0.013561, 0.012637, 0.012228, 0.011669, 0.010961, 0.010693, 0.010087, 0.009296, 0.008914, 0.008086, 0.007877, 0.007612, 0.007329, 0.006913, 0.006440, 0.006158, 0.006033, 0.005551, 0.005274, 0.004999, 0.004867, 0.004521, 0.004226, 0.004029, 0.003899, 0.003651, 0.003372, 0.003321, 0.002983, 0.002919, 0.002815, 0.002763, 0.002553, 0.002391, 0.002405, 0.002267, 0.002037, 0.001937, 0.001808, 0.001618, 0.001627, 0.001474, 0.001443, 0.001334, 0.001302, 0.001196, 0.001141, 0.001215, 0.001155, 0.001154, 0.001046, 0.001003, 0.000949, 0.000965, 0.000931, 0.000856, 0.000814, 0.000811, 0.000727, 0.000710, 0.000743, 0.000717, 0.000646, 0.000616, 0.000658, 0.000645, 0.000644, 0.000651, 0.000553, 0.000553, 0.000485, 0.000561, 0.000605, 0.000557, 0.000503, 0.000467, 0.000521 }; 49 | static double table2[] = { 0.840712, 0.798510, 0.759555, 0.722885, 0.686197, 0.653546, 0.620038, 0.590059, 0.562270, 0.534048, 0.507807, 0.482703, 0.459018, 0.434243, 0.415033, 0.394681, 0.373622, 0.355107, 0.339661, 0.323134, 0.309463, 0.294318, 0.281096, 0.266992, 0.254206, 0.243801, 0.232858, 0.221439, 0.212163, 0.202476, 0.190680, 0.183310, 0.176945, 0.168610, 0.160928, 0.153825, 0.147223, 0.140670, 0.135154, 0.129761, 0.123861, 0.118102, 0.113574, 0.110250, 0.104038, 0.099963, 0.095617, 0.092629, 0.088913, 0.085092, 0.082401, 0.078720, 0.077465, 0.073431, 0.072046, 0.068684, 0.067358, 0.063863, 0.062165, 0.061448, 0.058601, 0.057354, 0.055785, 0.053340, 0.052561, 0.050669, 0.049378, 0.047809, 0.045767, 0.045169, 0.043498, 0.042803, 0.041253, 0.041163, 0.039276, 0.038413, 0.037851, 0.036292, 0.035119, 0.034019, 0.032988, 0.032626, 0.031228, 0.030708, 0.030152, 0.028964, 0.028233, 0.027682, 0.026667, 0.026046 }; 50 | if (length>900.0) { 51 | idx = 89; 52 | } else { 53 | idx = (int) (length / 10.0); 54 | if (idx<=0) { 55 | idx=0; 56 | } else if (idx>0) { 57 | frac = (length/10.0)-idx; 58 | if (endpoint) 59 | return( table1[idx-1]+frac*(table1[idx]-table1[idx-1]) ); 60 | else 61 | return( table2[idx-1]+frac*(table2[idx]-table2[idx-1]) ); 62 | } 63 | } 64 | if (endpoint) 65 | return(table1[idx]); 66 | else 67 | return(table2[idx]); 68 | } 69 | 70 | void 71 | copy_particle( PARTICLE src, PARTICLE *dest ) 72 | { 73 | int i; 74 | HISTORY * ptr = dest->hist; 75 | *dest = src; 76 | dest->hist = ptr; 77 | for (i=0; ihist[i] = src.hist[i]; 79 | } 80 | } 81 | 82 | void 83 | resample( SAMPLE_SET sample1, SAMPLE_SET * sample2 ) 84 | { 85 | double u, c, delta, rezi = 1 / (double) sample1.numparticles; 86 | int i, j; 87 | delta = ( rand()/(double) RAND_MAX ) / (double) (sample1.numparticles+1); 88 | 89 | c = sample1.particle[0].val; 90 | for (i=0,j=0; jc) { 93 | i++; 94 | c += sample1.particle[i].val; 95 | } 96 | copy_particle( sample1.particle[i], &(sample2->particle[j]) ); 97 | } 98 | } 99 | 100 | void 101 | simple_convolve_map2( MAP2 *map) 102 | /* for fixed convolve size of 3 */ 103 | { 104 | int x, y; 105 | int maxX = map->mapsize.x-1; 106 | int maxY = map->mapsize.y-1; 107 | float **prob = map->mapprob; 108 | float **calc = map->calc; 109 | 110 | for (x=1;xmapsize.y;y++) { 112 | calc[x][y] = 0.25 * (prob[x-1][y]+prob[x+1][y]) 113 | + 0.5 * prob[x][y]; 114 | } 115 | } 116 | 117 | for (x=0;xmapsize.x;x++) { 118 | float *tmp = calc[x]; 119 | for (y=1;ymapsize.x-hk;x++) { 135 | for (y=0;ymapsize.y;y++) { 136 | ksum = 0.0; 137 | for (k=0;kmapprob[x+k-hk][y] ); 139 | } 140 | map->calc[x][y] = ksum; 141 | if (map->calc[x][y]>1.0) 142 | map->calc[x][y]=1.0; 143 | } 144 | } 145 | for (x=0;xmapsize.x;x++) { 146 | for (y=hk;ymapsize.y-hk;y++) { 147 | ksum = 0.0; 148 | for (k=0;kcalc[x][y+k-hk] ); 150 | } 151 | map->mapprob[x][y] = ksum; 152 | if (map->mapprob[x][y]>1.0) 153 | map->mapprob[x][y]=1.0; 154 | } 155 | } 156 | } 157 | 158 | 159 | int 160 | map_pos_from_rpos( logtools_rpos2_t rpos, MAP2 *map, logtools_ivector2_t *v ) 161 | { 162 | v->x = map->center.x + (int) round((rpos.x-map->offset.x)/ 163 | (double)map->resolution); 164 | v->y = map->center.y + (int) round((rpos.y-map->offset.y)/ 165 | (double)map->resolution); 166 | if (v->x<0) { 167 | return(FALSE); 168 | } else if (v->x>map->mapsize.x-1) { 169 | return(FALSE); 170 | } 171 | if (v->y<0) { 172 | return(FALSE); 173 | } else if (v->y>map->mapsize.y-1) { 174 | return(FALSE); 175 | } 176 | return(TRUE); 177 | } 178 | 179 | int 180 | map_pos_from_vec2( logtools_vector2_t pos, MAP2 *map, logtools_ivector2_t *v ) 181 | { 182 | v->x = map->center.x + (int) ((pos.x-map->offset.x)/ 183 | (double)map->resolution); 184 | v->y = map->center.y + (int) ((pos.y-map->offset.y)/ 185 | (double)map->resolution); 186 | if (v->x<0) { 187 | return(FALSE); 188 | } else if (v->x>map->mapsize.x-1) { 189 | return(FALSE); 190 | } 191 | if (v->y<0) { 192 | return(FALSE); 193 | } else if (v->y>map->mapsize.y-1) { 194 | return(FALSE); 195 | } 196 | return(TRUE); 197 | } 198 | 199 | void 200 | update_map( MAP2 * map, int numvalues, float * val, float * angle, 201 | logtools_rpos2_t estpos, double max_range, double max_usable ) 202 | { 203 | static int first_time = TRUE; 204 | static logtools_grid_line_t line; 205 | static int max_num_linepoints = 0; 206 | int i, j, x, y; 207 | logtools_ivector2_t start, end; 208 | logtools_vector2_t abspt; 209 | logtools_rmove2_t nomove = {0.0, 0.0, 0.0}; 210 | 211 | if (first_time) { 212 | max_num_linepoints = 213 | 10 * ( max_range / map->resolution ); 214 | line.grid = (logtools_ivector2_t *) malloc( max_num_linepoints * sizeof(logtools_ivector2_t) ); 215 | first_time = FALSE; 216 | } 217 | 218 | for (j=0;j max_range ) { 221 | abspt = logtools_compute_laser_points( estpos, max_range, nomove, 222 | angle[j] ); 223 | map_pos_from_vec2( abspt, map, &end ); 224 | } else { 225 | abspt = logtools_compute_laser_points( estpos, val[j]+(map->resolution), 226 | nomove, angle[j] ); 227 | map_pos_from_vec2( abspt, map, &end ); 228 | } 229 | map_pos_from_rpos( estpos, map, &start ); 230 | grid_line( start, end, &line ); 231 | for (i=0;i=0 && xmapsize.x && 235 | y>=0 && ymapsize.y ) { 236 | if (val[j]<=max_range ) { 237 | if (i>=line.numgrids-2) { 238 | map->maphit[x][y]++; 239 | } 240 | map->mapsum[x][y]++; 241 | } else { 242 | if (imapsum[x][y]++; 244 | } 245 | } 246 | } 247 | } 248 | } 249 | } 250 | } 251 | 252 | void 253 | map_initialize( MAP2 *map, int sx, int sy, int center_x, 254 | int center_y, double resolution, logtools_rpos2_t start ) 255 | { 256 | int x, y; 257 | 258 | map->mapsize.x = sx; 259 | map->mapsize.y = sy; 260 | map->resolution = resolution; 261 | map->offset = start; 262 | 263 | fprintf( stderr, "* INFO: allocating memory ... " ); 264 | map->maphit = mdalloc( 2, sizeof(float), sx, sy ); 265 | map->mapsum = mdalloc( 2, sizeof(short), sx, sy ); 266 | map->mapprob = mdalloc( 2, sizeof(float), sx, sy ); 267 | map->calc = mdalloc( 2, sizeof(float), sx, sy ); 268 | fprintf( stderr, "done\n" ); 269 | map->center.x = center_x; 270 | map->center.y = center_y; 271 | 272 | fprintf( stderr, "* INFO: map: %d %d\n", 273 | map->mapsize.x, map->mapsize.y ); 274 | fprintf( stderr, "* INFO: center: %.1f %.1f\n", 275 | map->center.x, map->center.y ); 276 | fprintf( stderr, "* INFO: resolution: %.2f\n", 277 | map->resolution ); 278 | fprintf( stderr, "* INFO: real-size: [%.1f %.1f] [%.1f %.1f]\n", 279 | -sx*map->resolution, sx*map->resolution, 280 | -sy*map->resolution, sy*map->resolution ); 281 | fprintf( stderr, "***************************************\n" ); 282 | 283 | for (x=0;xmapprob[x][y] = 0.0; 286 | map->calc[x][y] = 0.0; 287 | map->maphit[x][y] = 0.0; 288 | map->mapsum[x][y] = 0; 289 | } 290 | } 291 | } 292 | 293 | void 294 | map_clear( MAP2 *map ) 295 | { 296 | int x, y; 297 | for (x=0;xmapsize.x;x++) { 298 | for (y=0;ymapsize.y;y++) { 299 | map->mapprob[x][y] = 0.0; 300 | map->calc[x][y] = 0.0; 301 | map->maphit[x][y] = 0.0; 302 | map->mapsum[x][y] = 0; 303 | } 304 | } 305 | } 306 | 307 | void 308 | compute_probs_of_map( MAP2 * map ) 309 | { 310 | int x, y; 311 | for (x=0;xmapsize.x;x++) { 312 | for (y=0;ymapsize.y;y++) { 313 | if (map->mapsum[x][y]>0) { 314 | map->mapprob[x][y] = 315 | 3.0 * (map->maphit[x][y] / (double) ( map->mapsum[x][y] )); 316 | if (map->mapprob[x][y]>1.0) 317 | map->mapprob[x][y] = 0.999; 318 | } else { 319 | map->mapprob[x][y] = EPSILON; 320 | } 321 | } 322 | } 323 | 324 | } 325 | 326 | #define ADDB 2 327 | 328 | void 329 | compute_calc_of_map( MAP2 * map ) 330 | { 331 | int x, y, vx, vy, abort; 332 | for (x=0;xmapsize.x;x++) { 333 | for (y=0;ymapsize.y;y++) { 334 | if (map->mapprob[x][y]>0.7) { 335 | map->calc[x][y] = 0.0; 336 | abort = FALSE; 337 | for (vx=x-ADDB;vx=0 && vxmapsize.x && 340 | vy>=0 && vymapsize.y ) { 341 | if (map->mapsum[vx][vy]==0) { 342 | map->calc[x][y] = 1.0; 343 | abort = TRUE; 344 | } 345 | } 346 | } 347 | } 348 | } else { 349 | map->calc[x][y] = 0.0; 350 | } 351 | } 352 | } 353 | 354 | } 355 | 356 | logtools_bounding_box_t 357 | compute_laser_bounding_box( logtools_rpos2_t pos, 358 | logtools_lasersens2_data_t lsens, 359 | double laser_max_range ) 360 | { 361 | int i; 362 | logtools_vector2_t abspt, min,max; 363 | logtools_rmove2_t nomove = {0.0, 0.0, 0.0}; 364 | logtools_bounding_box_t bbox; 365 | 366 | min.x = MAXDOUBLE; min.y = MAXDOUBLE; 367 | max.x = -MAXDOUBLE; max.y = -MAXDOUBLE; 368 | 369 | for (i=0;imax.x) max.x = abspt.x; 376 | if (abspt.y>max.y) max.y = abspt.y; 377 | } 378 | } 379 | bbox.min = min; 380 | bbox.max = max; 381 | return(bbox); 382 | } 383 | 384 | 385 | void 386 | mark_map_cell( logtools_ivector2_t pos, MAP2 * map ) 387 | { 388 | int x, y; 389 | for (x=pos.x-2;x=0 && xmapsize.x && 392 | y>=0 && ymapsize.y ) { 393 | map->calc[x][y] = -1.0; 394 | } 395 | } 396 | } 397 | } 398 | 399 | double 400 | get_map_val( logtools_ivector2_t pos, MAP2 map ) 401 | { 402 | double val; 403 | if ( pos.x>=0 && pos.x=0 && pos.y0) { 406 | val = (double) (map.mapprob[pos.x][pos.y]); 407 | } else { 408 | val = EPSILON; 409 | } 410 | } else { 411 | val = EPSILON; 412 | } 413 | if (val>=1.0) 414 | return(1.0-EPSILON); 415 | else 416 | return(val); 417 | } 418 | 419 | double 420 | get_map_val3( logtools_ivector2_t pos, MAP2 map ) 421 | { 422 | double val, value; 423 | if ( pos.x>=0 && pos.x=0 && pos.y0) { 426 | value = (double) (map.mapprob[pos.x][pos.y]); 427 | if (value>0.2) 428 | val = 0.99; 429 | else 430 | val = 0.70; 431 | } else { 432 | val = 0.80; 433 | } 434 | } else { 435 | val = 0.80; 436 | } 437 | return(val); 438 | } 439 | 440 | double 441 | get_map_val2( logtools_ivector2_t pos, MAP2 map ) 442 | { 443 | double val; 444 | if ( pos.x>=0 && pos.x=0 && pos.y0) { 447 | val = EPSILON + (double) (map.mapprob[pos.x][pos.y]); 448 | } else { 449 | val = 1.0-EPSILON; 450 | } 451 | } else { 452 | val = 1.0-EPSILON; 453 | } 454 | if (val>=1.0) 455 | return(1.0-EPSILON); 456 | else 457 | return(val); 458 | } 459 | 460 | double 461 | stretch_function( double x ) 462 | { 463 | return( (settings.max_likelihood-settings.min_likelihood) * x + 464 | settings.min_likelihood ); 465 | } 466 | 467 | double 468 | get_beam_prob( logtools_rpos2_t pos, double val, double angle, 469 | double max_range, MAP2 * map ) 470 | { 471 | logtools_ivector2_t end; 472 | logtools_vector2_t abspt; 473 | logtools_rmove2_t nomove = {0.0, 0.0, 0.0}; 474 | 475 | if (val>=max_range) { 476 | abspt = logtools_compute_laser_points( pos, max_range-20.0, nomove, angle ); 477 | map_pos_from_vec2( abspt, map, &end ); 478 | mark_map_cell( end, map ); 479 | if ( end.x>=0 && end.xmapsize.x && 480 | end.y>=0 && end.ymapsize.y ) { 481 | /* maxrange and in map and previous seen */ 482 | if (map->mapsum[end.x][end.y]>0) { 483 | return(stretch_function(1.0-map->mapprob[end.x][end.y])); 484 | } else { 485 | return(settings.unknown_likelihood); 486 | } 487 | } else { 488 | /* maxrange and point out of map */ 489 | return(settings.unknown_likelihood); 490 | } 491 | } else { 492 | /* no maxrange */ 493 | abspt = logtools_compute_laser_points( pos, val, nomove, angle ); 494 | map_pos_from_vec2( abspt, map, &end ); 495 | mark_map_cell( end, map ); 496 | if ( end.x>=0 && end.xmapsize.x && 497 | end.y>=0 && end.ymapsize.y ) { 498 | /* no maxrange and in map and previous seen */ 499 | if (map->mapsum[end.x][end.y]>0) { 500 | return(stretch_function(map->mapprob[end.x][end.y])); 501 | } else { 502 | return(settings.unknown_likelihood); 503 | } 504 | } else { 505 | /* maxrange and point out of map */ 506 | return(settings.unknown_likelihood); 507 | } 508 | } 509 | } 510 | 511 | double 512 | compute_scan_probability( MAP2 * map, logtools_rpos2_t pos, 513 | logtools_lasersens2_data_t lsens, 514 | double max_range, double max_usable ) 515 | { 516 | static int first_time = TRUE; 517 | static logtools_grid_line_t line; 518 | static int max_num_linepoints = 0; 519 | int i, j, unknown; 520 | logtools_ivector2_t start, end, uknw; 521 | logtools_vector2_t abspt, startv, endv; 522 | logtools_rmove2_t nomove = {0.0, 0.0, 0.0}; 523 | double v, val = 0.0, prob, multprob; 524 | 525 | if (first_time) { 526 | max_num_linepoints = 527 | 10 * ( max_range / map->resolution ); 528 | line.grid = (logtools_ivector2_t *) malloc( max_num_linepoints * sizeof(logtools_ivector2_t) ); 529 | first_time = FALSE; 530 | } 531 | 532 | prob = 1.0; 533 | for (j=0;j= max_range ) { 537 | abspt = logtools_compute_laser_points( pos, max_range, 538 | nomove, lsens.laser.angle[j] ); 539 | map_pos_from_vec2( abspt, map, &end ); 540 | } else { 541 | abspt = logtools_compute_laser_points( pos, lsens.laser.val[j], 542 | nomove, lsens.laser.angle[j] ); 543 | map_pos_from_vec2( abspt, map, &end ); 544 | } 545 | map_pos_from_rpos( pos, map, &start ); 546 | grid_line( start, end, &line ); 547 | multprob = 1.0; 548 | if (line.numgrids>0) { 549 | unknown = FALSE; 550 | for (i=0;i=0 && line.grid[i].xmapsize.x && 553 | line.grid[i].y>=0 && line.grid[i].ymapsize.y ) { 554 | 555 | if (map->mapsum[line.grid[i].x][line.grid[i].y]==0) { 556 | /* unknown */ 557 | if (!unknown) { 558 | uknw = line.grid[i]; 559 | unknown = TRUE; 560 | } 561 | if (i==line.numgrids-1) { 562 | startv.x = uknw.x * map->resolution; 563 | startv.y = uknw.y * map->resolution; 564 | endv.x = line.grid[i].x * map->resolution; 565 | endv.y = line.grid[i].y * map->resolution; 566 | /* logprob += 567 | log10(prob_unknown_space(vector2_distance(startv, endv),1));*/ 568 | multprob *= 569 | prob_unknown_space(logtools_vector2_distance(startv, endv),1); 570 | } 571 | } else { 572 | /* known */ 573 | if (unknown) { 574 | unknown = FALSE; 575 | startv.x = uknw.x * map->resolution; 576 | startv.y = uknw.y * map->resolution; 577 | endv.x = line.grid[i].x * map->resolution; 578 | endv.y = line.grid[i].y * map->resolution; 579 | /* logprob += 580 | log10(prob_unknown_space(vector2_distance(startv,endv),0)); */ 581 | multprob *= 582 | prob_unknown_space(logtools_vector2_distance(startv, endv),0); 583 | } 584 | if (i= max_range ) { 590 | multprob *= (1.0-get_map_val( line.grid[i], *map )); 591 | /* logprob += 592 | log10((1.0-get_map_val( line.grid[i], *map ))); */ 593 | } else { 594 | v = get_map_val2( line.grid[i], *map ); 595 | multprob *= v; 596 | /* logprob += log10(v); */ 597 | } 598 | } 599 | } 600 | } 601 | } 602 | } 603 | } 604 | // logprob += log10(multprob); 605 | val = (1.0-settings.min_likelihood)*multprob+settings.min_likelihood; 606 | // printf( "(beamprob:%d-%f-%f)", j, multprob, val ); 607 | } else { 608 | val = log(get_beam_prob( pos, lsens.laser.val[j], 609 | lsens.laser.angle[j], max_range, map )); 610 | prob += val; 611 | } 612 | } 613 | } 614 | return(exp(prob)); 615 | } 616 | 617 | 618 | double 619 | compute_beam_prob( MAP2 * map, logtools_rpos2_t pos, 620 | double length, double max_range, 621 | double * prob1, double * prob2 ) 622 | { 623 | static int first_time = TRUE; 624 | static logtools_grid_line_t line; 625 | static int max_num_linepoints = 0; 626 | int i; 627 | logtools_ivector2_t start, end; 628 | logtools_vector2_t abspt; 629 | logtools_rmove2_t nomove = {0.0, 0.0, 0.0}; 630 | double lprob = 0.0; 631 | 632 | if (first_time) { 633 | max_num_linepoints = 634 | 10 * ( max_range / map->resolution ); 635 | line.grid = (logtools_ivector2_t *) malloc( max_num_linepoints * 636 | sizeof(logtools_ivector2_t) ); 637 | first_time = FALSE; 638 | } 639 | 640 | abspt = logtools_compute_laser_points( pos, length, nomove, 0.0 ); 641 | map_pos_from_vec2( abspt, map, &end ); 642 | map_pos_from_rpos( pos, map, &start ); 643 | 644 | grid_line( start, end, &line ); 645 | if (line.numgrids>0) { 646 | for (i=0;i=map->mapsize.x || 648 | line.grid[i].y<0 || line.grid[i].y>=map->mapsize.y) { 649 | // map->mapsum[line.grid[i].x][line.grid[i].y]==0 ) { 650 | return(FALSE); 651 | } 652 | if (i==line.numgrids-1) { 653 | *prob1 = lprob + log(get_map_val( line.grid[i], *map )); 654 | *prob2 = lprob + log(1.0-get_map_val( line.grid[i], *map )); 655 | map->calc[line.grid[i].x][line.grid[i].y] = 1; 656 | } else { 657 | lprob += log(1.0-get_map_val( line.grid[i], *map )); 658 | map->calc[line.grid[i].x][line.grid[i].y] = 1; 659 | } 660 | } 661 | } else { 662 | return( FALSE ); 663 | } 664 | return(TRUE); 665 | } 666 | 667 | int 668 | intersect_bboxes( logtools_bounding_box_t box1, logtools_bounding_box_t box2 ) 669 | { 670 | if (box1.min.x<=box2.min.x) { 671 | /* box1.min.x is smaller that box2 */ 672 | if (box1.max.x>box2.min.x) { 673 | /* intersection in x */ 674 | if (box1.min.y<=box2.min.y) { 675 | /* box1.min.y is smaller that box2 */ 676 | if (box1.max.y>box2.min.y) { 677 | /* intersection in y */ 678 | return(1); 679 | } else { 680 | return(0); 681 | } 682 | } else { 683 | /* box2.min.y is smaller that box1 */ 684 | if (box2.max.y>=box1.min.y) { 685 | /* intersection in y */ 686 | return(1); 687 | } else { 688 | return(0); 689 | } 690 | } 691 | } else { 692 | return(0); 693 | } 694 | } else { 695 | /* box2.min.x is smaller that box1 */ 696 | if (box2.max.x>=box1.min.x) { 697 | /* intersection in x */ 698 | if (box1.min.y<=box2.min.y) { 699 | /* box1.min.y is smaller that box2 */ 700 | if (box1.max.y>box2.min.y) { 701 | /* intersection in y */ 702 | return(1); 703 | } else { 704 | return(0); 705 | } 706 | } else { 707 | /* box2.min.y is smaller that box1 */ 708 | if (box2.max.y>=box1.min.y) { 709 | /* intersection in y */ 710 | return(1); 711 | } else { 712 | return(0); 713 | } 714 | } 715 | } else { 716 | return(0); 717 | } 718 | } 719 | return(0); 720 | } 721 | 722 | void 723 | write_map( MAP2 map, char *filename ) 724 | { 725 | int ok = TRUE; 726 | int i, j, idx; 727 | 728 | Image * image; 729 | ImageInfo image_info; 730 | double c = 0.0; 731 | 732 | #if defined MagickLibVersion && MagickLibVersion >= 0x500 733 | ExceptionInfo exception; 734 | double * pixel; 735 | #else 736 | float * red, * green, * blue, * opacity; 737 | #endif 738 | 739 | #if defined MagickLibVersion && MagickLibVersion >= 0x500 740 | fprintf( stderr, "alloc memory of pixel map (%d bytes) ... ", 741 | map.mapsize.x * map.mapsize.y * 3 * sizeof(double) ); 742 | if ( (pixel = (double *) malloc(map.mapsize.x * map.mapsize.y * 743 | 3 * sizeof(double)))==NULL ) 744 | ok = FALSE; 745 | fprintf( stderr, "%s\n", ok?"yes":"no" ); 746 | #else 747 | fprintf( stderr, "alloc memory of pixel map (%d bytes) ... ", 748 | map.mapsize.x * map.mapsize.y * 3 * sizeof(float) ); 749 | if ( (red = (float *) malloc(map.mapsize.x * map.mapsize.y * 750 | sizeof(float)))==NULL ) 751 | ok = FALSE; 752 | fprintf( stderr, "red: %s,", ok?"yes":"no" ); 753 | if ( (green = (float *) malloc(map.mapsize.x * map.mapsize.y * 754 | sizeof(float)))==NULL ) 755 | ok = FALSE; 756 | fprintf( stderr, "green: %s,", ok?"yes":"no" ); 757 | if ( (blue = (float *) malloc(map.mapsize.x * map.mapsize.y * 758 | sizeof(float)))==NULL ) 759 | ok = FALSE; 760 | fprintf( stderr, "blue: %s,", ok?"yes":"no" ); 761 | if ( (opacity = (float *) malloc(map.mapsize.x * map.mapsize.y * 762 | sizeof(float)))==NULL ) 763 | ok = FALSE; 764 | fprintf( stderr, "opacity: %s,", ok?"yes":"no" ); 765 | #endif 766 | for (i=0;i0) { 771 | if (c<0.0) 772 | c = 0.0; 773 | #if defined MagickLibVersion && MagickLibVersion >= 0x500 774 | pixel[idx*3] = c; 775 | pixel[idx*3+1] = c; 776 | pixel[idx*3+2] = c; 777 | #else 778 | red[idx] = c; 779 | green[idx] = c; 780 | blue[idx] = c; 781 | opacity[idx] = 1.0; 782 | #endif 783 | } else { 784 | #if defined MagickLibVersion && MagickLibVersion >= 0x500 785 | pixel[idx*3] = 200.0/255.0; 786 | pixel[idx*3+1] = 200.0/255.0; 787 | pixel[idx*3+2] = 255.0/255.0; 788 | #else 789 | red[idx] = 200.0/255.0; 790 | green[idx] = 200.0/255.0; 791 | blue[idx] = 255.0/255.0; 792 | opacity[idx] = 1.0; 793 | #endif 794 | } 795 | } 796 | } 797 | #if defined MagickLibVersion && MagickLibVersion >= 0x430 798 | GetExceptionInfo(&exception); 799 | #endif 800 | GetImageInfo(&image_info); 801 | fprintf( stderr, "create image of map ... " ); 802 | #if defined MagickLibVersion && MagickLibVersion >= 0x430 803 | image = ConstituteImage ( map.mapsize.x, map.mapsize.y, "RGB", 804 | DoublePixel, pixel, &exception ); 805 | if (image == (Image *) NULL) { 806 | fprintf( stderr, "ERROR: no memory!!!\n" ); 807 | exit(1); 808 | } 809 | #else 810 | #if defined MagickLibVersion && MagickLibVersion >= 0x420 811 | image = CreateImage( map.mapsize.x, map.mapsize.y, 812 | red, green, blue, opacity ); 813 | #else 814 | image = CreateImage( map.mapsize.x, map.mapsize.y, "RGB", 815 | red, green, blue, NULL ); 816 | if (image == (Image *) NULL) { 817 | fprintf( stderr, "ERROR: no memory!!!\n" ); 818 | exit(1); 819 | } 820 | #endif 821 | #endif 822 | strcpy( image->filename, filename ); 823 | fprintf( stderr, "done\n" ); 824 | fprintf( stderr, "write image in file %s ... ", filename ); 825 | WriteImage( &image_info, image ); 826 | fprintf( stderr, "done\n" ); 827 | DestroyImage(image); 828 | #if defined MagickLibVersion && MagickLibVersion >= 0x430 829 | DestroyConstitute(); 830 | free(pixel); 831 | #else 832 | free(red); 833 | free(green); 834 | free(blue); 835 | #endif 836 | 837 | 838 | } 839 | --------------------------------------------------------------------------------