├── breezyslam.png ├── matlab ├── mex_breezyslam.mexa64 ├── mex_breezyslam.mexw64 ├── mex_breezyslam.mexmaci64 ├── make.m ├── Deterministic_SLAM.m ├── Map.m ├── Scan.m ├── RMHC_SLAM.m ├── SinglePositionSLAM.m ├── WheeledRobot.m └── CoreSLAM.m ├── .gitignore ├── cpp ├── BreezySLAM.hpp ├── Map.cpp ├── Map.hpp ├── WheeledRobot.cpp ├── PoseChange.hpp ├── Position.hpp ├── Scan.hpp ├── Makefile ├── Scan.cpp ├── WheeledRobot.hpp ├── Laser.hpp └── Doxyfile ├── python ├── breezyslam │ ├── __init__.py │ ├── sensors.py │ └── vehicles.py ├── setup.py ├── pyextension_utils.c └── pyextension_utils.h ├── java └── edu │ └── wlu │ └── cs │ └── levy │ └── breezyslam │ ├── components │ ├── URG04LX.java │ ├── Position.java │ ├── Map.java │ ├── Scan.java │ ├── Makefile │ ├── PoseChange.java │ ├── Laser.java │ └── jnibreezyslam_components.c │ ├── algorithms │ ├── DeterministicSLAM.java │ ├── jnibreezyslam_algorithms.c │ ├── Makefile │ ├── RMHCSLAM.java │ ├── SinglePositionSLAM.java │ └── CoreSLAM.java │ ├── jni_utils.h │ └── robots │ └── WheeledRobot.java ├── c ├── coreslam_internals.h ├── ziggurat.h ├── random.h ├── random.c ├── coreslam_sisd.c ├── coreslam_i686.c ├── coreslam_armv7l.c └── coreslam.h ├── examples ├── urgslam.py ├── Makefile ├── pgm_utils.py ├── xvslam.py ├── progressbar.py ├── MinesRover.m ├── rpslam.py ├── rpslam_scipy.py ├── mines.py ├── log2pkl.py ├── logmovie.py ├── log2pgm.py ├── log2png.py └── logdemo.m └── LICENSE.md /breezyslam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/BreezySLAM/HEAD/breezyslam.png -------------------------------------------------------------------------------- /matlab/mex_breezyslam.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/BreezySLAM/HEAD/matlab/mex_breezyslam.mexa64 -------------------------------------------------------------------------------- /matlab/mex_breezyslam.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/BreezySLAM/HEAD/matlab/mex_breezyslam.mexw64 -------------------------------------------------------------------------------- /matlab/mex_breezyslam.mexmaci64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/BreezySLAM/HEAD/matlab/mex_breezyslam.mexmaci64 -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.h 2 | java/edu/wlu/cs/levy/breezyslam/components/Map.h 3 | java/edu/wlu/cs/levy/breezyslam/components/Scan.h 4 | examples/__pycache__ 5 | examples/*.pyc 6 | examples/*.pgm 7 | examples/*.png 8 | examples/*.map 9 | examples/log2pgm 10 | python/breezyslam/*.pyc 11 | python/breezyslam/__pycache__ 12 | python/build 13 | *.class 14 | *.o 15 | *.so 16 | *.swp 17 | 18 | -------------------------------------------------------------------------------- /cpp/BreezySLAM.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * \mainpage BreezySLAM: Simple, efficient SLAM in C++ 4 | * 5 | * 6 | * Copyright (C) 2014 Simon D. Levy 7 | 8 | * This code is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU Lesser General Public License as 10 | * published by the Free Software Foundation, either version 3 of the 11 | * License, or (at your option) any later version. 12 | * 13 | * This code is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU Lesser General Public License 19 | * along with this code. If not, see . 20 | */ 21 | -------------------------------------------------------------------------------- /python/breezyslam/__init__.py: -------------------------------------------------------------------------------- 1 | ''' 2 | BreezySLAM: Simple, efficient SLAM in Python 3 | 4 | Copyright (C) 2014 Simon D. Levy 5 | 6 | This code is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as 8 | published by the Free Software Foundation, either version 3 of the 9 | License, or (at your option) any later version. 10 | 11 | This code is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with this code. If not, see . 18 | You should also have received a copy of the Parrot Parrot AR.Drone 19 | ''' 20 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java: -------------------------------------------------------------------------------- 1 | package edu.wlu.cs.levy.breezyslam.components; 2 | 3 | /** 4 | * A class for the Hokuyo URG-04LX laser. 5 | */ 6 | public class URG04LX extends Laser 7 | { 8 | 9 | /** 10 | * Builds a URG04LX object. 11 | * Lidar unit. 12 | * @param detection_margin number of rays at edges of scan to ignore 13 | * @param offset_mm forward/backward offset of laser motor from robot center 14 | * @return a new URG04LX object 15 | * 16 | */ 17 | public URG04LX(int detection_margin, double offset_mm) 18 | { 19 | super(682, 10, 240, 4000, detection_margin, offset_mm); 20 | } 21 | 22 | /** 23 | * Builds a URG04LX object with zero detection margin, offset mm. 24 | */ 25 | public URG04LX() 26 | { 27 | this(0, 0); 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /matlab/make.m: -------------------------------------------------------------------------------- 1 | % Script for building BreezySLAM C extensions 2 | 3 | % Copyright (C) 2014 Simon D. Levy 4 | % 5 | % This code is free software: you can redistribute it and/or modify 6 | % it under the terms of the GNU Lesser General Public License as 7 | % published by the Free Software Foundation, either version 3 of the 8 | % License, or (at your option) any later version. 9 | % 10 | % This code is distributed in the hope that it will be useful, 11 | % but WITHOUT ANY WARRANTY without even the implied warranty of 12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | % GNU General Public License for more details. 14 | % 15 | % You should have received a copy of the GNU Lesser General Public License 16 | % along with this code. If not, see . 17 | 18 | 19 | mex mex_breezyslam.c ../c/coreslam.c ../c/coreslam_sisd.c ../c/random.c ../c/ziggurat.c 20 | -------------------------------------------------------------------------------- /c/coreslam_internals.h: -------------------------------------------------------------------------------- 1 | /* 2 | coreslam_internals.h internal support for CoreSLAM 3 | 4 | Copyright (C) 2014 Simon D. Levy 5 | 6 | This code is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as 8 | published by the Free Software Foundation, either version 3 of the 9 | License, or (at your option) any later version. 10 | 11 | This code is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with this code. If not, see . 18 | */ 19 | 20 | 21 | #ifdef _MSC_VER 22 | typedef __int64 int64_t; /* Define it from MSVC's internal type */ 23 | #define _USE_MATH_DEFINES 24 | #include 25 | #else 26 | #include /* Use the C99 official header */ 27 | #endif 28 | 29 | 30 | static const int NO_OBSTACLE = 65500; 31 | static const int OBSTACLE = 0; 32 | 33 | static double 34 | radians(double degrees) 35 | { 36 | return degrees * M_PI / 180; 37 | } 38 | -------------------------------------------------------------------------------- /c/ziggurat.h: -------------------------------------------------------------------------------- 1 | /* 2 | ziggurat.h Ziggurat random-number generator 3 | 4 | Downloaded from 5 | 6 | http://people.sc.fsu.edu/~jburkardt/c_src/ziggurat/ziggurat.c 7 | 8 | on 20 July 2014. 9 | 10 | This code is free software: you can redistribute it and/or modify 11 | it under the terms of the GNU Lesser General Public License as 12 | published by the Free Software Foundation, either version 3 of the 13 | License, or (at your option) any later version. 14 | 15 | This code is distributed in the hope that it will be useful, 16 | but WITHOUT ANY WARRANTY without even the implied warranty of 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | GNU General Public License for more details. 19 | 20 | You should have received a copy of the GNU Lesser General Public License 21 | along with this code. If not, see . 22 | */ 23 | 24 | #include 25 | 26 | uint32_t cong_seeded ( uint32_t *jcong ); 27 | double cpu_time ( void ); 28 | uint32_t kiss_seeded ( uint32_t *jcong, uint32_t *jsr, uint32_t *w, uint32_t *z ); 29 | uint32_t mwc_seeded ( uint32_t *w, uint32_t *z ); 30 | float r4_exp ( uint32_t *jsr, uint32_t ke[256], float fe[256], float we[256] ); 31 | void r4_exp_setup ( uint32_t ke[256], float fe[256], float we[256] ); 32 | float r4_nor ( uint32_t *jsr, uint32_t kn[128], float fn[128], float wn[128] ); 33 | void r4_nor_setup ( uint32_t kn[128], float fn[128], float wn[128] ); 34 | float r4_uni ( uint32_t *jsr ); 35 | uint32_t shr3_seeded ( uint32_t *jsr ); 36 | void timestamp ( void ); 37 | -------------------------------------------------------------------------------- /c/random.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | random.h - Function prototypes for a random-number generator supporting normal 4 | distributions 5 | 6 | Copyright (C) 2014 Simon D. Levy 7 | 8 | 9 | This code is free software: you can redistribute it and/or modify 10 | it under the terms of the GNU Lesser General Public License as 11 | published by the Free Software Foundation, either version 3 of the 12 | License, or (at your option) any later version. 13 | 14 | This code is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | GNU General Public License for more details. 18 | 19 | You should have received a copy of the GNU Lesser General Public License 20 | along with this code. If not, see . 21 | */ 22 | 23 | #include "stdlib.h" 24 | 25 | #ifdef __cplusplus 26 | extern "C" { 27 | #endif 28 | 29 | /* Returns size of random-number generator in bytes */ 30 | size_t random_size(void); 31 | 32 | /* Creates and initializes a new random-number generator */ 33 | void * random_new(int seed); 34 | 35 | /* Initializes a random-number generator */ 36 | void random_init(void * r, int seed); 37 | 38 | /* Make a copy of the specified random-number generator */ 39 | void * random_copy(void * r); 40 | 41 | /* Deallocates memory for a random-number generator */ 42 | void random_free(void * v); 43 | 44 | /* Returns a standard normal variate with mean mu, variance sigma */ 45 | double random_normal(void * v, double mu, double sigma); 46 | 47 | #ifdef __cplusplus 48 | } 49 | #endif 50 | -------------------------------------------------------------------------------- /c/random.c: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | random.c - Glue code for BreezySLAM pseudorandom number generator 4 | 5 | Copyright (C) 2014 Simon D. Levy 6 | 7 | This code is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU Lesser General Public License as 9 | published by the Free Software Foundation, either version 3 of the 10 | License, or (at your option) any later version. 11 | 12 | This code is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU Lesser General Public License 18 | along with this code. If not, see . 19 | */ 20 | 21 | #include "ziggurat.h" 22 | #include "random.h" 23 | 24 | #include 25 | #include 26 | 27 | typedef struct random_t 28 | { 29 | float fn[128]; 30 | uint32_t kn[128]; 31 | float wn[128]; 32 | uint32_t seed; 33 | 34 | } random_t; 35 | 36 | size_t random_size(void) 37 | { 38 | return sizeof(random_t); 39 | } 40 | 41 | 42 | void * random_new(int seed) 43 | { 44 | random_t * r = (random_t *)malloc(sizeof(random_t)); 45 | 46 | random_init(r, seed); 47 | 48 | return r; 49 | 50 | } 51 | 52 | 53 | void random_init(void * v, int seed) 54 | { 55 | random_t * r = (random_t *)v; 56 | 57 | r->seed = seed; 58 | 59 | r4_nor_setup (r->kn, r->fn, r->wn ); 60 | } 61 | 62 | 63 | double random_normal(void * v, double mu, double sigma) 64 | { 65 | random_t * r = (random_t *)v; 66 | 67 | return mu + sigma * r4_nor ( &r->seed, r->kn, r->fn, r->wn ); 68 | } 69 | 70 | void random_free(void * v) 71 | { 72 | free(v); 73 | } 74 | 75 | void * random_copy(void * v) 76 | { 77 | random_t * r = (random_t *)malloc(sizeof(random_t)); 78 | 79 | memcpy(r, v, sizeof(random_t)); 80 | 81 | return r; 82 | } 83 | 84 | -------------------------------------------------------------------------------- /cpp/Map.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * \mainpage BreezySLAM: Simple, efficient SLAM in C++ 4 | * 5 | * Map.cpp - implementation for Map class 6 | * 7 | * Copyright (C) 2014 Simon D. Levy 8 | 9 | * This code is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU Lesser General Public License as 11 | * published by the Free Software Foundation, either version 3 of the 12 | * License, or (at your option) any later version. 13 | * 14 | * This code is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU Lesser General Public License 20 | * along with this code. If not, see . 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | using namespace std; 29 | 30 | #include "coreslam.h" 31 | 32 | #include "Scan.hpp" 33 | #include "Position.hpp" 34 | #include "Map.hpp" 35 | 36 | Map::Map(int size_pixels, double size_meters) 37 | { 38 | this->map = new map_t; 39 | map_init(this->map, size_pixels, size_meters); 40 | } 41 | 42 | Map::~Map(void) 43 | { 44 | map_free(this->map); 45 | delete this->map; 46 | } 47 | 48 | void Map::update( 49 | Scan & scan, 50 | Position & position, 51 | int quality, 52 | double hole_width_mm) 53 | { 54 | position_t cpos; 55 | cpos.x_mm = position.x_mm; 56 | cpos.y_mm = position.y_mm; 57 | cpos.theta_degrees = position.theta_degrees; 58 | 59 | map_update(this->map, scan.scan, cpos, quality, hole_width_mm); 60 | } 61 | 62 | void Map::get(char * bytes) 63 | { 64 | map_get(this->map, bytes); 65 | } 66 | 67 | 68 | ostream& operator<< (ostream & out, Map & map) 69 | { 70 | char str[512]; 71 | 72 | map_string(*map.map, str); 73 | 74 | out << str; 75 | 76 | return out; 77 | } 78 | 79 | 80 | -------------------------------------------------------------------------------- /examples/urgslam.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ''' 4 | urgslam.py : BreezySLAM Python with Hokuyo URG04LX Lidar 5 | 6 | Copyright (C) 2015 Simon D. Levy 7 | 8 | This code is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU Lesser General Public License as 10 | published by the Free Software Foundation, either version 3 of the 11 | License, or (at your option) any later version. 12 | 13 | This code is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public License 19 | along with this code. If not, see . 20 | ''' 21 | 22 | MAP_SIZE_PIXELS = 500 23 | MAP_SIZE_METERS = 10 24 | LIDAR_DEVICE = '/dev/ttyACM0' 25 | 26 | from breezyslam.algorithms import RMHC_SLAM 27 | from breezyslam.sensors import URG04LX as LaserModel 28 | 29 | from breezylidar import URG04LX as Lidar 30 | 31 | from roboviz import MapVisualizer 32 | 33 | if __name__ == '__main__': 34 | 35 | # Connect to Lidar unit 36 | lidar = Lidar(LIDAR_DEVICE) 37 | 38 | # Create an RMHC SLAM object with a laser model and optional robot model 39 | slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) 40 | 41 | # Set up a SLAM display 42 | viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM') 43 | 44 | # Initialize empty map 45 | mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) 46 | 47 | while True: 48 | 49 | # Update SLAM with current Lidar scan 50 | slam.update(lidar.getScan()) 51 | 52 | # Get current robot position 53 | x, y, theta = slam.getpos() 54 | 55 | # Get current map bytes as grayscale 56 | slam.getmap(mapbytes) 57 | 58 | # Display map and robot pose, exiting gracefully if user closes it 59 | if not viz.display(x/1000., y/1000., theta, mapbytes): 60 | exit(0) 61 | -------------------------------------------------------------------------------- /matlab/Deterministic_SLAM.m: -------------------------------------------------------------------------------- 1 | classdef Deterministic_SLAM < SinglePositionSLAM 2 | %Deterministic_SLAM SLAM with no Monte Carlo search 3 | % Implements the getNewPosition() method of SinglePositionSLAM 4 | % by copying the start position. 5 | % 6 | % Copyright (C) 2014 Simon D. Levy 7 | % 8 | % This code is free software: you can redistribute it and/or modify 9 | % it under the terms of the GNU Lesser General Public License as 10 | % published by the Free Software Foundation, either version 3 of the 11 | % License, or (at your option) any later version. 12 | % 13 | % This code is distributed in the hope that it will be useful, 14 | % but WITHOUT ANY WARRANTY without even the implied warranty of 15 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | % GNU General Public License for more details. 17 | % 18 | % You should have received a copy of the GNU Lesser General Public License 19 | % along with this code. If not, see . 20 | 21 | methods 22 | 23 | function slam = Deterministic_SLAM(laser, map_size_pixels, map_size_meters) 24 | %Creates a Deterministic_SLAM object suitable for updating with new Lidar and odometry data. 25 | % slam = Deterministic_SLAM(laser, map_size_pixels, map_size_meters) 26 | % laser is a Laser object representing the specifications of your Lidar unit 27 | % map_size_pixels is the size of the square map in pixels 28 | % map_size_meters is the size of the square map in meters 29 | 30 | slam = slam@SinglePositionSLAM(laser, map_size_pixels, map_size_meters); 31 | 32 | end 33 | 34 | function new_pos = getNewPosition(~, start_pos) 35 | % Implements the _getNewPosition() method of SinglePositionSLAM. 36 | % new_pos = getNewPosition(~, start_pos) simply returns start_pos 37 | 38 | new_pos = start_pos; 39 | end 40 | 41 | end 42 | 43 | end 44 | 45 | -------------------------------------------------------------------------------- /examples/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for BreezySLAM demos 2 | # 3 | # Copyright (C) 2014 Simon D. Levy 4 | # 5 | # This code is free software: you can redistribute it and/or modify 6 | # it under the terms of the GNU Lesser General Public License as 7 | # published by the Free Software Foundation, either version 3 of the 8 | # License, or (at your option) any later version. 9 | # 10 | # This code is distributed in the hope that it will be useful, 11 | # but WITHOUT ANY WARRANTY without even the implied warranty of 12 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | # GNU General Public License for more details. 14 | 15 | # You should have received a copy of the GNU Lesser General Public License 16 | # along with this code. If not, see . 17 | 18 | # Where you put the libbreezyslam library 19 | LIBDIR = /usr/local/lib 20 | 21 | JAVADIR = ../java/edu/wlu/cs/levy/breezyslam 22 | 23 | # Use EOG or your favorite image-display program 24 | VIEWER = eog 25 | 26 | # Set these for different experiments 27 | DATASET = exp2 28 | USE_ODOMETRY = 0 29 | RANDOM_SEED = 9999 30 | 31 | all: log2pgm Log2PGM.class 32 | 33 | pltmovie: 34 | ./logdemoplt.py exp1 1 9999 35 | 36 | movie: 37 | ./logmovie.py exp1 1 9999 38 | 39 | pytest: 40 | ./log2pgm.py $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED) 41 | $(VIEWER) $(DATASET).pgm ~/Desktop/$(DATASET).pgm 42 | 43 | 44 | cpptest: log2pgm 45 | ./log2pgm $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED) 46 | $(VIEWER) $(DATASET).pgm 47 | 48 | javatest: Log2PGM.class 49 | java -classpath ../java:. -Djava.library.path=$(JAVADIR)/algorithms:$(JAVADIR)/components Log2PGM \ 50 | $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED) 51 | $(VIEWER) $(DATASET).pgm 52 | 53 | log2pgm: log2pgm.o 54 | g++ -O3 -o log2pgm log2pgm.o -L$(LIBDIR) -lbreezyslam 55 | 56 | log2pgm.o: log2pgm.cpp 57 | g++ -O3 -c -I ../cpp log2pgm.cpp 58 | 59 | Log2PGM.class: Log2PGM.java 60 | javac -classpath ../java Log2PGM.java 61 | 62 | $(DATASET).pgm: 63 | ./log2pgm.py $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED) 64 | 65 | backup: 66 | cp -r .. ~/Documents/slam/bak-breezyslam 67 | 68 | clean: 69 | rm -f log2pgm *.pyc *.pgm *.o *.class *~ 70 | -------------------------------------------------------------------------------- /python/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | setup.py - Python distutils setup file for BreezySLAM package. 5 | 6 | Copyright (C) 2014 Simon D. Levy 7 | 8 | This code is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU Lesser General Public License as 10 | published by the Free Software Foundation, either version 3 of the 11 | License, or (at your option) any later version. 12 | 13 | This code is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public License 19 | along with this code. If not, see . 20 | ''' 21 | 22 | # Support streaming SIMD extensions 23 | 24 | from platform import machine 25 | 26 | OPT_FLAGS = [] 27 | SIMD_FLAGS = [] 28 | 29 | arch = machine() 30 | 31 | print(arch) 32 | 33 | if arch in ['i686', 'x86_64']: 34 | SIMD_FLAGS = ['-msse3'] 35 | arch = 'i686' 36 | 37 | elif arch == 'armv7l': 38 | OPT_FLAGS = ['-O3'] 39 | SIMD_FLAGS = ['-mfpu=neon'] 40 | 41 | else: 42 | arch = 'sisd' 43 | 44 | SOURCES = [ 45 | 'pybreezyslam.c', 46 | 'pyextension_utils.c', 47 | '../c/coreslam.c', 48 | '../c/coreslam_' + arch + '.c', 49 | '../c/random.c', 50 | '../c/ziggurat.c'] 51 | 52 | from distutils.core import setup, Extension 53 | 54 | module = Extension('pybreezyslam', 55 | sources = SOURCES, 56 | extra_compile_args = ['-std=gnu99'] + SIMD_FLAGS + OPT_FLAGS 57 | ) 58 | 59 | 60 | setup (name = 'BreezySLAM', 61 | version = '0.1', 62 | description = 'Simple, efficient SLAM in Python', 63 | packages = ['breezyslam'], 64 | ext_modules = [module], 65 | author='Simon D. Levy and Suraj Bajracharya', 66 | author_email='simon.d.levy@gmail.com', 67 | url='https://github.com/simondlevy/BreezySLAM', 68 | license='LGPL', 69 | platforms='Linux; Windows; OS X', 70 | long_description = 'Provides core classes Position, Map, Laser, Scan, and algorithm CoreSLAM' 71 | ) 72 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java: -------------------------------------------------------------------------------- 1 | /** 2 | * DeterministicSLAM implements SinglePositionSLAM using by returning the starting position instead of searching 3 | * on it; i.e., using odometry alone. 4 | * 5 | * Copyright (C) 2014 Simon D. Levy 6 | * 7 | * This code is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as 9 | * published by the Free Software Foundation, either version 3 of the 10 | * License, or (at your option) any later version. 11 | * 12 | * This code is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with this code. If not, see . 19 | */ 20 | 21 | 22 | package edu.wlu.cs.levy.breezyslam.algorithms; 23 | 24 | import edu.wlu.cs.levy.breezyslam.components.Position; 25 | import edu.wlu.cs.levy.breezyslam.components.Laser; 26 | 27 | public class DeterministicSLAM extends SinglePositionSLAM 28 | { 29 | 30 | /** 31 | * Creates a DeterministicSLAM object. 32 | * @param laser a Laser object containing parameters for your Lidar equipment 33 | * @param map_size_pixels the size of the desired map (map is square) 34 | * @param map_size_meters the size of the area to be mapped, in meters 35 | * @return a new CoreSLAM object 36 | */ 37 | public DeterministicSLAM(Laser laser, int map_size_pixels, double map_size_meters) 38 | { 39 | super(laser, map_size_pixels, map_size_meters); 40 | } 41 | 42 | /** 43 | * Returns a new position identical to the starting position. Called automatically by 44 | * SinglePositionSLAM::updateMapAndPointcloud() 45 | * @param start_pos the starting position 46 | */ 47 | protected Position getNewPosition(Position start_position) 48 | { 49 | return new Position(start_position.x_mm, start_position.y_mm, start_position.theta_degrees); 50 | } 51 | 52 | } // DeterministicSLAM 53 | -------------------------------------------------------------------------------- /cpp/Map.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Map.hpp - header for Map class 4 | * 5 | * Copyright (C) 2014 Simon D. Levy 6 | 7 | * This code is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as 9 | * published by the Free Software Foundation, either version 3 of the 10 | * License, or (at your option) any later version. 11 | * 12 | * This code is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with this code. If not, see . 19 | */ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | using namespace std; 27 | 28 | typedef unsigned short pixel_t; 29 | 30 | 31 | class Scan; 32 | class Position; 33 | 34 | /** 35 | * A class for maps used in SLAM. 36 | */ 37 | class Map 38 | { 39 | friend class CoreSLAM; 40 | friend class SinglePositionSLAM; 41 | friend class RMHC_SLAM; 42 | 43 | public: 44 | 45 | /** 46 | * Builds a square Map object. 47 | * @param size_pixels size in pixels 48 | * @param size_meters size in meters 49 | * 50 | */ 51 | Map(int size_pixels, double size_meters); 52 | 53 | 54 | /** 55 | * Deallocates this Map object. 56 | * 57 | */ 58 | ~Map(void); 59 | 60 | 61 | /** 62 | * Puts current map values into bytearray, which should of which should be of 63 | * this->size map_size_pixels ^ 2. 64 | */ 65 | void get(char * bytes); 66 | 67 | /** 68 | * Updates this map object based on new data. 69 | * @param scan a new scan 70 | * @param position a new postion 71 | * @param quality speed with which scan is integerate into map (0 through 255) 72 | * @param hole_width_mm hole width in millimeters 73 | * 74 | */ 75 | void update( 76 | Scan & scan, 77 | Position & position, 78 | int quality, 79 | double hole_width_mm); 80 | 81 | 82 | friend ostream& operator<< (ostream & out, Map & map); 83 | 84 | private: 85 | 86 | struct map_t * map; 87 | }; 88 | 89 | -------------------------------------------------------------------------------- /examples/pgm_utils.py: -------------------------------------------------------------------------------- 1 | ''' 2 | pgm_utils.py : Python utilties for PGM files 3 | 4 | Copyright (C) 2014 Simon D. Levy 5 | 6 | This code is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as 8 | published by the Free Software Foundation, either version 3 of the 9 | License, or (at your option) any later version. 10 | 11 | This code is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with this code. If not, see . 18 | 19 | Change log: 20 | 21 | 20-APR-2014 - Simon D. Levy - Get params from command line 22 | ''' 23 | 24 | def pgm_load(filename): 25 | 26 | print('Loading image from file %s...' % filename) 27 | 28 | fd = open(filename, 'rt') 29 | 30 | # Skip constant header 31 | fd.readline() 32 | 33 | # Grab image size (assume square) 34 | imgsize = [int(tok) for tok in fd.readline().split()] 35 | 36 | # Start with empty list 37 | imglist = [] 38 | 39 | # Read lines and append them to list until done 40 | while True: 41 | 42 | line = fd.readline() 43 | 44 | if len(line) == 0: 45 | break 46 | 47 | imglist.extend([int(tok) for tok in line.split()]) 48 | 49 | fd.close() 50 | 51 | # Convert list into bytes 52 | imgbytes = bytearray(imglist) 53 | 54 | return imgbytes, imgsize 55 | 56 | def pgm_save(filename, imgbytes, imgsize): 57 | 58 | print('\nSaving image to file %s' % filename) 59 | 60 | output = open(filename, 'wt') 61 | 62 | output.write('P2\n%d %d 255\n' % imgsize) 63 | 64 | wid, hgt = imgsize 65 | 66 | for y in range(hgt): 67 | for x in range(wid): 68 | output.write('%d ' % imgbytes[y * wid + x]) 69 | output.write('\n') 70 | 71 | output.close() 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/jni_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * jni_utils.h Utilities for Java Native Interface code 3 | * 4 | * Copyright (C) 2014 Simon D. Levy 5 | * 6 | * This code is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU Lesser General Public License as 8 | * published by the Free Software Foundation, either version 3 of the 9 | * License, or (at your option) any later version. 10 | * 11 | * This code is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public License 17 | * along with this code. If not, see . 18 | */ 19 | 20 | 21 | #include 22 | #include 23 | #include "../../../../../../c/coreslam.h" 24 | 25 | #include 26 | #include 27 | 28 | static jfieldID get_fid(JNIEnv *env, jobject object, const char * fieldname, const char * fieldsig) 29 | { 30 | jclass cls = (*env)->GetObjectClass(env, object); 31 | return (*env)->GetFieldID(env, cls, fieldname, fieldsig); 32 | } 33 | 34 | static jfieldID get_this_fid(JNIEnv *env, jobject thisobject) 35 | { 36 | return get_fid(env, thisobject, "native_ptr", "J"); 37 | } 38 | 39 | static void * ptr_from_obj(JNIEnv *env, jobject thisobject) 40 | { 41 | return (void *)(*env)->GetLongField (env, thisobject, get_this_fid(env, thisobject)); 42 | } 43 | 44 | static void ptr_to_obj(JNIEnv *env, jobject thisobject, void * ptr) 45 | { 46 | (*env)->SetLongField (env, thisobject, get_this_fid(env, thisobject), (long)ptr); 47 | } 48 | 49 | static scan_t * cscan_from_jscan(JNIEnv *env, jobject thisobject) 50 | { 51 | return (scan_t *)ptr_from_obj(env, thisobject); 52 | } 53 | 54 | static map_t * cmap_from_jmap(JNIEnv *env, jobject thisobject) 55 | { 56 | return (map_t *)ptr_from_obj(env, thisobject); 57 | } 58 | 59 | static double get_double_field(JNIEnv *env, jobject object, const char * fieldname) 60 | { 61 | return (*env)->GetDoubleField (env, object, get_fid(env, object, fieldname, "D")); 62 | } 63 | 64 | 65 | -------------------------------------------------------------------------------- /examples/xvslam.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ''' 4 | xvslam.py : BreezySLAM Python with GetSurreal XV Lidar 5 | 6 | Copyright (C) 2016 Simon D. Levy 7 | 8 | This code is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU Lesser General Public License as 10 | published by the Free Software Foundation, either version 3 of the 11 | License, or (at your option) any later version. 12 | 13 | This code is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public License 19 | along with this code. If not, see . 20 | ''' 21 | 22 | MAP_SIZE_PIXELS = 500 23 | MAP_SIZE_METERS = 10 24 | LIDAR_DEVICE = '/dev/ttyACM0' 25 | 26 | from breezyslam.algorithms import RMHC_SLAM 27 | from breezyslam.sensors import XVLidar as LaserModel 28 | 29 | from xvlidar import XVLidar as Lidar 30 | 31 | from roboviz import MapVisualizer 32 | 33 | if __name__ == '__main__': 34 | 35 | # Connect to Lidar unit 36 | lidar = Lidar(LIDAR_DEVICE) 37 | 38 | # Create an RMHC SLAM object with a laser model and optional robot model 39 | slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) 40 | 41 | # Set up a SLAM display 42 | viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM') 43 | 44 | # Initialize an empty trajectory 45 | trajectory = [] 46 | 47 | # Initialize empty map 48 | mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) 49 | 50 | while True: 51 | 52 | # Update SLAM with current Lidar scan, using first element of (scan, quality) pairs 53 | slam.update([pair[0] for pair in lidar.getScan()]) 54 | 55 | # Get current robot position 56 | x, y, theta = slam.getpos() 57 | 58 | # Get current map bytes as grayscale 59 | slam.getmap(mapbytes) 60 | 61 | # Display map and robot pose, exiting gracefully if user closes it 62 | if not viz.display(x/1000., y/1000., theta, mapbytes): 63 | exit(0) 64 | -------------------------------------------------------------------------------- /examples/progressbar.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Downloaded from http://code.activestate.com/recipes/168639-progress-bar-class/ 4 | # 12 January 2013 5 | 6 | class ProgressBar: 7 | 8 | def __init__(self, minValue = 0, maxValue = 10, totalWidth=12): 9 | self.progBar = "[]" # This holds the progress bar string 10 | self.min = minValue 11 | self.max = maxValue 12 | self.span = maxValue - minValue 13 | self.width = totalWidth 14 | self.amount = 0 # When amount == max, we are 100% done 15 | self.updateAmount(0) # Build progress bar string 16 | 17 | def updateAmount(self, newAmount = 0): 18 | if newAmount < self.min: newAmount = self.min 19 | if newAmount > self.max: newAmount = self.max 20 | self.amount = newAmount 21 | 22 | # Figure out the new percent done, round to an integer 23 | diffFromMin = float(self.amount - self.min) 24 | percentDone = (diffFromMin / float(self.span)) * 100.0 25 | percentDone = round(percentDone) 26 | percentDone = int(percentDone) 27 | 28 | # Figure out how many hash bars the percentage should be 29 | allFull = self.width - 2 30 | numHashes = (percentDone / 100.0) * allFull 31 | numHashes = int(round(numHashes)) 32 | 33 | # build a progress bar with hashes and spaces 34 | self.progBar = "[" + '#'*numHashes + ' '*(allFull-numHashes) + "]" 35 | 36 | # figure out where to put the percentage, roughly centered 37 | percentPlace = int((len(self.progBar) / 2) - len(str(percentDone))) 38 | percentString = str(percentDone) + "%" 39 | 40 | # slice the percentage into the bar 41 | self.progBar = self.progBar[0:percentPlace] + percentString + self.progBar[percentPlace+len(percentString):] 42 | 43 | def __str__(self): 44 | return str(self.progBar) 45 | 46 | if __name__ == '__main__': 47 | 48 | import time 49 | import sys 50 | 51 | progbar = ProgressBar(0, 100, 80) 52 | 53 | for k in range(0,100): 54 | 55 | progbar.updateAmount(k) 56 | 57 | sys.stdout.write('%s\r' % progbar) 58 | 59 | time.sleep(.01) 60 | 61 | sys.stdout.write('\n') 62 | -------------------------------------------------------------------------------- /matlab/Map.m: -------------------------------------------------------------------------------- 1 | classdef Map 2 | %A class for maps (occupancy grids) used in SLAM 3 | % 4 | % Copyright (C) 2014 Simon D. Levy 5 | % 6 | % This code is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU Lesser General Public License as 8 | % published by the Free Software Foundation, either version 3 of the 9 | % License, or (at your option) any later version. 10 | % 11 | % This code is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU Lesser General Public License 17 | % along with this code. If not, see . 18 | 19 | properties (Access = {?RMHC_SLAM}) 20 | 21 | c_map 22 | end 23 | 24 | methods (Access = 'public') 25 | 26 | function map = Map(size_pixels, size_meters) 27 | % Map creates an empty square map 28 | % map = Map(size_pixels, size_meters) 29 | map.c_map = mex_breezyslam('Map_init', size_pixels, size_meters); 30 | 31 | end 32 | 33 | function disp(map) 34 | % Displays data about this map 35 | mex_breezyslam('Map_disp', map.c_map) 36 | 37 | end 38 | 39 | function update(map, scan, new_position, map_quality, hole_width_mm) 40 | % Updates this map with a new scan and position 41 | % 42 | % update(map, scan, new_position, map_quality, hole_width_mm) 43 | mex_breezyslam('Map_update', map.c_map, scan.c_scan, new_position, int32(map_quality), hole_width_mm) 44 | 45 | end 46 | 47 | function bytes = get(map) 48 | % Returns occupancy grid matrix of bytes for this map 49 | % 50 | % bytes = get(map) 51 | 52 | % Transpose for uniformity with Python, C++ versions 53 | bytes = mex_breezyslam('Map_get', map.c_map)'; 54 | 55 | end 56 | 57 | end % methods 58 | 59 | end % classdef 60 | -------------------------------------------------------------------------------- /cpp/WheeledRobot.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * \mainpage BreezySLAM: Simple, efficient SLAM in C++ 4 | * 5 | * WheeledRobot.cpp - C++ code for WheeledRobot class 6 | * 7 | * Copyright (C) 2014 Simon D. Levy 8 | 9 | * This code is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU Lesser General Public License as 11 | * published by the Free Software Foundation, either version 3 of the 12 | * License, or (at your option) any later version. 13 | * 14 | * This code is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU Lesser General Public License 20 | * along with this code. If not, see . 21 | */ 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | using namespace std; 29 | 30 | #include "WheeledRobot.hpp" 31 | #include "PoseChange.hpp" 32 | 33 | PoseChange 34 | WheeledRobot::computePoseChange( 35 | double timestamp, 36 | double left_wheel_odometry, 37 | double right_wheel_odometry) 38 | { 39 | PoseChange poseChange; 40 | 41 | double timestamp_seconds_curr, left_wheel_degrees_curr, right_wheel_degrees_curr; 42 | 43 | this->extractOdometry(timestamp, left_wheel_odometry, right_wheel_odometry, 44 | timestamp_seconds_curr, left_wheel_degrees_curr, right_wheel_degrees_curr); 45 | 46 | if (this->timestamp_seconds_prev > 0) 47 | { 48 | double left_diff_degrees = left_wheel_degrees_curr - this->left_wheel_degrees_prev; 49 | double right_diff_degrees = right_wheel_degrees_curr - this->right_wheel_degrees_prev; 50 | 51 | poseChange.dxy_mm = this->wheel_radius_mm * (radians(left_diff_degrees) + radians(right_diff_degrees)); 52 | 53 | poseChange.dtheta_degrees = this->wheel_radius_mm / this->half_axle_length_mm * 54 | (right_diff_degrees - left_diff_degrees); 55 | 56 | poseChange.dt_seconds = timestamp_seconds_curr - timestamp_seconds_prev; 57 | } 58 | 59 | // Store current odometry for next time 60 | this->timestamp_seconds_prev = timestamp_seconds_curr; 61 | this->left_wheel_degrees_prev = left_wheel_degrees_curr; 62 | this->right_wheel_degrees_prev = right_wheel_degrees_curr; 63 | 64 | return poseChange; 65 | } 66 | -------------------------------------------------------------------------------- /examples/MinesRover.m: -------------------------------------------------------------------------------- 1 | classdef MinesRover < WheeledRobot 2 | %MinesRover Class for MinesRover custom robot 3 | % 4 | % Copyright (C) 2014 Simon D. Levy 5 | % 6 | % This code is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU Lesser General Public License as 8 | % published by the Free Software Foundation, either version 3 of the 9 | % License, or (at your option) any later version. 10 | % 11 | % This code is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU Lesser General Public License 17 | % along with this code. If not, see . 18 | 19 | properties (Access = 'private') 20 | TICKS_PER_CYCLE = 2000; 21 | end 22 | 23 | methods (Access = 'private') 24 | 25 | function degrees = ticks_to_degrees(obj, ticks) 26 | 27 | degrees = ticks * (180. / obj.TICKS_PER_CYCLE); 28 | 29 | end 30 | 31 | end 32 | 33 | methods (Access = 'public') 34 | 35 | function robot = MinesRover() 36 | 37 | robot = robot@WheeledRobot(77, 165); 38 | 39 | end 40 | 41 | function disp(obj) 42 | % Displays information about this MinesRover 43 | 44 | fprintf('<%s ticks_per_cycle=%d>\n', obj.str(), obj.TICKS_PER_CYCLE) 45 | 46 | end 47 | 48 | function [poseChange, obj] = computePoseChange(obj, odometry) 49 | 50 | [poseChange, obj] = computePoseChange@WheeledRobot(obj, odometry(1), odometry(2), odometry(3)); 51 | 52 | end 53 | 54 | function [timestampSeconds, leftWheelDegrees, rightWheelDegrees] = ... 55 | extractOdometry(obj, timestamp, leftWheel, rightWheel) 56 | 57 | % Convert microseconds to seconds 58 | timestampSeconds = timestamp / 1e6; 59 | 60 | % Convert ticks to angles 61 | leftWheelDegrees = obj.ticks_to_degrees(leftWheel); 62 | rightWheelDegrees = obj.ticks_to_degrees(rightWheel); 63 | 64 | end 65 | 66 | end 67 | 68 | end 69 | 70 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/components/Position.java: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * BreezySLAM: Simple, efficient SLAM in Java 4 | * 5 | * Position.java - Java code for Position class 6 | * 7 | * Copyright (C) 2014 Simon D. Levy 8 | * 9 | * This code is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU Lesser General Public License as 11 | * published by the Free Software Foundation, either version 3 of the 12 | * License, or (at your option) any later version. 13 | * 14 | * This code is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU Lesser General Public License 20 | * along with this code. If not, see . 21 | */ 22 | 23 | package edu.wlu.cs.levy.breezyslam.components; 24 | 25 | /** 26 | * A class representing the position of a robot. 27 | */ 28 | public class Position 29 | { 30 | 31 | /** 32 | * Distance of robot from left edge of map, in millimeters. 33 | */ 34 | public double x_mm; 35 | 36 | /** 37 | * Distance of robot from top edge of map, in millimeters. 38 | */ 39 | public double y_mm; 40 | 41 | /** 42 | * Clockwise rotation of robot with respect to three o'clock (east), in degrees. 43 | */ 44 | public double theta_degrees; 45 | 46 | /** 47 | * Constructs a new position. 48 | * @param x_mm X coordinate in millimeters 49 | * @param y_mm Y coordinate in millimeters 50 | * @param theta_degrees rotation angle in degrees 51 | */ 52 | public Position(double x_mm, double y_mm, double theta_degrees) 53 | { 54 | this.x_mm = x_mm; 55 | this.y_mm = y_mm; 56 | this.theta_degrees = theta_degrees; 57 | } 58 | 59 | /** 60 | * Constructs a new Position object by copying another. 61 | * @param the other Positon object 62 | */ 63 | 64 | public Position(Position position) 65 | { 66 | this.x_mm = position.x_mm; 67 | this.y_mm = position.y_mm; 68 | this.theta_degrees = position.theta_degrees; 69 | } 70 | 71 | /** 72 | * Returns a string representation of this Position object. 73 | */ 74 | public String toString() 75 | { 76 | String format = ""; 77 | 78 | return String.format(format, this.x_mm, this.y_mm, this.theta_degrees); 79 | 80 | } 81 | } 82 | -------------------------------------------------------------------------------- /cpp/PoseChange.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * PoseChange.hpp - C++ header for PoseChange class 4 | * 5 | * Copyright (C) 2014 Simon D. Levy 6 | 7 | * This code is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as 9 | * published by the Free Software Foundation, either version 3 of the 10 | * License, or (at your option) any later version. 11 | * 12 | * This code is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with this code. If not, see . 19 | */ 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | using namespace std; 27 | 28 | 29 | /** 30 | * A class representing the forward and angular poseChange of a robot. 31 | */ 32 | class PoseChange 33 | { 34 | friend class Scan; 35 | 36 | public: 37 | 38 | /** 39 | * Creates a new PoseChange object with specified poseChange. 40 | */ 41 | PoseChange(double dxy_mm, double dtheta_degrees, double dtSeconds) 42 | { 43 | this->dxy_mm = dxy_mm; 44 | this->dtheta_degrees = dtheta_degrees; 45 | this->dt_seconds = dtSeconds; 46 | } 47 | 48 | /** 49 | * Creates a new PoseChange object with zero poseChange. 50 | */ 51 | PoseChange(void) 52 | { 53 | this->dxy_mm = 0; 54 | this->dtheta_degrees = 0; 55 | this->dt_seconds = 0; 56 | } 57 | 58 | 59 | 60 | /** 61 | * Updates this PoseChange object. 62 | * @param dxy_mm new forward distance traveled in millimeters 63 | * @param dtheta_degrees new angular rotation in degrees 64 | * @param dtSeconds time in seconds since last poseChange 65 | */ 66 | void update(double dxy_mm, double dtheta_degrees, double dtSeconds) 67 | { 68 | double velocity_factor = (dtSeconds > 0) ? (1 / dtSeconds) : 0; 69 | 70 | this->dxy_mm = dxy_mm * velocity_factor; 71 | 72 | this->dtheta_degrees = dtheta_degrees * velocity_factor; 73 | } 74 | 75 | friend ostream& operator<< (ostream & out, PoseChange & poseChange) 76 | { 77 | char str[100]; 78 | 79 | sprintf(str, ". 19 | */ 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | using namespace std; 27 | 28 | 29 | /** 30 | * A class representing the position of a robot. 31 | */ 32 | 33 | class Position 34 | { 35 | friend class CoreSLAM; 36 | 37 | public: 38 | 39 | /** 40 | * Constructs a new position. 41 | * @param x_mm X coordinate in millimeters 42 | * @param y_mm Y coordinate in millimeters 43 | * @param theta_degrees rotation angle in degrees 44 | */ 45 | 46 | Position(double x_mm, double y_mm, double theta_degrees) 47 | { 48 | this->x_mm = x_mm; 49 | this->y_mm = y_mm; 50 | this->theta_degrees = theta_degrees; 51 | 52 | } 53 | 54 | /** 55 | * Empty position constructor. All values are set to zero. 56 | */ 57 | Position(void) 58 | { 59 | this->x_mm = 0; 60 | this->y_mm = 0; 61 | this->theta_degrees = 0; 62 | } 63 | 64 | 65 | friend ostream& operator<< (ostream & out, Position & position) 66 | { 67 | char str[100]; 68 | 69 | //sprintf(str, "", 70 | sprintf(str, "", 71 | position.x_mm, position.y_mm, position.theta_degrees); 72 | 73 | out << str; 74 | 75 | return out; 76 | } 77 | 78 | /** 79 | * Distance of robot from left edge of map, in millimeters. 80 | */ 81 | double x_mm; 82 | 83 | 84 | /** 85 | * Distance of robot from top edge of map, in millimeters. 86 | */ 87 | double y_mm; 88 | 89 | /** 90 | * Clockwise rotation of robot with respect to three o'clock (east), in degrees. 91 | */ 92 | double theta_degrees; 93 | }; 94 | -------------------------------------------------------------------------------- /cpp/Scan.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * BreezySLAM: Simple, efficient SLAM in C++ 4 | * 5 | * Scan.hpp - header for Scan class 6 | * 7 | * Copyright (C) 2014 Simon D. Levy 8 | 9 | * This code is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU Lesser General Public License as 11 | * published by the Free Software Foundation, either version 3 of the 12 | * License, or (at your option) any later version. 13 | * 14 | * This code is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU Lesser General Public License 20 | * along with this code. If not, see . 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | using namespace std; 29 | 30 | class PoseChange; 31 | class Laser; 32 | 33 | 34 | /** 35 | * A class for Lidar scans. 36 | */ 37 | class Scan 38 | { 39 | friend class Map; 40 | friend class CoreSLAM; 41 | friend class RMHC_SLAM; 42 | 43 | public: 44 | 45 | /** 46 | * Builds a Scan object. 47 | * @param laser laser parameters 48 | * 49 | */ 50 | Scan(Laser * laser); 51 | 52 | /** 53 | * Builds a Scan object. 54 | * @param laser laser parameters 55 | * @param span supports spanning laser scan to cover the space better. 56 | * 57 | */ 58 | Scan(Laser * laser, int span); 59 | 60 | 61 | /** 62 | * Deallocates this Scan object. 63 | * 64 | */ 65 | ~Scan(void); 66 | 67 | 68 | /** 69 | * Updates this Scan object with new values from a Lidar scan. 70 | * @param scanvals_mm scanned Lidar distance values in millimeters 71 | * @param hole_width_millimeters hole width in millimeters 72 | * 73 | */ 74 | void 75 | update( 76 | int * scanvals_mm, 77 | double hole_width_millimeters); 78 | 79 | 80 | /** 81 | * Updates this Scan object with new values from a Lidar scan. 82 | * @param scanvals_mm scanned Lidar distance values in millimeters 83 | * @param hole_width_millimeters hole width in millimeters 84 | * @param poseChange forward velocity and angular velocity of robot at scan time 85 | * 86 | */ 87 | void 88 | update( 89 | int * scanvals_mm, 90 | double hole_width_millimeters, 91 | PoseChange & poseChange); 92 | 93 | friend ostream& operator<< (ostream & out, Scan & scan); 94 | 95 | private: 96 | 97 | struct scan_t * scan; 98 | 99 | void init(Laser * laser, int span); 100 | }; 101 | 102 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c: -------------------------------------------------------------------------------- 1 | /* 2 | * jnibreezyslam_algorithms.c Java Native Interface code for BreezySLAM algorithms 3 | * 4 | * Copyright (C) 2014 Simon D. Levy 5 | * 6 | * This code is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU Lesser General Public License as 8 | * published by the Free Software Foundation, either version 3 of the 9 | * License, or (at your option) any later version. 10 | * 11 | * This code is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public License 17 | * along with this code. If not, see . 18 | */ 19 | 20 | 21 | #include "../../../../../../../c/random.h" 22 | #include "../jni_utils.h" 23 | 24 | #include 25 | 26 | 27 | // RMHC_SLAM methods ----------------------------------------------------------------------------------------- 28 | 29 | JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_algorithms_RMHCSLAM_init (JNIEnv *env, jobject thisobject, jint random_seed) 30 | { 31 | ptr_to_obj(env, thisobject, random_new(random_seed)); 32 | } 33 | 34 | JNIEXPORT jobject JNICALL Java_edu_wlu_cs_levy_breezyslam_algorithms_RMHCSLAM_positionSearch (JNIEnv *env, jobject thisobject, 35 | jobject startpos_object, 36 | jobject map_object, 37 | jobject scan_object, 38 | jdouble sigma_xy_mm, 39 | jdouble sigma_theta_degrees, 40 | jint max_search_iter) 41 | { 42 | position_t startpos; 43 | 44 | startpos.x_mm = get_double_field(env, startpos_object, "x_mm"); 45 | startpos.y_mm = get_double_field(env, startpos_object, "y_mm"); 46 | startpos.theta_degrees = get_double_field(env, startpos_object, "theta_degrees"); 47 | 48 | void * random = ptr_from_obj(env, thisobject); 49 | 50 | position_t newpos = 51 | rmhc_position_search( 52 | startpos, 53 | cmap_from_jmap(env, map_object), 54 | cscan_from_jscan(env, scan_object), 55 | sigma_xy_mm, 56 | sigma_theta_degrees, 57 | max_search_iter, 58 | random); 59 | 60 | jclass cls = (*env)->FindClass(env, "edu/wlu/cs/levy/breezyslam/components/Position"); 61 | 62 | jmethodID constructor = (*env)->GetMethodID(env, cls, "", "(DDD)V"); 63 | 64 | jobject newpos_object = (*env)->NewObject(env, cls, constructor, newpos.x_mm, newpos.y_mm, newpos.theta_degrees); 65 | 66 | return newpos_object; 67 | } 68 | -------------------------------------------------------------------------------- /python/breezyslam/sensors.py: -------------------------------------------------------------------------------- 1 | ''' 2 | BreezySLAM: Simple, efficient SLAM in Python 3 | 4 | sensors.py: SLAM sensors (currently just Laser) 5 | 6 | Copyright (C) 2014 Simon D. Levy 7 | 8 | This code is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU Lesser General Public License as 10 | published by the Free Software Foundation, either version 3 of the 11 | License, or (at your option) any later version. 12 | 13 | This code is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public License 19 | along with this code. If not, see . 20 | ''' 21 | 22 | class Laser(object): 23 | ''' 24 | A class representing the specifications of a scanning laser rangefinder (Lidar). 25 | ''' 26 | def __init__(self, scan_size, scan_rate_hz, detection_angle_degrees, distance_no_detection_mm, detection_margin=0, offset_mm=0): 27 | 28 | self.scan_size = scan_size 29 | self.scan_rate_hz = scan_rate_hz 30 | self.detection_angle_degrees = detection_angle_degrees 31 | self.distance_no_detection_mm = distance_no_detection_mm 32 | self.detection_margin = detection_margin 33 | self.offset_mm = offset_mm 34 | 35 | def __str__(self): 36 | 37 | return 'scan_size=%d | scan_rate=%3.3f hz | detection_angle=%3.3f deg | distance_no_detection=%7.4f mm | detection_margin=%d | offset=%4.4f m' % \ 38 | (self.scan_size, self.scan_rate_hz, self.detection_angle_degrees, self.distance_no_detection_mm, self.detection_margin, self.offset_mm) 39 | 40 | def __repr__(self): 41 | 42 | return str(self) 43 | 44 | 45 | class URG04LX(Laser): 46 | ''' 47 | A class for the Hokuyo URG-04LX 48 | ''' 49 | def __init__(self, detectionMargin = 0, offsetMillimeters = 0): 50 | 51 | Laser.__init__(self, 682, 10, 240, 4000, detectionMargin, offsetMillimeters) 52 | 53 | class XVLidar(Laser): 54 | ''' 55 | A class for the GetSurreal XVLidar 56 | ''' 57 | def __init__(self, detectionMargin = 0, offsetMillimeters = 0): 58 | 59 | Laser.__init__(self, 360, 5.5, 360, 6000, detectionMargin, offsetMillimeters) 60 | 61 | class RPLidarA1(Laser): 62 | ''' 63 | A class for the SLAMTEC RPLidar A1 64 | ''' 65 | def __init__(self, detectionMargin = 0, offsetMillimeters = 0): 66 | 67 | Laser.__init__(self, 360, 5.5, 360, 12000, detectionMargin, offsetMillimeters) 68 | 69 | -------------------------------------------------------------------------------- /c/coreslam_sisd.c: -------------------------------------------------------------------------------- 1 | /* 2 | coreslam_sisd.c SISD default for CoreSLAM when SSE not available. 3 | 4 | Adapted from code in CoreSLAM.c downloaded from openslam.org on 01 January 2014. 5 | 6 | Copyright (C) 2014 Simon D. Levy 7 | 8 | This code is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU Lesser General Public License as 10 | published by the Free Software Foundation, either version 3 of the 11 | License, or (at your option) any later version. 12 | 13 | This code is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public License 19 | along with this code. If not, see . 20 | 21 | */ 22 | 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "coreslam.h" 29 | #include "coreslam_internals.h" 30 | 31 | int 32 | distance_scan_to_map( 33 | map_t * map, 34 | scan_t * scan, 35 | position_t position) 36 | { 37 | /* Pre-compute sine and cosine of angle for rotation */ 38 | double position_theta_radians = radians(position.theta_degrees); 39 | double costheta = cos(position_theta_radians) * map->scale_pixels_per_mm; 40 | double sintheta = sin(position_theta_radians) * map->scale_pixels_per_mm; 41 | 42 | /* Pre-compute pixel offset for translation */ 43 | double pos_x_pix = position.x_mm * map->scale_pixels_per_mm; 44 | double pos_y_pix = position.y_mm * map->scale_pixels_per_mm; 45 | 46 | int64_t sum = 0; /* sum of map values at those points */ 47 | int npoints = 0; /* number of points where scan matches map */ 48 | 49 | int i = 0; 50 | for (i=0; inpoints; i++) 51 | { 52 | /* Consider only scan points representing obstacles */ 53 | if (scan->value[i] == OBSTACLE) 54 | { 55 | /* Translate and rotate scan point to robot position */ 56 | int x = floor(pos_x_pix + costheta * scan->x_mm[i] - sintheta * scan->y_mm[i] + 0.5); 57 | int y = floor(pos_y_pix + sintheta * scan->x_mm[i] + costheta * scan->y_mm[i] + 0.5); 58 | 59 | /* Add point if in map bounds */ 60 | if (x >= 0 && x < map->size_pixels && y >= 0 && y < map->size_pixels) 61 | { 62 | sum += map->pixels[y * map->size_pixels + x]; 63 | npoints++; 64 | } 65 | } 66 | } 67 | 68 | /* Return sum scaled by number of points, or -1 if none */ 69 | return npoints ? (int)(sum * 1024 / npoints) : -1; 70 | } 71 | -------------------------------------------------------------------------------- /matlab/Scan.m: -------------------------------------------------------------------------------- 1 | classdef Scan 2 | %A class for Lidar scans used in SLAM 3 | % 4 | % Copyright (C) 2014 Simon D. Levy 5 | % 6 | % This code is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU Lesser General Public License as 8 | % published by the Free Software Foundation, either version 3 of the 9 | % License, or (at your option) any later version. 10 | % 11 | % This code is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU Lesser General Public License 17 | % along with this code. If not, see . 18 | 19 | properties (Access = {?Map, ?RMHC_SLAM}) 20 | 21 | c_scan 22 | end 23 | 24 | methods 25 | 26 | function scan = Scan(laser, span) 27 | % Creates a new scan object 28 | % scan = Scan(laser, [span]) 29 | % laser is a structure with the fields: 30 | % scan_size 31 | % scan_rate_hz 32 | % detection_angle_degrees 33 | % distance_no_detection_mm 34 | % distance_no_detection_mm 35 | % detection_margin 36 | % offset_mm = offset_mm 37 | % span (default=1) supports spanning laser scan to cover the space better 38 | 39 | if nargin < 2 40 | span = 1; 41 | end 42 | 43 | scan.c_scan = mex_breezyslam('Scan_init', laser, span); 44 | 45 | end 46 | 47 | function disp(scan) 48 | % Displays information about this Scan 49 | 50 | mex_breezyslam('Scan_disp', scan.c_scan) 51 | 52 | end 53 | 54 | function update(scan, scans_mm, hole_width_mm, velocities) 55 | % Updates scan with new lidar and velocity data 56 | % update(scans_mm, hole_width_mm, [velocities]) 57 | % scans_mm is a list of integers representing scanned distances in mm. 58 | % hole_width_mm is the width of holes (obstacles, walls) in millimeters. 59 | % velocities is an optional list[dxy_mm, dtheta_degrees] 60 | % i.e., robot's (forward, rotational velocity) for improving the quality of the scan. 61 | mex_breezyslam('Scan_update', scan.c_scan, int32(scans_mm), hole_width_mm, velocities) 62 | end 63 | 64 | end 65 | 66 | end 67 | -------------------------------------------------------------------------------- /cpp/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile : builds libbreezyslam.so C++ library 2 | # 3 | # Copyright (C) Simon D. Levy 2014 4 | # 5 | # This code is free software: you can redistribute it and/or modify 6 | # it under the terms of the GNU Lesser General Public License as 7 | # published by the Free Software Foundation, either version 3 of the 8 | # License, or (at your option) any later version. 9 | # 10 | # This code is distributed in the hope that it will be useful, 11 | # but WITHOUT ANY WARRANTY without even the implied warranty of 12 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | # GNU General Public License for more details. 14 | # 15 | # You should have received a copy of the GNU Lesser General Public License 16 | # along with this code. If not, see . 17 | 18 | # Where you want to put the library 19 | LIBDIR = /usr/local/lib 20 | 21 | # Set library extension based on OS 22 | ifeq ("$(shell uname)","Darwin") 23 | LIBEXT = dylib 24 | else ifeq ("$(shell uname)","Linux") 25 | CFLAGS = -fPIC 26 | LIBEXT = so 27 | else 28 | LIBEXT = dll 29 | endif 30 | 31 | ARCH = $(shell uname -m) 32 | 33 | # Set SIMD compile params based on architecture 34 | ifeq ("$(ARCH)","armv7l") 35 | SIMD_FLAGS = -mfpu=neon 36 | else ifeq ("$(ARCH)","i686") 37 | SIMD_FLAGS = -msse3 38 | else 39 | ARCH = sisd 40 | endif 41 | 42 | all: libbreezyslam.$(LIBEXT) 43 | 44 | test: breezytest 45 | ./breezytest 46 | 47 | libbreezyslam.$(LIBEXT): algorithms.o Scan.o Map.o WheeledRobot.o \ 48 | coreslam.o coreslam_$(ARCH).o random.o ziggurat.o 49 | g++ -O3 -shared algorithms.o Scan.o Map.o WheeledRobot.o \ 50 | coreslam.o coreslam_$(ARCH).o random.o ziggurat.o \ 51 | -o libbreezyslam.$(LIBEXT) -lm 52 | 53 | algorithms.o: algorithms.cpp algorithms.hpp Laser.hpp Position.hpp Map.hpp Scan.hpp PoseChange.hpp \ 54 | WheeledRobot.hpp ../c/coreslam.h 55 | g++ -O3 -I../c -c -Wall $(CFLAGS) algorithms.cpp 56 | 57 | Scan.o: Scan.cpp Scan.hpp PoseChange.hpp Laser.hpp ../c/coreslam.h 58 | g++ -O3 -I../c -c -Wall $(CFLAGS) Scan.cpp 59 | 60 | Map.o: Map.cpp Map.hpp Position.hpp Scan.hpp ../c/coreslam.h 61 | g++ -O3 -I../c -c -Wall $(CFLAGS) Map.cpp 62 | 63 | WheeledRobot.o: WheeledRobot.cpp WheeledRobot.hpp 64 | g++ -O3 -I../c -c -Wall $(CFLAGS) WheeledRobot.cpp 65 | 66 | coreslam.o: ../c/coreslam.c ../c/coreslam.h 67 | gcc -O3 -c -Wall $(CFLAGS) ../c/coreslam.c 68 | 69 | coreslam_$(ARCH).o: ../c/coreslam_$(ARCH).c ../c/coreslam.h 70 | gcc -O3 -c -Wall $(CFLAGS) $(SIMD_FLAGS) ../c/coreslam_$(ARCH).c 71 | 72 | random.o: ../c/random.c 73 | gcc -O3 -c -Wall $(CFLAGS) ../c/random.c 74 | 75 | ziggurat.o: ../c/ziggurat.c 76 | gcc -O3 -c -Wall $(CFLAGS) ../c/ziggurat.c 77 | 78 | install: libbreezyslam.$(LIBEXT) 79 | cp libbreezyslam.$(LIBEXT) $(LIBDIR) 80 | 81 | doc: 82 | doxygen 83 | 84 | clean: 85 | rm -rf libbreezyslam.$(LIBEXT) *.o Documentation \#* *~ 86 | -------------------------------------------------------------------------------- /cpp/Scan.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * \mainpage BreezySLAM: Simple, efficient SLAM in C++ 4 | * 5 | * Scan.cpp - implementation for Scan class 6 | * 7 | * Copyright (C) 2014 Simon D. Levy 8 | 9 | * This code is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU Lesser General Public License as 11 | * published by the Free Software Foundation, either version 3 of the 12 | * License, or (at your option) any later version. 13 | * 14 | * This code is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU Lesser General Public License 20 | * along with this code. If not, see . 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | using namespace std; 31 | 32 | #include "coreslam.h" 33 | 34 | #include "PoseChange.hpp" 35 | #include "Laser.hpp" 36 | #include "Scan.hpp" 37 | 38 | /** 39 | * A class for Lidar scans. 40 | */ 41 | 42 | Scan::Scan(Laser * laser, int span) 43 | { 44 | this->init(laser, span); 45 | } 46 | 47 | Scan::Scan(Laser * laser) 48 | { 49 | this->init(laser, 1); 50 | } 51 | 52 | 53 | void Scan::init(Laser * laser, int span) 54 | { 55 | this->scan = new scan_t; 56 | 57 | scan_init( 58 | this->scan, 59 | span, 60 | laser->scan_size, 61 | laser->scan_rate_hz, 62 | laser->detection_angle_degrees, 63 | laser->distance_no_detection_mm, 64 | laser->detection_margin, 65 | laser->offset_mm); 66 | } 67 | 68 | Scan::~Scan(void) 69 | { 70 | scan_free(this->scan); 71 | delete this->scan; 72 | } 73 | 74 | 75 | void 76 | Scan::update( 77 | int * scanvals_mm, 78 | double hole_width_millimeters, 79 | PoseChange & poseChange) 80 | { 81 | scan_update( 82 | this->scan, 83 | NULL, // no support for angles/interpolation yet 84 | scanvals_mm, 85 | this->scan->size,// no support for angles/interpolation yet 86 | hole_width_millimeters, 87 | poseChange.dxy_mm, 88 | poseChange.dtheta_degrees); 89 | } 90 | void 91 | Scan::update( 92 | int * scanvals_mm, 93 | double hole_width_millimeters) 94 | { 95 | PoseChange zeroPoseChange; 96 | this->update(scanvals_mm, hole_width_millimeters, zeroPoseChange); 97 | } 98 | 99 | ostream& operator<< (ostream & out, Scan & scan) 100 | { 101 | char str[512]; 102 | 103 | scan_string(*scan.scan, str); 104 | 105 | out << str; 106 | 107 | return out; 108 | } 109 | 110 | 111 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/components/Map.java: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * BreezySLAM: Simple, efficient SLAM in Java 4 | * 5 | * Map.java - Java code for Map class 6 | * 7 | * Copyright (C) 2014 Simon D. Levy 8 | * 9 | * This code is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU Lesser General Public License as 11 | * published by the Free Software Foundation, either version 3 of the 12 | * License, or (at your option) any later version. 13 | * 14 | * This code is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU Lesser General Public License 20 | * along with this code. If not, see . 21 | */ 22 | 23 | package edu.wlu.cs.levy.breezyslam.components; 24 | 25 | /** 26 | * A class for maps used in SLAM. 27 | */ 28 | public class Map 29 | { 30 | static 31 | { 32 | System.loadLibrary("jnibreezyslam_components"); 33 | } 34 | 35 | private native void init(int size_pixels, double size_meters); 36 | 37 | private double size_meters; 38 | 39 | private native void update( 40 | Scan scan, 41 | double position_x_mm, 42 | double position_y_mm, 43 | double position_theta_degrees, 44 | int quality, 45 | double hole_width_mm); 46 | 47 | private long native_ptr; 48 | 49 | /** 50 | * Returns a string representation of this Map object. 51 | */ 52 | public native String toString(); 53 | 54 | /** 55 | * Builds a square Map object. 56 | * @param size_pixels size in pixels 57 | * @param size_meters size in meters 58 | * 59 | */ 60 | public Map(int size_pixels, double size_meters) 61 | { 62 | this.init(size_pixels, size_meters); 63 | 64 | // for public accessor 65 | this.size_meters = size_meters; 66 | } 67 | 68 | /** 69 | * Puts current map values into bytearray, which should of which should be of 70 | * this.size map_size_pixels ^ 2. 71 | * @param bytes byte array that gets the map values 72 | */ 73 | public native void get(byte [] bytes); 74 | 75 | /** 76 | * Updates this map object based on new data. 77 | * @param scan a new scan 78 | * @param position a new postion 79 | * @param quality speed with which scan is integerate into map (0 through 255) 80 | * @param hole_width_mm hole width in millimeters 81 | * 82 | */ 83 | public void update(Scan scan, Position position, int quality, double hole_width_mm) 84 | { 85 | this.update(scan, position.x_mm, position.y_mm, position.theta_degrees, quality, hole_width_mm); 86 | } 87 | 88 | /** 89 | * Returns the size of this map in meters. 90 | */ 91 | public double sizeMeters() 92 | { 93 | return this.size_meters; 94 | } 95 | 96 | } 97 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/components/Scan.java: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * BreezySLAM: Simple, efficient SLAM in Java 4 | * 5 | * Scan.java - Java code for Scan class 6 | * 7 | * Copyright (C) 2014 Simon D. Levy 8 | * 9 | * This code is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU Lesser General Public License as 11 | * published by the Free Software Foundation, either version 3 of the 12 | * License, or (at your option) any later version. 13 | * 14 | * This code is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU Lesser General Public License 20 | * along with this code. If not, see . 21 | */ 22 | 23 | package edu.wlu.cs.levy.breezyslam.components; 24 | 25 | /** 26 | * A class for Lidar scans. 27 | */ 28 | public class Scan 29 | { 30 | static 31 | { 32 | System.loadLibrary("jnibreezyslam_components"); 33 | } 34 | 35 | private native void init( 36 | int span, 37 | int scan_size, 38 | double scan_rate_hz, 39 | double detection_angle_degrees, 40 | double distance_no_detection_mm, 41 | int detection_margin, 42 | double offset_mm); 43 | 44 | private long native_ptr; 45 | 46 | public native String toString(); 47 | 48 | /** 49 | * Returns a string representation of this Scan object. 50 | */ 51 | public native void update( 52 | int [] lidar_mm, 53 | double hole_width_mm, 54 | double poseChange_dxy_mm, 55 | double poseChange_dtheta_degrees); 56 | 57 | 58 | /** 59 | * Builds a Scan object. 60 | * @param laser laser parameters 61 | * @param span supports spanning laser scan to cover the space better. 62 | * 63 | */ 64 | public Scan(Laser laser, int span) 65 | { 66 | this.init(span, 67 | laser.scan_size, 68 | laser.scan_rate_hz, 69 | laser.detection_angle_degrees, 70 | laser.distance_no_detection_mm, 71 | laser.detection_margin, 72 | laser.offset_mm); 73 | } 74 | 75 | /** 76 | * Builds a Scan object. 77 | * @param laser laser parameters 78 | * 79 | */ 80 | public Scan(Laser laser) 81 | { 82 | this(laser, 1); 83 | } 84 | 85 | /** 86 | * Updates this Scan object with new values from a Lidar scan. 87 | * @param scanvals_mm scanned Lidar distance values in millimeters 88 | * @param hole_width_millimeters hole width in millimeters 89 | * @param poseChange forward velocity and angular velocity of robot at scan time 90 | * 91 | */ 92 | public void update(int [] scanvals_mm, double hole_width_millimeters, PoseChange poseChange) 93 | { 94 | this.update(scanvals_mm, hole_width_millimeters, poseChange.dxy_mm, poseChange.dtheta_degrees); 95 | } 96 | } 97 | 98 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/components/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for BreezySLAM components in Java 2 | # 3 | # Copyright (C) 2014 Simon D. Levy 4 | # 5 | # This code is free software: you can redistribute it and/or modify 6 | # it under the terms of the GNU Lesser General Public License as 7 | # published by the Free Software Foundation, either version 3 of the 8 | # License, or (at your option) any later version. 9 | # 10 | # This code is distributed in the hope that it will be useful, 11 | # but WITHOUT ANY WARRANTY without even the implied warranty of 12 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | # GNU General Public License for more details. 14 | 15 | # You should have received a copy of the GNU Lesser General Public License 16 | # along with this code. If not, see . 17 | 18 | BASEDIR = ../../../../../../.. 19 | JAVADIR = $(BASEDIR)/java 20 | CDIR = $(BASEDIR)/c 21 | JFLAGS = -Xlint 22 | JDKINC = -I /usr/lib/jvm/java-7-openjdk-amd64/include 23 | #JDKINC = -I /opt/jdk1.7.0_67/include -I /opt/jdk1.7.0_67/include/linux 24 | 25 | # Set library extension based on OS 26 | ifeq ("$(shell uname)","Darwin") 27 | LIBEXT = dylib 28 | else ifeq ("$(shell uname)","Linux") 29 | CFLAGS = -fPIC 30 | LIBEXT = so 31 | else 32 | LIBEXT = dll 33 | endif 34 | 35 | ARCH = $(shell uname -m) 36 | 37 | # Set SIMD compile params based on architecture 38 | ifeq ("$(ARCH)","armv7l") 39 | SIMD_FLAGS = -mfpu=neon 40 | else ifeq ("$(ARCH)","i686") 41 | SIMD_FLAGS = -msse3 42 | else 43 | ARCH = sisd 44 | endif 45 | 46 | ALL = libjnibreezyslam_components.$(LIBEXT) Laser.class Position.class PoseChange.class URG04LX.class 47 | 48 | all: $(ALL) 49 | 50 | libjnibreezyslam_components.$(LIBEXT): jnibreezyslam_components.o coreslam.o coreslam_$(ARCH).o 51 | gcc -shared -Wl,-soname,libjnibreezyslam_components.so -o libjnibreezyslam_components.so jnibreezyslam_components.o \ 52 | coreslam.o coreslam_$(ARCH).o 53 | 54 | jnibreezyslam_components.o: jnibreezyslam_components.c Map.h Scan.h ../jni_utils.h 55 | gcc $(JDKINC) -fPIC -c jnibreezyslam_components.c 56 | 57 | coreslam.o: $(CDIR)/coreslam.c $(CDIR)/coreslam.h 58 | gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/coreslam.c 59 | 60 | coreslam_$(ARCH).o: $(CDIR)/coreslam_$(ARCH).c $(CDIR)/coreslam.h 61 | gcc -O3 -c -Wall $(CFLAGS) $(SIMD_FLAGS) $(CDIR)/coreslam_$(ARCH).c 62 | 63 | Map.h: Map.class 64 | javah -o Map.h -classpath $(JAVADIR) -jni edu.wlu.cs.levy.breezyslam.components.Map 65 | 66 | Map.class: Map.java Scan.class Position.class 67 | javac $(JFLAGS) -classpath $(JAVADIR) Map.java 68 | 69 | Scan.h: Scan.class 70 | javah -o Scan.h -classpath $(JAVADIR) -jni edu.wlu.cs.levy.breezyslam.components.Scan 71 | 72 | Scan.class: Scan.java Laser.class 73 | javac $(JFLAGS) -classpath $(JAVADIR) Scan.java 74 | 75 | Laser.class: Laser.java 76 | javac $(JFLAGS) Laser.java 77 | 78 | URG04LX.class: URG04LX.java Laser.class 79 | javac $(JFLAGS) -classpath $(JAVADIR) URG04LX.java 80 | 81 | PoseChange.class: PoseChange.java 82 | javac $(JFLAGS) PoseChange.java 83 | 84 | 85 | Position.class: Position.java 86 | javac $(JFLAGS) Position.java 87 | 88 | clean: 89 | rm -f *.so *.class *.o *.h *~ 90 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for BreezySLAM algorithms in Java 2 | # 3 | # Copyright (C) 2014 Simon D. Levy 4 | # 5 | # This code is free software: you can redistribute it and/or modify 6 | # it under the terms of the GNU Lesser General Public License as 7 | # published by the Free Software Foundation, either version 3 of the 8 | # License, or (at your option) any later version. 9 | # 10 | # This code is distributed in the hope that it will be useful, 11 | # but WITHOUT ANY WARRANTY without even the implied warranty of 12 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | # GNU General Public License for more details. 14 | 15 | # You should have received a copy of the GNU Lesser General Public License 16 | # along with this code. If not, see . 17 | 18 | BASEDIR = ../../../../../../.. 19 | JAVADIR = $(BASEDIR)/java 20 | CDIR = $(BASEDIR)/c 21 | JFLAGS = -Xlint 22 | JDKINC = -I /usr/lib/jvm/java-7-openjdk-amd64/include 23 | #JDKINC = -I /opt/jdk1.7.0_67/include -I /opt/jdk1.7.0_67/include/linux 24 | 25 | # Set library extension based on OS 26 | ifeq ("$(shell uname)","Darwin") 27 | LIBEXT = dylib 28 | else ifeq ("$(shell uname)","Linux") 29 | CFLAGS = -fPIC 30 | LIBEXT = so 31 | else 32 | LIBEXT = dll 33 | endif 34 | 35 | # Set SIMD compile params based on architecture 36 | ifeq ("$(ARCH)","armv7l") 37 | SIMD_FLAGS = -mfpu=neon 38 | else ifeq ("$(ARCH)","i686") 39 | SIMD_FLAGS = -msse3 40 | else 41 | ARCH = sisd 42 | endif 43 | 44 | 45 | ALL = libjnibreezyslam_algorithms.$(LIBEXT) CoreSLAM.class SinglePositionSLAM.class DeterministicSLAM.class RMHCSLAM.class 46 | 47 | all: $(ALL) 48 | 49 | libjnibreezyslam_algorithms.$(LIBEXT): jnibreezyslam_algorithms.o coreslam.o random.o ziggurat.o coreslam_$(ARCH).o 50 | gcc -shared -Wl,-soname,libjnibreezyslam_algorithms.so -o libjnibreezyslam_algorithms.so jnibreezyslam_algorithms.o \ 51 | coreslam.o coreslam_$(ARCH).o random.o ziggurat.o 52 | 53 | jnibreezyslam_algorithms.o: jnibreezyslam_algorithms.c RMHCSLAM.h ../jni_utils.h 54 | gcc $(JDKINC) -fPIC -c jnibreezyslam_algorithms.c 55 | 56 | 57 | CoreSLAM.class: CoreSLAM.java 58 | javac -classpath $(JAVADIR):. CoreSLAM.java 59 | 60 | 61 | SinglePositionSLAM.class: SinglePositionSLAM.java 62 | javac -classpath $(JAVADIR):. SinglePositionSLAM.java 63 | 64 | DeterministicSLAM.class: DeterministicSLAM.java 65 | javac -classpath $(JAVADIR):. DeterministicSLAM.java 66 | 67 | RMHCSLAM.class: RMHCSLAM.java 68 | javac -classpath $(JAVADIR):. RMHCSLAM.java 69 | 70 | RMHCSLAM.h: RMHCSLAM.class 71 | javah -o RMHCSLAM.h -classpath $(JAVADIR) -jni edu.wlu.cs.levy.breezyslam.algorithms.RMHCSLAM 72 | 73 | coreslam.o: $(CDIR)/coreslam.c $(CDIR)/coreslam.h 74 | gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/coreslam.c 75 | 76 | coreslam_$(ARCH).o: $(CDIR)/coreslam_$(ARCH).c $(CDIR)/coreslam.h 77 | gcc -O3 -c -Wall $(CFLAGS) $(SIMD_FLAGS) $(CDIR)/coreslam_$(ARCH).c 78 | 79 | random.o: $(CDIR)/random.c 80 | gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/random.c 81 | 82 | ziggurat.o: $(CDIR)/ziggurat.c 83 | gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/ziggurat.c 84 | 85 | clean: 86 | rm -f *.class *.o *.h *.$(LIBEXT) *~ 87 | 88 | backup: 89 | cp *.java bak 90 | cp Makefile bak 91 | -------------------------------------------------------------------------------- /matlab/RMHC_SLAM.m: -------------------------------------------------------------------------------- 1 | classdef RMHC_SLAM < SinglePositionSLAM 2 | %RMHC_SLAM Random Mutation Hill-Climbing SLAM 3 | % Implements the getNewPosition() method of SinglePositionSLAM 4 | % using Random-Mutation Hill-Climbing search. Uses its own internal 5 | % pseudorandom-number generator for efficiency. 6 | % 7 | % Copyright (C) 2014 Simon D. Levy 8 | % 9 | % This code is free software: you can redistribute it and/or modify 10 | % it under the terms of the GNU Lesser General Public License as 11 | % published by the Free Software Foundation, either version 3 of the 12 | % License, or (at your option) any later version. 13 | % 14 | % This code is distributed in the hope that it will be useful, 15 | % but WITHOUT ANY WARRANTY without even the implied warranty of 16 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | % GNU General Public License for more details. 18 | % 19 | % You should have received a copy of the GNU Lesser General Public License 20 | % along with this code. If not, see . 21 | 22 | properties (Access = 'public') 23 | sigma_xy_mm = 100; % std. dev. of X/Y component of search 24 | sigma_theta_degrees = 20; % std. dev. of angular component of search 25 | max_search_iter = 1000; % max. # of search iterations per update 26 | end 27 | 28 | properties (Access = 'private') 29 | c_randomizer 30 | end 31 | 32 | methods 33 | 34 | function slam = RMHC_SLAM(laser, map_size_pixels, map_size_meters, random_seed) 35 | %Creates an RMHC_SLAM object suitable for updating with new Lidar and odometry data. 36 | % slam = RMHC_SLAM(laser, map_size_pixels, map_size_meters, random_seed) 37 | % laser is a Laser object representing the specifications of your Lidar unit 38 | % map_size_pixels is the size of the square map in pixels 39 | % map_size_meters is the size of the square map in meters 40 | % random_seed supports reproducible results; defaults to system time if unspecified 41 | 42 | slam = slam@SinglePositionSLAM(laser, map_size_pixels, map_size_meters); 43 | 44 | if nargin < 3 45 | random_seed = floor(cputime) & hex2dec('FFFF'); 46 | end 47 | 48 | slam.c_randomizer = mex_breezyslam('Randomizer_init', random_seed); 49 | 50 | end 51 | 52 | function new_pos = getNewPosition(slam, start_pos) 53 | % Implements the _getNewPosition() method of SinglePositionSLAM. 54 | % Uses Random-Mutation Hill-Climbing search to look for a 55 | % better position based on a starting position. 56 | 57 | [new_pos.x_mm,new_pos.y_mm,new_pos.theta_degrees] = ... 58 | mex_breezyslam('rmhcPositionSearch', ... 59 | start_pos, ... 60 | slam.map.c_map, ... 61 | slam.scan_for_distance.c_scan, ... 62 | slam.laser,... 63 | slam.sigma_xy_mm,... 64 | slam.sigma_theta_degrees,... 65 | slam.max_search_iter,... 66 | slam.c_randomizer); 67 | end 68 | 69 | end 70 | 71 | end 72 | 73 | -------------------------------------------------------------------------------- /examples/rpslam.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ''' 4 | rpslam.py : BreezySLAM Python with SLAMTECH RP A1 Lidar 5 | 6 | Copyright (C) 2018 Simon D. Levy 7 | 8 | This code is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU Lesser General Public License as 10 | published by the Free Software Foundation, either version 3 of the 11 | License, or (at your option) any later version. 12 | 13 | This code is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public License 19 | along with this code. If not, see . 20 | ''' 21 | 22 | MAP_SIZE_PIXELS = 500 23 | MAP_SIZE_METERS = 10 24 | LIDAR_DEVICE = '/dev/ttyUSB0' 25 | 26 | 27 | # Ideally we could use all 250 or so samples that the RPLidar delivers in one 28 | # scan, but on slower computers you'll get an empty map and unchanging position 29 | # at that rate. 30 | MIN_SAMPLES = 200 31 | 32 | from breezyslam.algorithms import RMHC_SLAM 33 | from breezyslam.sensors import RPLidarA1 as LaserModel 34 | from rplidar import RPLidar as Lidar 35 | from roboviz import MapVisualizer 36 | 37 | if __name__ == '__main__': 38 | 39 | # Connect to Lidar unit 40 | lidar = Lidar(LIDAR_DEVICE) 41 | 42 | # Create an RMHC SLAM object with a laser model and optional robot model 43 | slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) 44 | 45 | # Set up a SLAM display 46 | viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM') 47 | 48 | # Initialize an empty trajectory 49 | trajectory = [] 50 | 51 | # Initialize empty map 52 | mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) 53 | 54 | # Create an iterator to collect scan data from the RPLidar 55 | iterator = lidar.iter_scans() 56 | 57 | # We will use these to store previous scan in case current scan is inadequate 58 | previous_distances = None 59 | previous_angles = None 60 | 61 | # First scan is crap, so ignore it 62 | next(iterator) 63 | 64 | while True: 65 | 66 | # Extract (quality, angle, distance) triples from current scan 67 | items = [item for item in next(iterator)] 68 | 69 | # Extract distances and angles from triples 70 | distances = [item[2] for item in items] 71 | angles = [item[1] for item in items] 72 | 73 | # Update SLAM with current Lidar scan and scan angles if adequate 74 | if len(distances) > MIN_SAMPLES: 75 | slam.update(distances, scan_angles_degrees=angles) 76 | previous_distances = distances.copy() 77 | previous_angles = angles.copy() 78 | 79 | # If not adequate, use previous 80 | elif previous_distances is not None: 81 | slam.update(previous_distances, scan_angles_degrees=previous_angles) 82 | 83 | # Get current robot position 84 | x, y, theta = slam.getpos() 85 | 86 | # Get current map bytes as grayscale 87 | slam.getmap(mapbytes) 88 | 89 | # Display map and robot pose, exiting gracefully if user closes it 90 | if not viz.display(x/1000., y/1000., theta, mapbytes): 91 | exit(0) 92 | 93 | # Shut down the lidar connection 94 | lidar.stop() 95 | lidar.disconnect() 96 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/components/PoseChange.java: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * BreezySLAM: Simple, efficient SLAM in Java 4 | * 5 | * PoseChange.java - Java code for PoseChange class, encoding triple 6 | * (dxy_mm, dtheta_degrees, dt_seconds) 7 | * 8 | * Copyright (C) 2014 Simon D. Levy 9 | * 10 | * This code is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU Lesser General Public License as 12 | * published by the Free Software Foundation, either version 3 of the 13 | * License, or (at your option) any later version. 14 | * 15 | * This code is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU Lesser General Public License 21 | * along with this code. If not, see . 22 | */ 23 | 24 | package edu.wlu.cs.levy.breezyslam.components; 25 | 26 | /** 27 | * A class representing the forward and angular velocities of a robot as determined by odometry. 28 | */ 29 | public class PoseChange 30 | { 31 | 32 | /** 33 | * Creates a new PoseChange object with specified velocities. 34 | */ 35 | public PoseChange(double dxy_mm, double dtheta_degrees, double dtSeconds) 36 | { 37 | this.dxy_mm = dxy_mm; 38 | this.dtheta_degrees = dtheta_degrees; 39 | this.dt_seconds = dtSeconds; 40 | } 41 | 42 | /** 43 | * Creates a new PoseChange object with zero velocities. 44 | */ 45 | public PoseChange() 46 | { 47 | this.dxy_mm = 0; 48 | this.dtheta_degrees = 0; 49 | this.dt_seconds = 0; 50 | } 51 | 52 | /** 53 | * Updates this PoseChange object. 54 | * @param dxy_mm new forward distance traveled in millimeters 55 | * @param dtheta_degrees new angular rotation in degrees 56 | * @param dtSeconds time in seconds since last velocities 57 | */ 58 | public void update(double dxy_mm, double dtheta_degrees, double dtSeconds) 59 | { 60 | double velocity_factor = (dtSeconds > 0) ? (1 / dtSeconds) : 0; 61 | 62 | this.dxy_mm = dxy_mm * velocity_factor; 63 | 64 | this.dtheta_degrees = dtheta_degrees * velocity_factor; 65 | } 66 | 67 | /** 68 | * Returns a string representation of this PoseChange object. 69 | */ 70 | public String toString() 71 | { 72 | return String.format(". 20 | ''' 21 | 22 | MAP_SIZE_PIXELS = 500 23 | MAP_SIZE_METERS = 10 24 | LIDAR_DEVICE = '/dev/ttyUSB0' 25 | 26 | # Ideally we could use all 250 or so samples that the RPLidar delivers in one 27 | # scan, but on slower computers you'll get an empty map and unchanging position 28 | # at that rate. 29 | MIN_SAMPLES = 180 30 | 31 | from breezyslam.algorithms import RMHC_SLAM 32 | from breezyslam.sensors import RPLidarA1 as LaserModel 33 | 34 | from rplidar import RPLidar as Lidar 35 | 36 | from roboviz import MapVisualizer 37 | 38 | from scipy.interpolate import interp1d 39 | import numpy as np 40 | 41 | if __name__ == '__main__': 42 | 43 | # Connect to Lidar unit 44 | lidar = Lidar(LIDAR_DEVICE) 45 | 46 | # Create an RMHC SLAM object with a laser model and optional robot model 47 | slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) 48 | 49 | # Set up a SLAM display 50 | viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM') 51 | 52 | # Initialize an empty trajectory 53 | trajectory = [] 54 | 55 | # Initialize empty map 56 | mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) 57 | 58 | # Create an iterator to collect scan data from the RPLidar 59 | iterator = lidar.iter_scans() 60 | 61 | # We will use this to store previous scan in case current scan is inadequate 62 | previous_distances = None 63 | 64 | # First scan is crap, so ignore it 65 | next(iterator) 66 | 67 | while True: 68 | 69 | # Extrat (quality, angle, distance) triples from current scan 70 | items = [item for item in next(iterator)] 71 | 72 | # Extract distances and angles from triples 73 | distances = [item[2] for item in items] 74 | angles = [item[1] for item in items] 75 | 76 | print(len(distances)) 77 | 78 | # Update SLAM with current Lidar scan and scan angles if adequate 79 | if len(distances) > MIN_SAMPLES: 80 | 81 | # First interpolate to get 360 angles from 0 through 359, and corresponding distances 82 | f = interp1d(angles, distances, fill_value='extrapolate') 83 | distances = list(f(np.arange(360))) # slam.update wants a list 84 | 85 | # Then update with interpolated distances 86 | slam.update(distances) 87 | 88 | # Store interplated distances for next time 89 | previous_distances = distances.copy() 90 | 91 | # If not adequate, use previous 92 | elif previous_distances is not None: 93 | slam.update(previous_distances) 94 | 95 | # Get current robot position 96 | x, y, theta = slam.getpos() 97 | 98 | # Get current map bytes as grayscale 99 | slam.getmap(mapbytes) 100 | 101 | # Display map and robot pose, exiting gracefully if user closes it 102 | if not viz.display(x/1000., y/1000., theta, mapbytes): 103 | exit(0) 104 | 105 | # Shut down the lidar connection 106 | lidar.stop() 107 | lidar.disconnect() 108 | -------------------------------------------------------------------------------- /python/pyextension_utils.c: -------------------------------------------------------------------------------- 1 | /* 2 | pyextension_utils.c : C extension utilities for Python 3 | 4 | Copyright (C) 2014 Simon D. Levy 5 | 6 | This code is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as 8 | published by the Free Software Foundation, either version 3 of the 9 | License, or (at your option) any later version. 10 | 11 | This code is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with this code. If not, see . 18 | */ 19 | 20 | #include 21 | 22 | static void raise_argument_exception(const char * classname, const char * methodname, const char * details) 23 | { 24 | char errmsg[1000]; 25 | sprintf(errmsg, "Wrong/insufficient arguments passed to %s.%s()%s %s", 26 | classname, methodname, 27 | details ? ":" : "", details ? details : ""); 28 | PyErr_SetString(PyExc_TypeError, errmsg); 29 | 30 | } 31 | 32 | PyObject * null_on_raise_argument_exception_with_details(const char * classname, const char * methodname, const char * details) 33 | { 34 | raise_argument_exception(classname, methodname, details); 35 | return NULL; 36 | } 37 | 38 | PyObject *null_on_raise_argument_exception(const char * classname, const char * methodname) 39 | { 40 | return null_on_raise_argument_exception_with_details(classname, methodname, NULL); 41 | } 42 | 43 | int error_on_raise_argument_exception_with_details(const char * classname, const char * methodname, const char * details) 44 | { 45 | raise_argument_exception(classname, methodname, details); 46 | return -1; 47 | } 48 | 49 | int error_on_raise_argument_exception(const char * classname) 50 | { 51 | return error_on_raise_argument_exception_with_details(classname, "__init__", NULL); 52 | } 53 | 54 | int type_is_ready(PyTypeObject * type) 55 | { 56 | return (PyType_Ready(type) >= 0); 57 | } 58 | 59 | 60 | void add_class(PyObject * module, PyTypeObject * type, const char * classname) 61 | { 62 | Py_INCREF(type); 63 | PyModule_AddObject(module, classname, (PyObject *)type); 64 | 65 | } 66 | 67 | int double_from_tuple(PyObject * tup, int pos, double * val) 68 | { 69 | PyObject * py_val = PyTuple_GetItem(tup, pos); 70 | 71 | if (py_val) 72 | { 73 | *val = PyFloat_AsDouble(py_val); 74 | return PyErr_Occurred() ? 0 : 1; 75 | } 76 | 77 | return 0; 78 | } 79 | 80 | 81 | int 82 | double_from_obj( 83 | PyObject * obj, 84 | const char * name, 85 | double * val) 86 | { 87 | PyObject * attr = PyObject_GetAttrString(obj, name); 88 | 89 | if (!attr) 90 | { 91 | return 0; 92 | } 93 | 94 | *val = PyFloat_AsDouble(attr); 95 | 96 | return 1; 97 | } 98 | 99 | 100 | int 101 | int_from_obj( 102 | PyObject * obj, 103 | const char * name, 104 | int * val) 105 | { 106 | PyObject * attr = PyObject_GetAttrString(obj, name); 107 | 108 | if (!attr) 109 | { 110 | return 0; 111 | } 112 | 113 | *val = PyLong_AsLong(attr); 114 | 115 | return 1; 116 | } 117 | 118 | 119 | int 120 | error_on_check_argument_type( 121 | PyObject * obj, 122 | PyTypeObject * typ, 123 | int pos, 124 | const char * typname, 125 | const char * classname, 126 | const char *methodname) 127 | { 128 | 129 | if (!PyObject_TypeCheck(obj, typ)) 130 | { 131 | char details[200]; 132 | sprintf(details, "argument %d is not of type %s", pos+1, typname); 133 | raise_argument_exception(classname, methodname, details); 134 | return -1; 135 | } 136 | 137 | return 0; 138 | } 139 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java: -------------------------------------------------------------------------------- 1 | /** 2 | * RMHCSLAM implements SinglePositionSLAM using random-mutation hill-climbing search on the starting position. 3 | * 4 | * Copyright (C) 2014 Simon D. Levy 5 | * 6 | * This code is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU Lesser General Public License as 8 | * published by the Free Software Foundation, either version 3 of the 9 | * License, or (at your option) any later version. 10 | * 11 | * This code is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public License 17 | * along with this code. If not, see . 18 | */ 19 | 20 | package edu.wlu.cs.levy.breezyslam.algorithms; 21 | 22 | import edu.wlu.cs.levy.breezyslam.components.Position; 23 | import edu.wlu.cs.levy.breezyslam.components.Laser; 24 | import edu.wlu.cs.levy.breezyslam.components.PoseChange; 25 | import edu.wlu.cs.levy.breezyslam.components.Map; 26 | import edu.wlu.cs.levy.breezyslam.components.Scan; 27 | 28 | public class RMHCSLAM extends SinglePositionSLAM 29 | { 30 | private static final double DEFAULT_SIGMA_XY_MM = 100; 31 | private static final double DEFAULT_SIGMA_THETA_DEGREES = 20; 32 | private static final int DEFAULT_MAX_SEARCH_ITER = 1000; 33 | 34 | static 35 | { 36 | System.loadLibrary("jnibreezyslam_algorithms"); 37 | } 38 | 39 | private native void init(int random_seed); 40 | 41 | private native Object positionSearch( 42 | Position start_pos, 43 | Map map, 44 | Scan scan, 45 | double sigma_xy_mm, 46 | double sigma_theta_degrees, 47 | int max_search_iter); 48 | 49 | 50 | private long native_ptr; 51 | 52 | /** 53 | * Creates an RMHCSLAM object. 54 | * @param laser a Laser object containing parameters for your Lidar equipment 55 | * @param map_size_pixels the size of the desired map (map is square) 56 | * @param map_size_meters the size of the area to be mapped, in meters 57 | * @param random_seed seed for psuedorandom number generator in particle filter 58 | * @return a new CoreSLAM object 59 | */ 60 | public RMHCSLAM(Laser laser, int map_size_pixels, double map_size_meters, int random_seed) 61 | { 62 | super(laser, map_size_pixels, map_size_meters); 63 | 64 | this.init(random_seed); 65 | } 66 | 67 | /** 68 | * The standard deviation in millimeters of the Gaussian distribution of 69 | * the (X,Y) component of position in the particle filter; default = 100 70 | */ 71 | public double sigma_xy_mm = DEFAULT_SIGMA_XY_MM;; 72 | 73 | /** 74 | * The standard deviation in degrees of the Gaussian distribution of 75 | * the angular rotation component of position in the particle filter; default = 20 76 | */ 77 | public double sigma_theta_degrees = DEFAULT_SIGMA_THETA_DEGREES; 78 | 79 | /** 80 | * The maximum number of iterations for particle-filter search; default = 1000 81 | */ 82 | public int max_search_iter = DEFAULT_MAX_SEARCH_ITER; 83 | 84 | /** 85 | * Returns a new position based on RMHC search from a starting position. Called automatically by 86 | * SinglePositionSLAM::updateMapAndPointcloud() 87 | * @param start_position the starting position 88 | */ 89 | protected Position getNewPosition(Position start_position) 90 | { 91 | Position newpos = 92 | (Position)positionSearch( 93 | start_position, 94 | this.map, 95 | this.scan_for_distance, 96 | this.sigma_xy_mm, 97 | this.sigma_theta_degrees, 98 | this.max_search_iter); 99 | 100 | return newpos; 101 | } 102 | 103 | } // RMHCSLAM 104 | -------------------------------------------------------------------------------- /c/coreslam_i686.c: -------------------------------------------------------------------------------- 1 | /* 2 | coreslam_i686.c Intel Streaming SIMD Extensions for CoreSLAM 3 | 4 | Based on 5 | 6 | @InProceedings{, 7 | author={El Hamzaoui, O. and Steux, B.}, 8 | title={A fast scan matching for grid-based laser SLAM using streaming SIMD extensions}, 9 | booktitle={Control Automation Robotics Vision (ICARCV), 2010 11th International Conference on}, 10 | year={2010}, 11 | month={Dec}, 12 | pages={1986-1990} 13 | } 14 | 15 | Copyright (C) 2014 Simon D. Levy 16 | 17 | This code is free software: you can redistribute it and/or modify 18 | it under the terms of the GNU Lesser General Public License as 19 | published by the Free Software Foundation, either version 3 of the 20 | License, or (at your option) any later version. 21 | 22 | This code is distributed in the hope that it will be useful, 23 | but WITHOUT ANY WARRANTY without even the implied warranty of 24 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | GNU General Public License for more details. 26 | 27 | You should have received a copy of the GNU Lesser General Public License 28 | along with this code. If not, see . 29 | 30 | */ 31 | 32 | 33 | #ifdef _MSC_VER 34 | typedef __int64 int64_t; /* Define it from MSVC's internal type */ 35 | #else 36 | #include /* Use the C99 official header */ 37 | #endif 38 | 39 | #include 40 | 41 | #include "coreslam.h" 42 | #include "coreslam_internals.h" 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | /* This structure supports extracting two 32-bit integer coordinates from a 64-bit register */ 49 | typedef union 50 | { 51 | struct 52 | { 53 | int y; 54 | int x; 55 | 56 | } pos; 57 | 58 | __m64 mmx; 59 | 60 | } cs_pos_mmx_t; 61 | 62 | 63 | int 64 | distance_scan_to_map( 65 | map_t * map, 66 | scan_t * scan, 67 | position_t position) 68 | { 69 | int npoints = 0; /* number of points where scan matches map */ 70 | int64_t sum = 0; /* sum of map values at those points */ 71 | 72 | /* Pre-compute sine and cosine of angle for rotation */ 73 | double position_theta_radians = radians(position.theta_degrees); 74 | double costheta = cos(position_theta_radians) * map->scale_pixels_per_mm; 75 | double sintheta = sin(position_theta_radians) * map->scale_pixels_per_mm; 76 | 77 | /* Pre-compute pixel offset for translation */ 78 | double pos_x_pix = position.x_mm * map->scale_pixels_per_mm; 79 | double pos_y_pix = position.y_mm * map->scale_pixels_per_mm; 80 | 81 | __m128 sincos128 = _mm_set_ps (costheta, -sintheta, sintheta, costheta); 82 | __m128 posxy128 = _mm_set_ps (pos_x_pix, pos_y_pix, pos_x_pix, pos_y_pix); 83 | 84 | int i = 0; 85 | for (i=0; inpoints; i++) 86 | { 87 | /* Consider only scan points representing obstacles */ 88 | if (scan->value[i] == OBSTACLE) 89 | { 90 | /* Compute coordinate pair using SSE */ 91 | __m128 xy128 = _mm_set_ps (scan->x_mm[i], scan->y_mm[i], scan->x_mm[i], scan->y_mm[i]); 92 | xy128 = _mm_mul_ps(sincos128, xy128); 93 | xy128 = _mm_hadd_ps(xy128, xy128); 94 | xy128 = _mm_add_ps(xy128, posxy128); 95 | cs_pos_mmx_t pos; 96 | pos.mmx = _mm_cvtps_pi32(xy128); 97 | 98 | /* Extract coordinates */ 99 | int x = pos.pos.x; 100 | int y = pos.pos.y; 101 | 102 | /* Empty the multimedia state to avoid floating-point errors later */ 103 | _mm_empty(); 104 | 105 | /* Add point if in map bounds */ 106 | if (x >= 0 && x < map->size_pixels && y >= 0 && y < map->size_pixels) 107 | { 108 | sum += map->pixels[y * map->size_pixels + x]; 109 | npoints++; 110 | } 111 | } 112 | } 113 | 114 | /* Return sum scaled by number of points, or -1 if none */ 115 | return npoints ? (int)(sum * 1024 / npoints) : -1; 116 | } 117 | 118 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/components/Laser.java: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * BreezySLAM: Simple, efficient SLAM in Java 4 | * 5 | * Laser.java - Java code for Laser model class 6 | * 7 | * Copyright (C) 2014 Simon D. Levy 8 | * 9 | * This code is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU Lesser General Public License as 11 | * published by the Free Software Foundation, either version 3 of the 12 | * License, or (at your option) any later version. 13 | * 14 | * This code is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU Lesser General Public License 20 | * along with this code. If not, see . 21 | */ 22 | 23 | package edu.wlu.cs.levy.breezyslam.components; 24 | 25 | /** 26 | * A class for scanning laser rangefinder (Lidar) parameters. 27 | */ 28 | public class Laser 29 | { 30 | 31 | protected int scan_size; 32 | protected double scan_rate_hz; 33 | protected double detection_angle_degrees; 34 | protected double distance_no_detection_mm; 35 | protected int detection_margin; 36 | protected double offset_mm; 37 | 38 | /** 39 | * Builds a Laser object from parameters based on the specifications for your 40 | * Lidar unit. 41 | * @param scan_size number of rays per scan 42 | * @param scan_rate_hz laser scan rate in Hertz 43 | * @param detection_angle_degrees detection angle in degrees (e.g. 240, 360) 44 | * @param detection_margin number of rays at edges of scan to ignore 45 | * @param offset_mm forward/backward offset of laser motor from robot center 46 | * @return a new Laser object 47 | * 48 | */ 49 | public Laser( 50 | int scan_size, 51 | double scan_rate_hz, 52 | double detection_angle_degrees, 53 | double distance_no_detection_mm, 54 | int detection_margin, 55 | double offset_mm) 56 | { 57 | this.scan_size = scan_size; 58 | this.scan_rate_hz = scan_rate_hz; 59 | this.detection_angle_degrees = detection_angle_degrees; 60 | this.distance_no_detection_mm = distance_no_detection_mm; 61 | this.detection_margin = detection_margin; 62 | this.offset_mm = offset_mm; 63 | } 64 | 65 | /** 66 | * Builds a Laser object by copying another Laser object. 67 | * Lidar unit. 68 | * @param laser the other Laser object 69 | * 70 | */ 71 | public Laser(Laser laser) 72 | { 73 | this.scan_size = laser.scan_size; 74 | this.scan_rate_hz = laser.scan_rate_hz; 75 | this.detection_angle_degrees = laser.detection_angle_degrees; 76 | this.distance_no_detection_mm = laser.distance_no_detection_mm; 77 | this.detection_margin = laser.detection_margin; 78 | this.offset_mm = laser.offset_mm; 79 | } 80 | 81 | /** 82 | * Returns a string representation of this Laser object. 83 | */ 84 | 85 | public String toString() 86 | { 87 | String format = "scan_size=%d | scan_rate=%3.3f hz | " + 88 | "detection_angle=%3.3f deg | " + 89 | "distance_no_detection=%7.4f mm | " + 90 | "detection_margin=%d | offset=%4.4f m"; 91 | 92 | return String.format(format, this.scan_size, this.scan_rate_hz, 93 | this.detection_angle_degrees, 94 | this.distance_no_detection_mm, 95 | this.detection_margin, 96 | this.offset_mm); 97 | 98 | } 99 | 100 | /** 101 | * Returns the offset of the laser in mm, from the center of the robot. 102 | * 103 | */ 104 | public double getOffsetMm() 105 | { 106 | return this.offset_mm; 107 | } 108 | } 109 | -------------------------------------------------------------------------------- /c/coreslam_armv7l.c: -------------------------------------------------------------------------------- 1 | /* 2 | coreslam_armv7l.c.c ARM Cortex Neon acceleration for CoreSLAM 3 | 4 | Copyright (C) 2014 by Simon D. Levy 5 | 6 | This code is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as 8 | published by the Free Software Foundation, either version 3 of the 9 | License, or (at your option) any later version. 10 | 11 | This code is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with this code. If not, see . 18 | */ 19 | 20 | 21 | #ifdef _MSC_VER 22 | typedef __int64 int64_t; /* Define it from MSVC's internal type */ 23 | #else 24 | #include /* Use the C99 official header */ 25 | #endif 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | #include "coreslam.h" 33 | #include "coreslam_internals.h" 34 | 35 | /* Performs one rotation/translation */ 36 | static void 37 | neon_coord_4( 38 | float32x4_t a_4, 39 | float32x4_t b_4, 40 | float32x4_t x_4, 41 | float32x4_t y_4, 42 | float32x4_t pos_4f, 43 | float32x4_t point5_4, 44 | int * result) 45 | { 46 | float32x4_t tmp1 = vmulq_f32(a_4, x_4); 47 | float32x4_t tmp2 = vmulq_f32(b_4, y_4); 48 | tmp2 = vaddq_f32(tmp1, tmp2); 49 | tmp2 = vaddq_f32(tmp2, pos_4f); 50 | tmp2 = vaddq_f32(tmp2, point5_4); 51 | int32x4_t c_4 = vcvtq_s32_f32(tmp2); 52 | vst1q_s32(result, c_4); 53 | } 54 | 55 | 56 | int 57 | distance_scan_to_map( 58 | map_t * map, 59 | scan_t * scan, 60 | position_t position) 61 | { 62 | /* Pre-compute sine and cosine of angle for rotation */ 63 | double position_theta_radians = radians(position.theta_degrees); 64 | double costheta = cos(position_theta_radians) * map->scale_pixels_per_mm; 65 | double sintheta = sin(position_theta_radians) * map->scale_pixels_per_mm; 66 | 67 | /* Pre-compute pixel offset for translation */ 68 | double pos_x_pix = position.x_mm * map->scale_pixels_per_mm; 69 | double pos_y_pix = position.y_mm * map->scale_pixels_per_mm; 70 | 71 | 72 | float32x4_t half_4 = vdupq_n_f32(0.5); 73 | 74 | float32x4_t costheta_4 = vdupq_n_f32(costheta); 75 | float32x4_t sintheta_4 = vdupq_n_f32(sintheta); 76 | float32x4_t nsintheta_4 = vdupq_n_f32(-sintheta); 77 | 78 | float32x4_t pos_x_4 = vdupq_n_f32(pos_x_pix); 79 | float32x4_t pos_y_4 = vdupq_n_f32(pos_y_pix); 80 | 81 | int npoints = 0; /* number of points where scan matches map */ 82 | int64_t sum = 0; 83 | 84 | /* Stride by 4 over obstacle points in scan */ 85 | int i = 0; 86 | for (i=0; iobst_npoints; i+=4) 87 | { 88 | /* Duplicate current obstacle point X and Y in 128-bit registers */ 89 | float32x4_t scan_x_4 = vld1q_f32(&scan->obst_x_mm[i]); 90 | float32x4_t scan_y_4 = vld1q_f32(&scan->obst_y_mm[i]); 91 | 92 | /* Compute X coordinate of 4 rotated / translated points at once */ 93 | int xarr[4]; 94 | neon_coord_4(costheta_4, nsintheta_4, scan_x_4, scan_y_4, pos_x_4, half_4, xarr); 95 | 96 | /* Compute Y coordinate of 4 rotated / translated points at once */ 97 | int yarr[4]; 98 | neon_coord_4(sintheta_4, costheta_4, scan_x_4, scan_y_4, pos_y_4, half_4, yarr); 99 | 100 | /* Handle rotated/translated points serially */ 101 | int j; 102 | for (j=0; j<4 && (i+j)obst_npoints; ++j) 103 | { 104 | int x = xarr[j]; 105 | int y = yarr[j]; 106 | 107 | /* Add point if in map bounds */ 108 | if (x >= 0 && x < map->size_pixels && y >= 0 && y < map->size_pixels) 109 | { 110 | sum += map->pixels[y * map->size_pixels + x]; 111 | npoints++; 112 | } 113 | } 114 | } 115 | 116 | return npoints ? (int)(sum * 1024 / npoints) : -1; 117 | } 118 | -------------------------------------------------------------------------------- /examples/mines.py: -------------------------------------------------------------------------------- 1 | ''' 2 | mines.py - classes for the SLAM apparatus used at Paris Mines Tech 3 | 4 | For details see 5 | 6 | @inproceedings{coreslam-2010, 7 | author = {Bruno Steux and Oussama El Hamzaoui}, 8 | title = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code}, 9 | booktitle = {11th International Conference on Control, Automation, 10 | Vehicleics and Vision, ICARCV 2010, Singapore, 7-10 11 | December 2010, Proceedings}, 12 | pages = {1975-1979}, 13 | publisher = {IEEE}, 14 | year = {2010} 15 | } 16 | 17 | Copyright (C) 2014 Simon D. Levy 18 | 19 | This code is free software: you can redistribute it and/or modify 20 | it under the terms of the GNU Lesser General Public License as 21 | published by the Free Software Foundation, either version 3 of the 22 | License, or (at your option) any later version. 23 | 24 | This code is distributed in the hope that it will be useful, 25 | but WITHOUT ANY WARRANTY without even the implied warranty of 26 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 27 | GNU General Public License for more details. 28 | 29 | You should have received a copy of the GNU Lesser General Public License 30 | along with this code. If not, see . 31 | ''' 32 | 33 | from breezyslam.vehicles import WheeledVehicle 34 | from breezyslam.sensors import URG04LX 35 | 36 | 37 | 38 | # Method to load all from file ------------------------------------------------ 39 | # Each line in the file has the format: 40 | # 41 | # TIMESTAMP ... Q1 Q1 ... Distances 42 | # (usec) (mm) 43 | # 0 ... 2 3 ... 24 ... 44 | # 45 | #where Q1, Q2 are odometry values 46 | 47 | def load_data(datadir, dataset): 48 | 49 | filename = '%s/%s.dat' % (datadir, dataset) 50 | print('Loading data from %s...' % filename) 51 | 52 | fd = open(filename, 'rt') 53 | 54 | timestamps = [] 55 | scans = [] 56 | odometries = [] 57 | 58 | while True: 59 | 60 | s = fd.readline() 61 | 62 | if len(s) == 0: 63 | break 64 | 65 | toks = s.split()[0:-1] # ignore '' 66 | 67 | timestamp = int(toks[0]) 68 | 69 | odometry = timestamp, int(toks[2]), int(toks[3]) 70 | 71 | lidar = [int(tok) for tok in toks[24:]] 72 | 73 | timestamps.append(timestamp) 74 | scans.append(lidar) 75 | odometries.append(odometry) 76 | 77 | fd.close() 78 | 79 | return timestamps, scans, odometries 80 | 81 | class MinesLaser(URG04LX): 82 | 83 | def __init__(self): 84 | 85 | URG04LX.__init__(self, 70, 145) 86 | 87 | # Class for MinesRover custom robot ------------------------------------------ 88 | 89 | class Rover(WheeledVehicle): 90 | 91 | def __init__(self): 92 | 93 | WheeledVehicle.__init__(self, 77, 165) 94 | 95 | self.ticks_per_cycle = 2000 96 | 97 | def __str__(self): 98 | 99 | return '<%s ticks_per_cycle=%d>' % (WheeledVehicle.__str__(self), self.ticks_per_cycle) 100 | 101 | def computePoseChange(self, odometry): 102 | 103 | return WheeledVehicle.computePoseChange(self, odometry[0], odometry[1], odometry[2]) 104 | 105 | def extractOdometry(self, timestamp, leftWheel, rightWheel): 106 | 107 | # Convert microseconds to seconds, ticks to angles 108 | return timestamp / 1e6, \ 109 | self._ticks_to_degrees(leftWheel), \ 110 | self._ticks_to_degrees(rightWheel) 111 | 112 | def odometryStr(self, odometry): 113 | 114 | return '' % \ 115 | (odometry[0], odometry[1], odometry[2]) 116 | 117 | def _ticks_to_degrees(self, ticks): 118 | 119 | return ticks * (180. / self.ticks_per_cycle) 120 | 121 | 122 | 123 | 124 | 125 | -------------------------------------------------------------------------------- /matlab/SinglePositionSLAM.m: -------------------------------------------------------------------------------- 1 | classdef SinglePositionSLAM < CoreSLAM 2 | %SinglePositionSLAM Abstract class for CoreSLAM with single-point cloud 3 | % Implementing classes should provide the method 4 | % 5 | % getNewPosition(self, start_position) 6 | % 7 | % to compute a new position based on searching from a starting position. 8 | % 9 | % Copyright (C) 2014 Simon D. Levy 10 | % 11 | % This code is free software: you can redistribute it and/or modify 12 | % it under the terms of the GNU Lesser General Public License as 13 | % published by the Free Software Foundation, either version 3 of the 14 | % License, or (at your option) any later version. 15 | % 16 | % This code is distributed in the hope that it will be useful, 17 | % but WITHOUT ANY WARRANTY without even the implied warranty of 18 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | % GNU General Public License for more details. 20 | % 21 | % You should have received a copy of the GNU Lesser General Public License 22 | % along with this code. If not, see . 23 | 24 | properties (Access = 'private') 25 | position 26 | end 27 | 28 | methods (Abstract) 29 | getNewPosition(slam, start_pos) 30 | end 31 | 32 | methods (Access = 'private') 33 | 34 | function c = costheta(slam) 35 | c = cosd(slam.position.theta_degrees); 36 | end 37 | 38 | function s = sintheta(slam) 39 | s = sind(slam.position.theta_degrees); 40 | end 41 | 42 | end 43 | 44 | methods (Access = 'public') 45 | 46 | function slam = SinglePositionSLAM(laser, map_size_pixels, map_size_meters) 47 | 48 | slam = slam@CoreSLAM(laser, map_size_pixels, map_size_meters); 49 | 50 | % Initialize the position (x, y, theta) 51 | init_coord_mm = 500 * map_size_meters; % center of map 52 | slam.position.x_mm = init_coord_mm; 53 | slam.position.y_mm = init_coord_mm; 54 | slam.position.theta_degrees = 0; 55 | 56 | end 57 | 58 | function [x_mm, y_mm, theta_degrees] = getpos(slam) 59 | % Returns the current position. 60 | % [x_mm, y_mm, theta_degrees] = getpos(slam) 61 | 62 | x_mm = slam.position.x_mm; 63 | y_mm = slam.position.y_mm; 64 | theta_degrees = slam.position.theta_degrees; 65 | end 66 | 67 | end 68 | 69 | methods (Access = 'protected') 70 | 71 | function slam = updateMapAndPointcloud(slam, velocities) 72 | 73 | % Start at current position 74 | start_pos = slam.position; 75 | 76 | % Add effect of velocities 77 | start_pos.x_mm = start_pos.x_mm + velocities(1) * slam.costheta(); 78 | start_pos.y_mm = start_pos.y_mm + velocities(1) * slam.sintheta(); 79 | start_pos.theta_degrees = start_pos.theta_degrees + velocities(2); 80 | 81 | % Add offset from laser 82 | start_pos.x_mm = start_pos.x_mm + slam.laser.offset_mm * slam.costheta(); 83 | start_pos.y_mm = start_pos.y_mm + slam.laser.offset_mm * slam.sintheta(); 84 | 85 | % Get new position from implementing class 86 | new_position = slam.getNewPosition(start_pos); 87 | 88 | % Update the map with this new position 89 | slam.map.update(slam.scan_for_mapbuild, new_position, slam.map_quality, slam.hole_width_mm) 90 | 91 | % Update the current position with this new position, adjusted by laser offset 92 | slam.position = new_position; 93 | slam.position.x_mm = slam.position.x_mm - slam.laser.offset_mm * slam.costheta(); 94 | slam.position.y_mm = slam.position.y_mm - slam.laser.offset_mm * slam.sintheta(); 95 | 96 | end 97 | 98 | end 99 | 100 | end 101 | 102 | -------------------------------------------------------------------------------- /python/breezyslam/vehicles.py: -------------------------------------------------------------------------------- 1 | ''' 2 | BreezySLAM: Simple, efficient SLAM in Python 3 | 4 | vehicles.py: odometry models for different kinds of vehicles 5 | (currently just wheeled vehicles) 6 | 7 | Copyright (C) 2014 Suraj Bajracharya and Simon D. Levy 8 | 9 | This code is free software: you can redistribute it and/or modify 10 | it under the terms of the GNU Lesser General Public License as 11 | published by the Free Software Foundation, either version 3 of the 12 | License, or (at your option) any later version. 13 | 14 | This code is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | GNU General Public License for more details. 18 | 19 | You should have received a copy of the GNU Lesser General Public License 20 | along with this code. If not, see . 21 | ''' 22 | 23 | import math 24 | 25 | class WheeledVehicle(object): 26 | ''' 27 | An abstract class supporting ododmetry for wheeled robots. Your implementing 28 | class should provide the method: 29 | 30 | extractOdometry(self, timestamp, leftWheel, rightWheel) --> 31 | (timestampSeconds, leftWheelDegrees, rightWheelDegrees) 32 | ''' 33 | 34 | def __init__(self, wheelRadiusMillimeters, halfAxleLengthMillimeters): 35 | ''' 36 | wheelRadiusMillimeters radius of each odometry wheel 37 | halfAxleLengthMillimeters half the length of the axle between the odometry wheels 38 | ''' 39 | self.wheelRadiusMillimeters = wheelRadiusMillimeters 40 | self.halfAxleLengthMillimeters = halfAxleLengthMillimeters 41 | 42 | self.timestampSecondsPrev = None 43 | self.leftWheelDegreesPrev = None 44 | self.rightWheelDegreesPrev = None 45 | 46 | def __str__(self): 47 | 48 | return '' % \ 49 | (self.wheelRadiusMillimeters, self.halfAxleLengthMillimeters) 50 | 51 | def __repr__(self): 52 | 53 | return self.__str__() 54 | 55 | def computePoseChange(self, timestamp, leftWheelOdometry, rightWheelOdometry): 56 | ''' 57 | Computes pose change based on odometry. 58 | 59 | Parameters: 60 | 61 | timestamp time stamp, in whatever units your robot uses 62 | leftWheelOdometry odometry for left wheel, in whatever form your robot uses 63 | rightWheelOdometry odometry for right wheel, in whatever form your robot uses 64 | 65 | Returns a tuple (dxyMillimeters, dthetaDegrees, dtSeconds) 66 | 67 | dxyMillimeters forward distance traveled, in millimeters 68 | dthetaDegrees change in angular position, in degrees 69 | dtSeconds elapsed time since previous odometry, in seconds 70 | ''' 71 | dxyMillimeters = 0 72 | dthetaDegrees = 0 73 | dtSeconds = 0 74 | 75 | timestampSecondsCurr, leftWheelDegreesCurr, rightWheelDegreesCurr = \ 76 | self.extractOdometry(timestamp, leftWheelOdometry, rightWheelOdometry) 77 | 78 | if self.timestampSecondsPrev != None: 79 | 80 | leftDiffDegrees = leftWheelDegreesCurr - self.leftWheelDegreesPrev 81 | rightDiffDegrees = rightWheelDegreesCurr - self.rightWheelDegreesPrev 82 | 83 | dxyMillimeters = self.wheelRadiusMillimeters * \ 84 | (math.radians(leftDiffDegrees) + math.radians(rightDiffDegrees)) 85 | 86 | dthetaDegrees = (float(self.wheelRadiusMillimeters) / self.halfAxleLengthMillimeters) * \ 87 | (rightDiffDegrees - leftDiffDegrees) 88 | 89 | dtSeconds = timestampSecondsCurr - self.timestampSecondsPrev 90 | 91 | # Store current odometry for next time 92 | self.timestampSecondsPrev = timestampSecondsCurr 93 | self.leftWheelDegreesPrev = leftWheelDegreesCurr 94 | self.rightWheelDegreesPrev = rightWheelDegreesCurr 95 | 96 | # Return linear velocity, angular velocity, time difference 97 | return dxyMillimeters, dthetaDegrees, dtSeconds 98 | 99 | -------------------------------------------------------------------------------- /cpp/WheeledRobot.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * BreezySLAM: Simple, efficient SLAM in C++ 4 | * 5 | * WheeledRobot.hpp - C++ header for WheeledRobot class 6 | * 7 | * Copyright (C) 2014 Simon D. Levy 8 | 9 | * This code is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU Lesser General Public License as 11 | * published by the Free Software Foundation, either version 3 of the 12 | * License, or (at your option) any later version. 13 | * 14 | * This code is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU Lesser General Public License 20 | * along with this code. If not, see . 21 | */ 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | using namespace std; 29 | 30 | class PoseChange; 31 | 32 | /** 33 | * An abstract class for wheeled robots. Supports computation of forward and angular 34 | * poseChange based on odometry. Your subclass should should implement the 35 | * extractOdometry method. 36 | */ 37 | class WheeledRobot 38 | { 39 | 40 | protected: 41 | 42 | /** 43 | * Builds a WheeledRobot object. Parameters should be based on the specifications for 44 | * your robot. 45 | * @param wheel_radius_mm radius of each odometry wheel, in meters 46 | * @param half_axle_length_mm half the length of the axle between the odometry wheels, in meters 47 | * @return a new WheeledRobot object 48 | */ 49 | WheeledRobot(double wheel_radius_mm, double half_axle_length_mm) 50 | { 51 | this->wheel_radius_mm = wheel_radius_mm; 52 | this->half_axle_length_mm = half_axle_length_mm; 53 | 54 | this->timestamp_seconds_prev = 0; 55 | this->left_wheel_degrees_prev = 0; 56 | this->right_wheel_degrees_prev = 0; 57 | } 58 | 59 | /** 60 | * Computes forward and angular poseChange based on odometry. 61 | * @param timestamp time stamp, in whatever units your robot uses 62 | * @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses 63 | * @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses 64 | * @return poseChange object representing poseChange for these odometry values 65 | */ 66 | 67 | PoseChange 68 | computePoseChange( 69 | double timestamp, 70 | double left_wheel_odometry, 71 | double right_wheel_odometry); 72 | 73 | /** 74 | * Extracts usable odometry values from your robot's odometry. 75 | * @param timestamp time stamp, in whatever units your robot uses 76 | * @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses 77 | * @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses 78 | * @param timestamp_seconds gets time stamp in seconds 79 | * @param left_wheel_degrees gets left wheel rotation in degrees 80 | * @param right_wheel_degrees gets right wheel rotation in degrees 81 | */ 82 | virtual void extractOdometry( 83 | double timestamp, 84 | double left_wheel_odometry, 85 | double right_wheel_odometry, 86 | double & timestamp_seconds, 87 | double & left_wheel_degrees, 88 | double & right_wheel_degrees) = 0; 89 | 90 | friend ostream& operator<< (ostream & out, WheeledRobot & robot) 91 | { 92 | 93 | char subclassStr[100]; 94 | robot.descriptorString(subclassStr); 95 | 96 | char str[200]; 97 | sprintf(str, "", 98 | robot.wheel_radius_mm, robot.half_axle_length_mm, subclassStr); 99 | 100 | out << str; 101 | 102 | return out; 103 | } 104 | 105 | 106 | /** 107 | * Gets a descriptor string for your robot. 108 | * @param str gets the descriptor string 109 | */ 110 | virtual void descriptorString(char * str) = 0; 111 | 112 | private: 113 | 114 | double wheel_radius_mm; 115 | double half_axle_length_mm; 116 | 117 | double timestamp_seconds_prev; 118 | double left_wheel_degrees_prev; 119 | double right_wheel_degrees_prev; 120 | 121 | static double radians(double degrees) 122 | { 123 | return degrees * M_PI / 180; 124 | } 125 | 126 | 127 | }; 128 | -------------------------------------------------------------------------------- /python/pyextension_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | pyextension_utils.h : header for Python C extension utilities 3 | 4 | Copyright (C) 2014 Simon D. Levy 5 | 6 | This code is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as 8 | published by the Free Software Foundation, either version 3 of the 9 | License, or (at your option) any later version. 10 | 11 | This code is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with this code. If not, see . 18 | */ 19 | 20 | 21 | /** 22 | * Raises a bad-argument exception and returns NULL 23 | * @param classname name of class in which exception happened 24 | * @param methodname name of method in which exception happened 25 | * @param details details of exception 26 | * @return NULL 27 | */ 28 | 29 | PyObject * 30 | null_on_raise_argument_exception_with_details( 31 | const char * classname, 32 | const char * methodname, 33 | const char * details); 34 | 35 | /** 36 | * Raises a bad-argument exception and returns NULL 37 | * @param classname name of class in which exception happened 38 | * @param methodname name of method in which exception happened 39 | * @return NULL 40 | */ 41 | PyObject * 42 | null_on_raise_argument_exception( 43 | const char * classname, 44 | const char * methodname); 45 | 46 | /** 47 | * Raises a bad-argument exception and returns -1 48 | * @param classname name of class in which exception happened 49 | * @param methodname name of method in which exception happened 50 | * @param details details of exception 51 | * @return -1 52 | */ 53 | int 54 | error_on_raise_argument_exception_with_details( 55 | const char * classname, 56 | const char * methodname, 57 | const char * details); 58 | 59 | /** 60 | * Raises a bad-argument exception and returns -1 on __init__. 61 | * @param classname name of class in which exception happened 62 | * @param methodname name of method in which exception happened 63 | * @return -1 64 | */ 65 | int 66 | error_on_raise_argument_exception( 67 | const char * classname); 68 | 69 | /** 70 | * Returns 1 if type is ready, 0 otherwise 71 | * @param type the type object 72 | * @return 1 if type is ready, 0 otherwise 73 | */ 74 | int 75 | type_is_ready( 76 | PyTypeObject * type); 77 | 78 | /** 79 | * Adds a class to a module. 80 | * @param module the module 81 | * @param type the type object 82 | * @param name the class name 83 | */ 84 | void 85 | add_class( 86 | PyObject * module, 87 | PyTypeObject * type, 88 | const char * classname); 89 | 90 | /** 91 | * Extracts a double from a tuple. 92 | * @param tup the tuple 93 | * @param pos position in the tuple 94 | * @param val gets the returned value 95 | * @return true on sucess, false on failure 96 | */ 97 | int 98 | double_from_tuple( 99 | PyObject * tup, 100 | int pos, 101 | double * val); 102 | 103 | /** 104 | * Extracts a named double from an object. 105 | * @param obj the obj 106 | * @param name the name 107 | * @param val gets the returned value 108 | * @return true on sucess, false on failure 109 | */ 110 | int 111 | double_from_obj( 112 | PyObject * obj, 113 | const char * name, 114 | double * val); 115 | 116 | /** 117 | * Extracts a named int from an object. 118 | * @param obj the obj 119 | * @param name the name 120 | * @param val gets the returned value 121 | * @return true on sucess, false on failure 122 | */ 123 | int 124 | int_from_obj( 125 | PyObject * obj, 126 | const char * name, 127 | int * val); 128 | 129 | 130 | /** 131 | * Raises a bad-argument exception and returns -1 on passing of incorrent argument type 132 | * @param classname name of class in which exception happened 133 | * @param methodname name of method in which exception happened 134 | * @param typname name of expected type 135 | * @return -1 on error; 0 on success 136 | */ 137 | int 138 | error_on_check_argument_type( 139 | PyObject * obj, 140 | PyTypeObject * typ, 141 | int pos, 142 | const char * typname, 143 | const char * classname, 144 | const char *methodname); 145 | 146 | 147 | -------------------------------------------------------------------------------- /cpp/Laser.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Laser.hpp - C++ headers for Laser model classes 4 | * 5 | * Copyright (C) 2014 Simon D. Levy 6 | 7 | * This code is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as 9 | * published by the Free Software Foundation, either version 3 of the 10 | * License, or (at your option) any later version. 11 | * 12 | * This code is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with this code. If not, see . 19 | */ 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | using namespace std; 27 | 28 | 29 | /** 30 | * A class for scanning laser rangefinder (Lidar) parameters. 31 | */ 32 | class Laser 33 | { 34 | friend class CoreSLAM; 35 | friend class SinglePositionSLAM; 36 | friend class RMHC_SLAM; 37 | friend class Scan; 38 | 39 | protected: 40 | 41 | int scan_size; /* number of points per scan */ 42 | double scan_rate_hz; /* scans per second */ 43 | double detection_angle_degrees; /* e.g. 240, 360 */ 44 | double distance_no_detection_mm; /* default value when the laser returns 0 */ 45 | int detection_margin; /* first scan element to consider */ 46 | double offset_mm; /* position of the laser wrt center of rotation */ 47 | 48 | public: 49 | 50 | /** 51 | * Builds a Laser object from parameters based on the specifications for your 52 | * Lidar unit. 53 | * @param scan_size number of rays per scan 54 | * @param scan_rate_hz laser scan rate in Hertz 55 | * @param detection_angle_degrees detection angle in degrees (e.g. 240, 360) 56 | * @param detection_margin number of rays at edges of scan to ignore 57 | * @param offset_mm forward/backward offset of laser motor from robot center 58 | * @return a new Laser object 59 | * 60 | */ 61 | Laser( 62 | int scan_size, 63 | float scan_rate_hz, 64 | float detection_angle_degrees, 65 | float distance_no_detection_mm, 66 | int detection_margin = 0, 67 | float offset_mm = 0. 68 | ) 69 | 70 | { 71 | this->scan_size = scan_size; 72 | this->scan_rate_hz = scan_rate_hz; 73 | this->detection_angle_degrees = detection_angle_degrees; 74 | this->distance_no_detection_mm = distance_no_detection_mm; 75 | this->detection_margin = detection_margin; 76 | this->offset_mm = offset_mm; 77 | } 78 | 79 | /** 80 | * Builds an empty Laser object (all parameters zero). 81 | */ 82 | Laser(void); 83 | 84 | friend ostream& operator<< (ostream & out, Laser & laser) 85 | { 86 | char str[512]; 87 | 88 | sprintf(str, 89 | "", 93 | laser.scan_size, laser.scan_rate_hz, 94 | laser.detection_angle_degrees, 95 | laser.distance_no_detection_mm, 96 | laser.detection_margin, 97 | laser.offset_mm); 98 | 99 | out << str; 100 | 101 | return out; 102 | } 103 | }; 104 | 105 | /** 106 | * A class for the Hokuyo URG-04LX laser. 107 | */ 108 | class URG04LX : public Laser 109 | { 110 | 111 | public: 112 | 113 | /** 114 | * Builds a URG04LX object. 115 | * Lidar unit. 116 | * @param detection_margin number of rays at edges of scan to ignore 117 | * @param offset_mm forward/backward offset of laser motor from robot center 118 | * @return a new URG04LX object 119 | * 120 | */ 121 | URG04LX(int detection_margin = 0, float offset_mm = 0) : 122 | Laser(682, 10, 240, 4000, detection_margin, offset_mm) { } 123 | 124 | /** 125 | * Builds an empty URG04LX object (all parameters zero). 126 | */ 127 | URG04LX(void) : Laser() {} 128 | 129 | }; 130 | 131 | -------------------------------------------------------------------------------- /examples/log2pkl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ''' 4 | log2pkl.py : BreezySLAM Python demo. Reads logfile with odometry and scan data 5 | from Paris Mines Tech and pickles the map file for loading by another 6 | program 7 | 8 | Copyright (C) 2014 Simon D. Levy 9 | 10 | This code is free software: you can redistribute it and/or modify 11 | it under the terms of the GNU Lesser General Public License as 12 | published by the Free Software Foundation, either version 3 of the 13 | License, or (at your option) any later version. 14 | 15 | This code is distributed in the hope that it will be useful, 16 | but WITHOUT ANY WARRANTY without even the implied warranty of 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | GNU General Public License for more details. 19 | 20 | You should have received a copy of the GNU Lesser General Public License 21 | along with this code. If not, see . 22 | ''' 23 | 24 | # Map size, scale 25 | MAP_SIZE_PIXELS = 800 26 | MAP_SIZE_METERS = 32 27 | 28 | from breezyslam.algorithms import Deterministic_SLAM, RMHC_SLAM 29 | 30 | from mines import MinesLaser, Rover, load_data 31 | from progressbar import ProgressBar 32 | 33 | from sys import argv, exit, stdout 34 | from time import time 35 | import pickle 36 | 37 | def main(): 38 | 39 | # Bozo filter for input args 40 | if len(argv) < 3: 41 | print('Usage: %s [random_seed]' % argv[0]) 42 | print('Example: %s exp2 1 9999' % argv[0]) 43 | exit(1) 44 | 45 | # Grab input args 46 | dataset = argv[1] 47 | use_odometry = True if int(argv[2]) else False 48 | seed = int(argv[3]) if len(argv) > 3 else 0 49 | 50 | # Load the data from the file, ignoring timestamps 51 | _, lidars, odometries = load_data('.', dataset) 52 | 53 | # Build a robot model if we want odometry 54 | robot = Rover() if use_odometry else None 55 | 56 | # Create a CoreSLAM object with laser params and optional robot object 57 | slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \ 58 | if seed \ 59 | else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) 60 | 61 | # Report what we're doing 62 | nscans = len(lidars) 63 | print('Processing %d scans with%s odometry / with%s particle filter...' % \ 64 | (nscans, \ 65 | '' if use_odometry else 'out', '' if seed else 'out')) 66 | progbar = ProgressBar(0, nscans, 80) 67 | 68 | # Start with an empty trajectory of positions 69 | trajectory = [] 70 | 71 | # Start timing 72 | start_sec = time() 73 | 74 | # Loop over scans 75 | for scanno in range(nscans): 76 | 77 | if use_odometry: 78 | 79 | # Convert odometry to velocities 80 | velocities = robot.computePoseChange(odometries[scanno]) 81 | 82 | # Update SLAM with lidar and velocities 83 | slam.update(lidars[scanno], velocities) 84 | 85 | else: 86 | 87 | # Update SLAM with lidar alone 88 | slam.update(lidars[scanno]) 89 | 90 | # Get new position 91 | x_mm, y_mm, theta_degrees = slam.getpos() 92 | 93 | # Add new position to trajectory 94 | trajectory.append((x_mm, y_mm)) 95 | 96 | # Tame impatience 97 | progbar.updateAmount(scanno) 98 | stdout.write('\r%s' % str(progbar)) 99 | stdout.flush() 100 | 101 | # Report elapsed time 102 | elapsed_sec = time() - start_sec 103 | print('\n%d scans in %f sec = %f scans / sec' % (nscans, elapsed_sec, nscans/elapsed_sec)) 104 | 105 | 106 | # Create a byte array to receive the computed maps 107 | mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) 108 | 109 | # Get final map 110 | slam.getmap(mapbytes) 111 | 112 | # Pickle the map to a file 113 | pklname = dataset + '.map' 114 | print('Writing map to file ' + pklname) 115 | pickle.dump(mapbytes, open(pklname, 'wb')) 116 | 117 | 118 | # Helpers --------------------------------------------------------- 119 | 120 | def mm2pix(mm): 121 | 122 | return int(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS)) 123 | 124 | 125 | main() 126 | -------------------------------------------------------------------------------- /c/coreslam.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | coreslam.h adapted from CoreSLAM.h downloaded from openslam.org on 01 January 2014. 4 | Contains efficient implementations of scan and map structures. 5 | 6 | Copyright (C) 2014 Simon D. Levy 7 | 8 | This code is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU Lesser General Public License as 10 | published by the Free Software Foundation, either version 3 of the 11 | License, or (at your option) any later version. 12 | 13 | This code is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public License 19 | along with this code. If not, see . 20 | */ 21 | 22 | /* Default parameters --------------------------------------------------------*/ 23 | 24 | static const int DEFAULT_MAP_QUALITY = 50; /* out of 255 */ 25 | static const double DEFAULT_HOLE_WIDTH_MM = 600; 26 | 27 | static const double DEFAULT_SIGMA_XY_MM = 100; 28 | static const double DEFAULT_SIGMA_THETA_DEGREES = 20; 29 | 30 | static const double DEFAULT_MAX_SEARCH_ITER = 1000; 31 | 32 | 33 | /* Core types --------------------------------------------------------------- */ 34 | 35 | typedef struct position_t 36 | { 37 | double x_mm; 38 | double y_mm; 39 | double theta_degrees; 40 | 41 | } position_t; 42 | 43 | typedef unsigned short pixel_t; 44 | 45 | typedef struct map_t { 46 | 47 | pixel_t * pixels; 48 | int size_pixels; 49 | double size_meters; 50 | 51 | double scale_pixels_per_mm; 52 | 53 | } map_t; 54 | 55 | 56 | typedef struct scan_t 57 | { 58 | double * x_mm; 59 | double * y_mm; 60 | int * value; 61 | int npoints; 62 | int span; 63 | 64 | int size; /* number of rays per scan */ 65 | double rate_hz; /* scans per second */ 66 | double detection_angle_degrees; /* e.g. 240, 360 */ 67 | double distance_no_detection_mm; /* default value when the laser returns 0 */ 68 | int detection_margin; /* first scan element to consider */ 69 | double offset_mm; /* position of the laser wrt center of rotation */ 70 | 71 | /* for angle/distance interpolation */ 72 | void * interpolation; 73 | 74 | /* for SSE */ 75 | float * obst_x_mm; 76 | float * obst_y_mm; 77 | int obst_npoints; 78 | 79 | } scan_t; 80 | 81 | /* Exported functions ------------------------------------------------------- */ 82 | 83 | #ifdef __cplusplus 84 | extern "C" 85 | { 86 | #endif 87 | 88 | int * 89 | int_alloc( 90 | int size); 91 | 92 | float * 93 | float_alloc( 94 | int size); 95 | 96 | void 97 | map_init( 98 | map_t * map, 99 | int size_pixels, 100 | double size_meters); 101 | 102 | void 103 | map_free( 104 | map_t * map); 105 | 106 | void map_string( 107 | map_t map, 108 | char * str); 109 | 110 | void 111 | map_update( 112 | map_t * map, 113 | scan_t * scan, 114 | position_t position, 115 | int map_quality, 116 | double hole_width_mm); 117 | 118 | void scan_init( 119 | scan_t * scan, 120 | int span, 121 | int size, 122 | double scan_rate_hz, 123 | double detection_angle_degrees, 124 | double distance_no_detection_mm, 125 | int detection_margin, 126 | double offset_mm); 127 | 128 | void 129 | scan_free( 130 | scan_t * scan); 131 | 132 | void scan_string( 133 | scan_t scan, 134 | char * str); 135 | 136 | void 137 | scan_update( 138 | scan_t * scan, 139 | float * lidar_angles_deg, 140 | int * lidar_distances_mm, 141 | int scan_size, 142 | double hole_width_mm, 143 | double velocities_dxy_mm, 144 | double velocities_dtheta_degrees); 145 | 146 | void 147 | map_get( 148 | map_t * map, 149 | char * bytes); 150 | 151 | void 152 | map_set( 153 | map_t * map, 154 | char * bytes); 155 | 156 | /* Returns -1 for infinity */ 157 | int 158 | distance_scan_to_map( 159 | map_t * map, 160 | scan_t * scan, 161 | position_t position); 162 | 163 | 164 | /* Random-Mutation Hill-Climbing search */ 165 | position_t 166 | rmhc_position_search( 167 | position_t start_pos, 168 | map_t * map, 169 | scan_t * scan, 170 | double sigma_xy_mm, 171 | double sigma_theta_degrees, 172 | int max_search_iter, 173 | void * randomizer); 174 | 175 | #ifdef __cplusplus 176 | } 177 | #endif 178 | 179 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java: -------------------------------------------------------------------------------- 1 | /** 2 | * SinglePositionSLAM is an abstract class that implements CoreSLAM using a point-cloud 3 | * with a single point (particle, position). Implementing classes should provide the method 4 | * 5 | * Position getNewPosition(Position start_position) 6 | * 7 | * to compute a new position based on searching from a starting position. 8 | * 9 | * Copyright (C) 2014 Simon D. Levy 10 | * 11 | * This code is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU Lesser General Public License as 13 | * published by the Free Software Foundation, either version 3 of the 14 | * License, or (at your option) any later version. 15 | * 16 | * This code is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU Lesser General Public License 22 | * along with this code. If not, see . 23 | */ 24 | 25 | package edu.wlu.cs.levy.breezyslam.algorithms; 26 | 27 | import edu.wlu.cs.levy.breezyslam.components.Position; 28 | import edu.wlu.cs.levy.breezyslam.components.PoseChange; 29 | import edu.wlu.cs.levy.breezyslam.components.Laser; 30 | 31 | 32 | public abstract class SinglePositionSLAM extends CoreSLAM 33 | { 34 | /** 35 | * Returns the current position. 36 | * @return the current position as a Position object. 37 | */ 38 | public Position getpos() 39 | { 40 | return this.position; 41 | } 42 | 43 | /** 44 | * Creates a SinglePositionSLAM object. 45 | * @param laser a Laser object containing parameters for your Lidar equipment 46 | * @param map_size_pixels the size of the desired map (map is square) 47 | * @param map_size_meters the size of the area to be mapped, in meters 48 | * @return a new SinglePositionSLAM object 49 | */ 50 | protected SinglePositionSLAM(Laser laser, int map_size_pixels, double map_size_meters) 51 | { 52 | super(laser, map_size_pixels, map_size_meters); 53 | 54 | this.position = new Position(this.init_coord_mm(), this.init_coord_mm(), 0); 55 | } 56 | 57 | 58 | /** 59 | * Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM::update() 60 | * @param poseChange poseChange for odometry 61 | */ 62 | protected void updateMapAndPointcloud(PoseChange poseChange) 63 | { 64 | // Start at current position 65 | Position start_pos = new Position(this.position); 66 | 67 | // Add effect of poseChange 68 | start_pos.x_mm += poseChange.getDxyMm() * this.costheta(); 69 | start_pos.y_mm += poseChange.getDxyMm() * this.sintheta(); 70 | start_pos.theta_degrees += poseChange.getDthetaDegrees(); 71 | 72 | // Add offset from laser 73 | start_pos.x_mm += this.laser.getOffsetMm() * this.costheta(); 74 | start_pos.y_mm += this.laser.getOffsetMm() * this.sintheta(); 75 | 76 | // Get new position from implementing class 77 | Position new_position = this.getNewPosition(start_pos); 78 | 79 | // Update the map with this new position 80 | this.map.update(this.scan_for_mapbuild, new_position, this.map_quality, this.hole_width_mm); 81 | 82 | // Update the current position with this new position, adjusted by laser offset 83 | this.position = new Position(new_position); 84 | this.position.x_mm -= this.laser.getOffsetMm() * this.costheta(); 85 | this.position.y_mm -= this.laser.getOffsetMm() * this.sintheta(); 86 | } 87 | 88 | /** 89 | * Returns a new position based on searching from a starting position. Called automatically by 90 | * SinglePositionSLAM::updateMapAndPointcloud() 91 | * @param start_pos the starting position 92 | */ 93 | 94 | protected abstract Position getNewPosition(Position start_pos); 95 | 96 | private Position position; 97 | 98 | private double theta_radians() 99 | { 100 | return java.lang.Math.toRadians(this.position.theta_degrees); 101 | } 102 | 103 | private double costheta() 104 | { 105 | return java.lang.Math.cos(this.theta_radians()); 106 | } 107 | 108 | private double sintheta() 109 | { 110 | return java.lang.Math.sin(this.theta_radians()); 111 | } 112 | 113 | private double init_coord_mm() 114 | { 115 | // Center of map 116 | return 500 * this.map.sizeMeters(); 117 | } 118 | } 119 | -------------------------------------------------------------------------------- /examples/logmovie.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ''' 4 | logdemo.py : BreezySLAM Python demo. Reads logfile with odometry and scan data 5 | from Paris Mines Tech and displays showing robot pose and map in 6 | real time. 7 | 8 | For details see 9 | 10 | @inproceedings{coreslam-2010, 11 | author = {Bruno Steux and Oussama El Hamzaoui}, 12 | title = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code}, 13 | booktitle = {11th International Conference on Control, Automation, 14 | Robotics and Vision, ICARCV 2010, Singapore, 7-10 15 | December 2010, Proceedings}, 16 | pages = {1975-1979}, 17 | publisher = {IEEE}, 18 | year = {2010} 19 | } 20 | 21 | Copyright (C) 2016 Simon D. Levy and Matt Lubas 22 | 23 | This code is free software: you can redistribute it and/or modify 24 | it under the terms of the GNU Lesser General Public License as 25 | published by the Free Software Foundation, either version 3 of the 26 | License, or (at your option) any later version. 27 | 28 | This code is distributed in the hope that it will be useful, 29 | but WITHOUT ANY WARRANTY without even the implied warranty of 30 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 31 | GNU General Public License for more details. 32 | 33 | You should have received a copy of the GNU Lesser General Public License 34 | along with this code. If not, see . 35 | ''' 36 | 37 | # Map size, scale 38 | MAP_SIZE_PIXELS = 800 39 | MAP_SIZE_METERS = 32 40 | 41 | from breezyslam.algorithms import Deterministic_SLAM, RMHC_SLAM 42 | from mines import MinesLaser, Rover, load_data 43 | from roboviz import MapVisualizer 44 | 45 | from sys import argv, exit 46 | from time import sleep 47 | from threading import Thread 48 | 49 | def threadfunc(robot, slam, timestamps, lidars, odometries, mapbytes, pose): 50 | ''' 51 | Threaded function runs SLAM, setting the map bytes and robot pose for display 52 | on the main thread. 53 | ''' 54 | 55 | # Initialize time for delay 56 | prevtime = 0 57 | 58 | # Loop over scans 59 | for scanno in range(len(lidars)): 60 | 61 | if odometries is None: 62 | 63 | # Update SLAM with lidar alone 64 | slam.update(lidars[scanno]) 65 | 66 | else: 67 | 68 | # Convert odometry to velocities 69 | velocities = robot.computePoseChange(odometries[scanno]) 70 | 71 | # Update SLAM with lidar and velocities 72 | slam.update(lidars[scanno], velocities) 73 | 74 | # Get new position 75 | pose[0],pose[1],pose[2] = slam.getpos() 76 | 77 | # Get new map 78 | slam.getmap(mapbytes) 79 | 80 | # Add delay to yield to main thread 81 | currtime = timestamps[scanno] / 1.e6 # Convert usec to sec 82 | if prevtime > 0: 83 | sleep(currtime-prevtime) 84 | prevtime = currtime 85 | 86 | def main(): 87 | 88 | # Bozo filter for input args 89 | if len(argv) < 4: 90 | print('Usage: %s [random_seed]' % argv[0]) 91 | print('Example: %s exp2 1 9999' % argv[0]) 92 | exit(1) 93 | 94 | # Grab input args 95 | dataset = argv[1] 96 | use_odometry = True if int(argv[2]) else False 97 | seed = int(argv[3]) if len(argv) > 3 else 0 98 | 99 | # Allocate byte array to receive map updates 100 | mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) 101 | 102 | # Load the data from the file 103 | timestamps, lidars, odometries = load_data('.', dataset) 104 | 105 | # Build a robot model if we want odometry 106 | robot = Rover() if use_odometry else None 107 | 108 | # Create a CoreSLAM object with laser params and optional robot object 109 | slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \ 110 | if seed \ 111 | else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) 112 | 113 | # Set up a SLAM display, named by dataset 114 | viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, dataset) 115 | 116 | # Pose will be modified in our threaded code 117 | pose = [0,0,0] 118 | 119 | # Launch the data-collection / update thread 120 | thread = Thread(target=threadfunc, args=(robot, slam, timestamps, lidars, odometries if use_odometry else None, mapbytes, pose)) 121 | thread.daemon = True 122 | thread.start() 123 | 124 | # Loop forever,displaying current map and pose 125 | while True: 126 | 127 | # Display map and robot pose, exiting gracefully if user closes it 128 | if not viz.display(pose[0]/1000., pose[1]/1000., pose[2], mapbytes): 129 | exit(0) 130 | 131 | main() 132 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * CoreSLAM.java abstract Java class for CoreSLAM algorithm in BreezySLAM 4 | * 5 | * Copyright (C) 2014 Simon D. Levy 6 | 7 | * This code is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as 9 | * published by the Free Software Foundation, either version 3 of the 10 | * License, or (at your option) any later version. 11 | * 12 | * This code is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with this code. If not, see . 19 | */ 20 | 21 | 22 | package edu.wlu.cs.levy.breezyslam.algorithms; 23 | 24 | import edu.wlu.cs.levy.breezyslam.components.Laser; 25 | import edu.wlu.cs.levy.breezyslam.components.PoseChange; 26 | import edu.wlu.cs.levy.breezyslam.components.Map; 27 | import edu.wlu.cs.levy.breezyslam.components.Scan; 28 | 29 | /** 30 | * CoreSLAM is an abstract class that uses the classes Position, Map, Scan, and Laser 31 | * to run variants of the simple CoreSLAM (tinySLAM) algorithm described in 32 | * 
 33 | *     @inproceedings{coreslam-2010,
 34 | *       author    = {Bruno Steux and Oussama El Hamzaoui}, 
 35 | *       title     = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code},  
 36 | *       booktitle = {11th International Conference on Control, Automation,   
 37 | *                    Robotics and Vision, ICARCV 2010, Singapore, 7-10  
 38 | *                    December 2010, Proceedings},  
 39 | *       pages     = {1975-1979},  
 40 | *       publisher = {IEEE},  
 41 | *       year      = {2010}
 42 | *     }
 43 | *
44 | * Implementing classes should provide the method 45 | * 46 | * void updateMapAndPointcloud(int * scan_mm, PoseChange & poseChange) 47 | * 48 | * to update the map and point-cloud (particle cloud). 49 | * 50 | */ 51 | public abstract class CoreSLAM { 52 | 53 | /** 54 | * The quality of the map (0 through 255) 55 | */ 56 | public int map_quality = 50; 57 | 58 | /** 59 | * The width in millimeters of each "hole" in the map (essentially, wall width) 60 | */ 61 | public double hole_width_mm = 600; 62 | 63 | protected Laser laser; 64 | 65 | protected PoseChange poseChange; 66 | 67 | protected Map map; 68 | 69 | protected Scan scan_for_mapbuild; 70 | protected Scan scan_for_distance; 71 | 72 | public CoreSLAM(Laser laser, int map_size_pixels, double map_size_meters) 73 | { 74 | // Set default params 75 | this.laser = new Laser(laser); 76 | 77 | // Initialize poseChange (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry 78 | this.poseChange = new PoseChange(); 79 | 80 | // Initialize a scan for computing distance to map, and one for updating map 81 | this.scan_for_mapbuild = this.scan_create(3); 82 | this.scan_for_distance = this.scan_create(1); 83 | 84 | // Initialize the map 85 | this.map = new Map(map_size_pixels, map_size_meters); 86 | } 87 | 88 | private Scan scan_create(int span) 89 | { 90 | return new Scan(this.laser, span); 91 | } 92 | 93 | private void scan_update(Scan scan, int [] scan_mm) 94 | { 95 | scan.update(scan_mm, this.hole_width_mm, this.poseChange); 96 | } 97 | 98 | 99 | public void update(int [] scan_mm, PoseChange poseChange) 100 | { 101 | // Build a scan for computing distance to map, and one for updating map 102 | this.scan_update(this.scan_for_mapbuild, scan_mm); 103 | this.scan_update(this.scan_for_distance, scan_mm); 104 | 105 | // Update poseChange 106 | this.poseChange.update(poseChange.getDxyMm(), poseChange.getDthetaDegrees(), poseChange.getDtSeconds()); 107 | 108 | // Implementing class updates map and pointcloud 109 | this.updateMapAndPointcloud(poseChange); 110 | } 111 | 112 | /** 113 | * Updates the scan, and calls the the implementing class's updateMapAndPointcloud method with zero poseChange 114 | * (no odometry). 115 | * @param scan_mm Lidar scan values, whose count is specified in the scan_size 116 | * attribute of the Laser object passed to the CoreSlam constructor 117 | */ 118 | public void update(int [] scan_mm) 119 | { 120 | PoseChange zero_poseChange = new PoseChange(); 121 | 122 | this.update(scan_mm, zero_poseChange); 123 | } 124 | 125 | 126 | protected abstract void updateMapAndPointcloud(PoseChange poseChange); 127 | 128 | public void getmap(byte [] mapbytes) 129 | { 130 | this.map.get(mapbytes); 131 | } 132 | 133 | } 134 | -------------------------------------------------------------------------------- /matlab/WheeledRobot.m: -------------------------------------------------------------------------------- 1 | classdef WheeledRobot 2 | %WheeledRobot An abstract class supporting ododmetry for wheeled robots. 3 | % Your implementing class should provide the method: 4 | % 5 | % extractOdometry(obj, timestamp, leftWheel, rightWheel) --> 6 | % (timestampSeconds, leftWheelDegrees, rightWheelDegrees) 7 | % 8 | % Copyright (C) 2014 Simon D. Levy 9 | % 10 | % This code is free software: you can redistribute it and/or modify 11 | % it under the terms of the GNU Lesser General Public License as 12 | % published by the Free Software Foundation, either version 3 of the 13 | % License, or (at your option) any later version. 14 | % 15 | % This code is distributed in the hope that it will be useful, 16 | % but WITHOUT ANY WARRANTY without even the implied warranty of 17 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | % GNU General Public License for more details. 19 | % 20 | % You should have received a copy of the GNU Lesser General Public License 21 | % along with this code. If not, see . 22 | 23 | properties (Access = 'private') 24 | 25 | wheelRadiusMillimeters 26 | halfAxleLengthMillimeters 27 | 28 | timestampSecondsPrev 29 | leftWheelDegreesPrev 30 | rightWheelDegreesPrev 31 | 32 | end 33 | 34 | methods (Access = 'protected') 35 | 36 | function s = str(obj) 37 | % Returns a string representation of this WheeledRobot 38 | s = sprintf('', ... 39 | obj.wheelRadiusMillimeters, obj.halfAxleLengthMillimeters); 40 | end 41 | 42 | function robot = WheeledRobot(wheelRadiusMillimeters, halfAxleLengthMillimeters) 43 | % Constructs a WheeledRobot object 44 | % robot = WheeledRobot(wheelRadiusMillimeters, halfAxleLengthMillimeters) 45 | robot.wheelRadiusMillimeters = wheelRadiusMillimeters; 46 | robot.halfAxleLengthMillimeters = halfAxleLengthMillimeters; 47 | 48 | end 49 | 50 | function r = deg2rad(~, d) 51 | % Converts degrees to radians 52 | r = d * pi / 180; 53 | end 54 | 55 | end 56 | 57 | methods (Abstract) 58 | 59 | extractOdometry(obj, timestamp, leftWheel, rightWheel) 60 | 61 | end 62 | 63 | methods (Access = 'public') 64 | 65 | function [poseChange, obj] = computePoseChange(obj, timestamp, leftWheelOdometry, rightWheelOdometry) 66 | % Computes forward and angular poseChange based on odometry. 67 | % 68 | % Parameters: 69 | 70 | % timestamp time stamp, in whatever units your robot uses 71 | % leftWheelOdometry odometry for left wheel, in whatever form your robot uses 72 | % rightWheelOdometry odometry for right wheel, in whatever form your robot uses 73 | % 74 | % Returns a tuple (dxyMillimeters, dthetaDegrees, dtSeconds) 75 | 76 | % dxyMillimeters forward distance traveled, in millimeters 77 | % dthetaDegrees change in angular position, in degrees 78 | % dtSeconds elapsed time since previous odometry, in seconds 79 | 80 | dxyMillimeters = 0; 81 | dthetaDegrees = 0; 82 | dtSeconds = 0; 83 | 84 | [timestampSecondsCurr, leftWheelDegreesCurr, rightWheelDegreesCurr] = ... 85 | obj.extractOdometry(timestamp, leftWheelOdometry, rightWheelOdometry); 86 | 87 | if obj.timestampSecondsPrev 88 | 89 | leftDiffDegrees = leftWheelDegreesCurr - obj.leftWheelDegreesPrev; 90 | rightDiffDegrees = rightWheelDegreesCurr - obj.rightWheelDegreesPrev; 91 | 92 | dxyMillimeters = obj.wheelRadiusMillimeters * ... 93 | (obj.deg2rad(leftDiffDegrees) + obj.deg2rad(rightDiffDegrees)); 94 | 95 | dthetaDegrees = (obj.wheelRadiusMillimeters / obj.halfAxleLengthMillimeters) * ... 96 | (rightDiffDegrees - leftDiffDegrees); 97 | 98 | dtSeconds = timestampSecondsCurr - obj.timestampSecondsPrev; 99 | end 100 | 101 | % Store current odometry for next time 102 | obj.timestampSecondsPrev = timestampSecondsCurr; 103 | obj.leftWheelDegreesPrev = leftWheelDegreesCurr; 104 | obj.rightWheelDegreesPrev = rightWheelDegreesCurr; 105 | 106 | % Return linear velocity, angular velocity, time difference 107 | poseChange = [dxyMillimeters, dthetaDegrees, dtSeconds]; 108 | 109 | end 110 | end 111 | end 112 | 113 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c: -------------------------------------------------------------------------------- 1 | /* 2 | * jnibreezyslam_components.c Java Native Interface code for BreezySLAM components 3 | * 4 | * Copyright (C) 2014 Simon D. Levy 5 | * 6 | * This code is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU Lesser General Public License as 8 | * published by the Free Software Foundation, either version 3 of the 9 | * License, or (at your option) any later version. 10 | * 11 | * This code is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public License 17 | * along with this code. If not, see . 18 | */ 19 | 20 | #include "../jni_utils.h" 21 | 22 | #include "Map.h" 23 | #include "Scan.h" 24 | 25 | #include 26 | #include 27 | 28 | #define MAXSTR 100 29 | 30 | // Map methods ----------------------------------------------------------------------------------------- 31 | 32 | JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_init (JNIEnv *env, jobject thisobject, jint size_pixels, jdouble size_meters) 33 | { 34 | map_t * map = (map_t *)malloc(sizeof(map_t)); 35 | map_init(map, (int)size_pixels, (double)size_meters); 36 | ptr_to_obj(env, thisobject, map); 37 | } 38 | 39 | JNIEXPORT jstring JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_toString (JNIEnv *env, jobject thisobject) 40 | { 41 | map_t * map = cmap_from_jmap(env, thisobject); 42 | 43 | char str[MAXSTR]; 44 | 45 | map_string(*map, str); 46 | 47 | return (*env)->NewStringUTF(env, str); 48 | } 49 | 50 | JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_get (JNIEnv *env, jobject thisobject, jbyteArray bytes) 51 | { 52 | map_t * map = cmap_from_jmap(env, thisobject); 53 | 54 | jbyte * ptr = (*env)->GetByteArrayElements(env, bytes, NULL); 55 | 56 | map_get(map, ptr); 57 | 58 | (*env)->ReleaseByteArrayElements(env, bytes, ptr, 0); 59 | } 60 | 61 | JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_update (JNIEnv *env, jobject thisobject, 62 | jobject scanobject, 63 | jdouble position_x_mm, 64 | jdouble position_y_mm, 65 | jdouble position_theta_degrees, 66 | jint quality, 67 | jdouble hole_width_mm) 68 | { 69 | map_t * map = cmap_from_jmap(env, thisobject); 70 | 71 | scan_t * scan = cscan_from_jscan(env, scanobject); 72 | 73 | position_t position; 74 | 75 | position.x_mm = position_x_mm; 76 | position.y_mm = position_y_mm; 77 | position.theta_degrees = position_theta_degrees; 78 | 79 | map_update(map, scan, position, quality, hole_width_mm); 80 | } 81 | 82 | 83 | // Scan methods ----------------------------------------------------------------------------------------- 84 | 85 | JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Scan_init (JNIEnv *env, jobject thisobject, 86 | jint span, 87 | jint scan_size, 88 | jdouble scan_rate_hz, 89 | jdouble detection_angle_degrees, 90 | jdouble distance_no_detection_mm, 91 | jint detection_margin, 92 | jdouble offset_mm) 93 | { 94 | scan_t * scan = (scan_t *)malloc(sizeof(scan_t)); 95 | 96 | scan_init(scan, 97 | span, 98 | scan_size, 99 | scan_rate_hz, 100 | detection_angle_degrees, 101 | distance_no_detection_mm, 102 | detection_margin, 103 | offset_mm); 104 | 105 | ptr_to_obj(env, thisobject, scan); 106 | } 107 | 108 | JNIEXPORT jstring JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Scan_toString (JNIEnv *env, jobject thisobject) 109 | { 110 | scan_t * scan = cscan_from_jscan(env, thisobject); 111 | 112 | char str[MAXSTR]; 113 | 114 | scan_string(*scan, str); 115 | 116 | return (*env)->NewStringUTF(env, str); 117 | } 118 | 119 | JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Scan_update (JNIEnv *env, jobject thisobject, 120 | jintArray lidar_mm, 121 | jdouble hole_width_mm, 122 | jdouble velocities_dxy_mm, 123 | jdouble velocities_dtheta_degrees) 124 | { 125 | scan_t * scan = cscan_from_jscan(env, thisobject); 126 | 127 | jint * lidar_mm_c = (*env)->GetIntArrayElements(env, lidar_mm, 0); 128 | 129 | // no support for angles/interpolation yet 130 | scan_update(scan, NULL, lidar_mm_c, scan->size, hole_width_mm, velocities_dxy_mm, velocities_dtheta_degrees); 131 | 132 | (*env)->ReleaseIntArrayElements(env, lidar_mm, lidar_mm_c, 0); 133 | } 134 | -------------------------------------------------------------------------------- /cpp/Doxyfile: -------------------------------------------------------------------------------- 1 | #--------------------------------------------------------------------------- 2 | # Doxyfile: Project-related configuration options 3 | # 4 | # Adapted from 5 | # 6 | # http://www.bluequartz.net/projects/EIM_Segmentation/SoftwareDocumentation/html/exampledoxyfile.html 7 | # 8 | # downloaded 21 March 2014 9 | # 10 | # Copyright (C) Simon D. Levy 2014 11 | # 12 | # This code is free software: you can redistribute it and/or modify 13 | # it under the terms of the GNU Lesser General Public License as 14 | # published by the Free Software Foundation, either version 3 of the 15 | # License, or (at your option) any later version. 16 | # 17 | # This code is distributed in the hope that it will be useful, 18 | # but WITHOUT ANY WARRANTY without even the implied warranty of 19 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | # GNU General Public License for more details. 21 | # 22 | # You should have received a copy of the GNU Lesser General Public License 23 | # along with this code. If not, see . 24 | #--------------------------------------------------------------------------- 25 | DOXYFILE_ENCODING = UTF-8 26 | PROJECT_NAME = BreezySLAM 27 | PROJECT_NUMBER = 28 | OUTPUT_DIRECTORY = Documentation 29 | CREATE_SUBDIRS = YES 30 | OUTPUT_LANGUAGE = English 31 | BRIEF_MEMBER_DESC = YES 32 | REPEAT_BRIEF = YES 33 | ABBREVIATE_BRIEF = "The $name class" \ 34 | "The $name widget" \ 35 | "The $name file" \ 36 | is \ 37 | provides \ 38 | specifies \ 39 | contains \ 40 | represents \ 41 | a \ 42 | an \ 43 | the 44 | ALWAYS_DETAILED_SEC = NO 45 | INLINE_INHERITED_MEMB = NO 46 | FULL_PATH_NAMES = YES 47 | STRIP_FROM_PATH = Source 48 | SHORT_NAMES = NO 49 | JAVADOC_AUTOBRIEF = NO 50 | MULTILINE_CPP_IS_BRIEF = NO 51 | INHERIT_DOCS = YES 52 | SEPARATE_MEMBER_PAGES = NO 53 | TAB_SIZE = 4 54 | OPTIMIZE_OUTPUT_FOR_C = NO 55 | OPTIMIZE_OUTPUT_JAVA = NO 56 | BUILTIN_STL_SUPPORT = YES 57 | CPP_CLI_SUPPORT = NO 58 | DISTRIBUTE_GROUP_DOC = YES 59 | SUBGROUPING = YES 60 | #--------------------------------------------------------------------------- 61 | # Build related configuration options 62 | #--------------------------------------------------------------------------- 63 | EXTRACT_ALL = YES 64 | EXTRACT_PRIVATE = NO 65 | EXTRACT_STATIC = YES 66 | EXTRACT_LOCAL_CLASSES = YES 67 | EXTRACT_LOCAL_METHODS = NO 68 | HIDE_UNDOC_MEMBERS = NO 69 | HIDE_UNDOC_CLASSES = NO 70 | HIDE_FRIEND_COMPOUNDS = YES 71 | HIDE_IN_BODY_DOCS = NO 72 | INTERNAL_DOCS = NO 73 | CASE_SENSE_NAMES = NO 74 | HIDE_SCOPE_NAMES = NO 75 | SHOW_INCLUDE_FILES = NO 76 | INLINE_INFO = YES 77 | SORT_MEMBER_DOCS = YES 78 | SORT_BRIEF_DOCS = NO 79 | SORT_BY_SCOPE_NAME = NO 80 | GENERATE_TODOLIST = YES 81 | GENERATE_TESTLIST = YES 82 | GENERATE_BUGLIST = YES 83 | GENERATE_DEPRECATEDLIST= YES 84 | MAX_INITIALIZER_LINES = 30 85 | SHOW_USED_FILES = YES 86 | #--------------------------------------------------------------------------- 87 | # configuration options related to warning and progress messages 88 | #--------------------------------------------------------------------------- 89 | QUIET = NO 90 | WARNINGS = YES 91 | WARN_IF_UNDOCUMENTED = YES 92 | WARN_IF_DOC_ERROR = YES 93 | WARN_NO_PARAMDOC = NO 94 | WARN_FORMAT = "$file:$line: $text" 95 | #--------------------------------------------------------------------------- 96 | # configuration options related to the input files 97 | #--------------------------------------------------------------------------- 98 | INPUT = breezyslam.hpp Position.hpp Map.hpp Scan.hpp \ 99 | Velocities.hpp Laser.hpp WheeledRobot.hpp algorithms.hpp 100 | INPUT_ENCODING = UTF-8 101 | FILE_PATTERNS = *.c *.cc *.cxx *.cpp *.c++ \ 102 | *.h *.hh *.hxx *.hpp *.h++ *.m *.mm *.dox \ 103 | RECURSIVE = YES 104 | EXCLUDE_SYMLINKS = NO 105 | EXAMPLE_PATTERNS = * 106 | EXAMPLE_RECURSIVE = NO 107 | FILTER_SOURCE_FILES = NO 108 | #--------------------------------------------------------------------------- 109 | # configuration options related to the alphabetical class index 110 | #--------------------------------------------------------------------------- 111 | ALPHABETICAL_INDEX = YES 112 | COLS_IN_ALPHA_INDEX = 3 113 | IGNORE_PREFIX = 114 | #--------------------------------------------------------------------------- 115 | # configuration options related to the HTML output 116 | #--------------------------------------------------------------------------- 117 | GENERATE_HTML = YES 118 | HTML_OUTPUT = html 119 | HTML_FILE_EXTENSION = .html 120 | HTML_HEADER = 121 | HTML_FOOTER = 122 | HTML_STYLESHEET = 123 | GENERATE_HTMLHELP = NO 124 | CHM_FILE = 125 | HHC_LOCATION = 126 | GENERATE_CHI = NO 127 | BINARY_TOC = NO 128 | TOC_EXPAND = NO 129 | DISABLE_INDEX = NO 130 | ENUM_VALUES_PER_LINE = 4 131 | GENERATE_TREEVIEW = NO 132 | TREEVIEW_WIDTH = 250 133 | -------------------------------------------------------------------------------- /examples/log2pgm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ''' 4 | log2pgm.py : BreezySLAM Python demo. Reads logfile with odometry and scan data 5 | from Paris Mines Tech and produces a .PGM image file showing robot 6 | trajectory and final map. 7 | 8 | For details see 9 | 10 | @inproceedings{coreslam-2010, 11 | author = {Bruno Steux and Oussama El Hamzaoui}, 12 | title = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code}, 13 | booktitle = {11th International Conference on Control, Automation, 14 | Vehicleics and Vision, ICARCV 2010, Singapore, 7-10 15 | December 2010, Proceedings}, 16 | pages = {1975-1979}, 17 | publisher = {IEEE}, 18 | year = {2010} 19 | } 20 | 21 | Copyright (C) 2014 Simon D. Levy 22 | 23 | This code is free software: you can redistribute it and/or modify 24 | it under the terms of the GNU Lesser General Public License as 25 | published by the Free Software Foundation, either version 3 of the 26 | License, or (at your option) any later version. 27 | 28 | This code is distributed in the hope that it will be useful, 29 | but WITHOUT ANY WARRANTY without even the implied warranty of 30 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 31 | GNU General Public License for more details. 32 | 33 | You should have received a copy of the GNU Lesser General Public License 34 | along with this code. If not, see . 35 | 36 | Change log: 37 | 38 | 20-APR-2014 - Simon D. Levy - Get params from command line 39 | 05-JUN-2014 - SDL - get random seed from command line 40 | ''' 41 | 42 | # Map size, scale 43 | MAP_SIZE_PIXELS = 800 44 | MAP_SIZE_METERS = 32 45 | 46 | from breezyslam.algorithms import Deterministic_SLAM, RMHC_SLAM 47 | 48 | from mines import MinesLaser, Rover, load_data 49 | from progressbar import ProgressBar 50 | from pgm_utils import pgm_save 51 | 52 | from sys import argv, exit, stdout 53 | from time import time 54 | 55 | def main(): 56 | 57 | # Bozo filter for input args 58 | if len(argv) < 3: 59 | print('Usage: %s [random_seed]' % argv[0]) 60 | print('Example: %s exp2 1 9999' % argv[0]) 61 | exit(1) 62 | 63 | # Grab input args 64 | dataset = argv[1] 65 | use_odometry = True if int(argv[2]) else False 66 | seed = int(argv[3]) if len(argv) > 3 else 0 67 | 68 | # Load the data from the file, ignoring timestamps 69 | _, lidars, odometries = load_data('.', dataset) 70 | 71 | # Build a robot model if we want odometry 72 | robot = Rover() if use_odometry else None 73 | 74 | # Create a CoreSLAM object with laser params and optional robot object 75 | slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \ 76 | if seed \ 77 | else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) 78 | 79 | # Report what we're doing 80 | nscans = len(lidars) 81 | print('Processing %d scans with%s odometry / with%s particle filter...' % \ 82 | (nscans, \ 83 | '' if use_odometry else 'out', '' if seed else 'out')) 84 | progbar = ProgressBar(0, nscans, 80) 85 | 86 | # Start with an empty trajectory of positions 87 | trajectory = [] 88 | 89 | # Start timing 90 | start_sec = time() 91 | 92 | # Loop over scans 93 | for scanno in range(nscans): 94 | 95 | if use_odometry: 96 | 97 | # Convert odometry to pose change (dxy_mm, dtheta_degrees, dt_seconds) 98 | velocities = robot.computePoseChange(odometries[scanno]) 99 | 100 | # Update SLAM with lidar and velocities 101 | slam.update(lidars[scanno], velocities) 102 | 103 | else: 104 | 105 | # Update SLAM with lidar alone 106 | slam.update(lidars[scanno]) 107 | 108 | # Get new position 109 | x_mm, y_mm, theta_degrees = slam.getpos() 110 | 111 | # Add new position to trajectory 112 | trajectory.append((x_mm, y_mm)) 113 | 114 | # Tame impatience 115 | progbar.updateAmount(scanno) 116 | stdout.write('\r%s' % str(progbar)) 117 | stdout.flush() 118 | 119 | # Report elapsed time 120 | elapsed_sec = time() - start_sec 121 | print('\n%d scans in %f sec = %f scans / sec' % (nscans, elapsed_sec, nscans/elapsed_sec)) 122 | 123 | 124 | # Create a byte array to receive the computed maps 125 | mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) 126 | 127 | # Get final map 128 | slam.getmap(mapbytes) 129 | 130 | # Put trajectory into map as black pixels 131 | for coords in trajectory: 132 | 133 | x_mm, y_mm = coords 134 | 135 | x_pix = mm2pix(x_mm) 136 | y_pix = mm2pix(y_mm) 137 | 138 | mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0; 139 | 140 | # Save map and trajectory as PGM file 141 | pgm_save('%s.pgm' % dataset, mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS)) 142 | 143 | # Helpers --------------------------------------------------------- 144 | 145 | def mm2pix(mm): 146 | 147 | return int(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS)) 148 | 149 | 150 | main() 151 | -------------------------------------------------------------------------------- /java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * BreezySLAM: Simple, efficient SLAM in Java 4 | * 5 | * WheeledRobot.java - Java class for wheeled robots 6 | * 7 | * Copyright (C) 2014 Simon D. Levy 8 | * 9 | * This code is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU Lesser General Public License as 11 | * published by the Free Software Foundation, either version 3 of the 12 | * License, or (at your option) any later version. 13 | * 14 | * This code is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU Lesser General Public License 20 | * along with this code. If not, see . 21 | */ 22 | 23 | 24 | package edu.wlu.cs.levy.breezyslam.robots; 25 | 26 | import edu.wlu.cs.levy.breezyslam.components.PoseChange; 27 | 28 | 29 | /** 30 | * An abstract class for wheeled robots. Supports computation of forward and angular 31 | * poseChange based on odometry. Your subclass should should implement the 32 | * extractOdometry method. 33 | */ 34 | public abstract class WheeledRobot 35 | { 36 | 37 | /** 38 | * Builds a WheeledRobot object. Parameters should be based on the specifications for 39 | * your robot. 40 | * @param wheel_radius_mm radius of each odometry wheel, in meters 41 | * @param half_axle_length_mm half the length of the axle between the odometry wheels, in meters 42 | * @return a new WheeledRobot object 43 | */ 44 | protected WheeledRobot(double wheel_radius_mm, double half_axle_length_mm) 45 | { 46 | this.wheel_radius_mm = wheel_radius_mm; 47 | this.half_axle_length_mm = half_axle_length_mm; 48 | 49 | this.timestamp_seconds_prev = 0; 50 | this.left_wheel_degrees_prev = 0; 51 | this.right_wheel_degrees_prev = 0; 52 | } 53 | 54 | public String toString() 55 | { 56 | 57 | return String.format("", 58 | this.wheel_radius_mm, this.half_axle_length_mm, this.descriptorString()); 59 | } 60 | 61 | /** 62 | * Computes forward and angular poseChange based on odometry. 63 | * @param timestamp time stamp, in whatever units your robot uses 64 | * @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses 65 | * @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses 66 | * @return poseChange object representing poseChange for these odometry values 67 | */ 68 | 69 | public PoseChange computePoseChange( double timestamp, double left_wheel_odometry, double right_wheel_odometry) 70 | { 71 | WheelOdometry odometry = this.extractOdometry(timestamp, left_wheel_odometry, right_wheel_odometry); 72 | 73 | double dxy_mm = 0; 74 | double dtheta_degrees = 0; 75 | double dt_seconds = 0; 76 | 77 | if (this.timestamp_seconds_prev > 0) 78 | { 79 | double left_diff_degrees = odometry.left_wheel_degrees - this.left_wheel_degrees_prev; 80 | double right_diff_degrees = odometry.right_wheel_degrees - this.right_wheel_degrees_prev; 81 | 82 | dxy_mm = this.wheel_radius_mm * (java.lang.Math.toRadians(left_diff_degrees) + java.lang.Math.toRadians(right_diff_degrees)); 83 | 84 | dtheta_degrees = this.wheel_radius_mm / this.half_axle_length_mm * (right_diff_degrees - left_diff_degrees); 85 | 86 | dt_seconds = odometry.timestamp_seconds - this.timestamp_seconds_prev; 87 | } 88 | 89 | // Store current odometry for next time 90 | this.timestamp_seconds_prev = odometry.timestamp_seconds; 91 | this.left_wheel_degrees_prev = odometry.left_wheel_degrees; 92 | this.right_wheel_degrees_prev = odometry.right_wheel_degrees; 93 | 94 | return new PoseChange(dxy_mm, dtheta_degrees, dt_seconds); 95 | } 96 | 97 | /** 98 | * Extracts usable odometry values from your robot's odometry. 99 | * @param timestamp time stamp, in whatever units your robot uses 100 | * @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses 101 | * @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses 102 | * @return WheelOdometry object containing timestamp in seconds, left wheel degrees, right wheel degrees 103 | */ 104 | protected abstract WheelOdometry extractOdometry(double timestamp, double left_wheel_odometry, double right_wheel_odometry); 105 | 106 | protected abstract String descriptorString(); 107 | 108 | protected class WheelOdometry 109 | { 110 | public WheelOdometry(double timestamp_seconds, double left_wheel_degrees, double right_wheel_degrees) 111 | { 112 | this.timestamp_seconds = timestamp_seconds; 113 | this.left_wheel_degrees = left_wheel_degrees; 114 | this.right_wheel_degrees = right_wheel_degrees; 115 | } 116 | 117 | public double timestamp_seconds; 118 | public double left_wheel_degrees; 119 | public double right_wheel_degrees; 120 | 121 | } 122 | 123 | 124 | private double wheel_radius_mm; 125 | private double half_axle_length_mm; 126 | 127 | private double timestamp_seconds_prev; 128 | private double left_wheel_degrees_prev; 129 | private double right_wheel_degrees_prev; 130 | } 131 | -------------------------------------------------------------------------------- /examples/log2png.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | ''' 4 | log2png.py : BreezySLAM Python demo. Reads logfile with odometry and scan data 5 | from Paris Mines Tech and produces a .PNG image file showing robot 6 | trajectory and final map. 7 | 8 | For details see 9 | 10 | @inproceedings{coreslam-2010, 11 | author = {Bruno Steux and Oussama El Hamzaoui}, 12 | title = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code}, 13 | booktitle = {11th International Conference on Control, Automation, 14 | Vehicleics and Vision, ICARCV 2010, Singapore, 7-10 15 | December 2010, Proceedings}, 16 | pages = {1975-1979}, 17 | publisher = {IEEE}, 18 | year = {2010} 19 | } 20 | 21 | Copyright (C) 2014 Simon D. Levy 22 | 23 | This code is free software: you can redistribute it and/or modify 24 | it under the terms of the GNU Lesser General Public License as 25 | published by the Free Software Foundation, either version 3 of the 26 | License, or (at your option) any later version. 27 | 28 | This code is distributed in the hope that it will be useful, 29 | but WITHOUT ANY WARRANTY without even the implied warranty of 30 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 31 | GNU General Public License for more details. 32 | 33 | You should have received a copy of the GNU Lesser General Public License 34 | along with this code. If not, see . 35 | 36 | Change log: 37 | 38 | 20-APR-2014 - Simon D. Levy - Get params from command line 39 | 05-JUN-2014 - SDL - get random seed from command line 40 | ''' 41 | 42 | # Map size, scale 43 | MAP_SIZE_PIXELS = 800 44 | MAP_SIZE_METERS = 32 45 | 46 | from breezyslam.algorithms import Deterministic_SLAM, RMHC_SLAM 47 | 48 | from mines import MinesLaser, Rover, load_data 49 | from progressbar import ProgressBar 50 | 51 | from sys import argv, exit, stdout 52 | from time import time 53 | 54 | from PIL import Image 55 | 56 | def main(): 57 | 58 | # Bozo filter for input args 59 | if len(argv) < 3: 60 | print('Usage: %s [random_seed]' % argv[0]) 61 | print('Example: %s exp2 1 9999' % argv[0]) 62 | exit(1) 63 | 64 | # Grab input args 65 | dataset = argv[1] 66 | use_odometry = True if int(argv[2]) else False 67 | seed = int(argv[3]) if len(argv) > 3 else 0 68 | 69 | # Load the data from the file, ignoring timestamps 70 | _, lidars, odometries = load_data('.', dataset) 71 | 72 | # Build a robot model if we want odometry 73 | robot = Rover() if use_odometry else None 74 | 75 | # Create a CoreSLAM object with laser params and optional robot object 76 | slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \ 77 | if seed \ 78 | else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS) 79 | 80 | # Report what we're doing 81 | nscans = len(lidars) 82 | print('Processing %d scans with%s odometry / with%s particle filter...' % \ 83 | (nscans, \ 84 | '' if use_odometry else 'out', '' if seed else 'out')) 85 | progbar = ProgressBar(0, nscans, 80) 86 | 87 | # Start with an empty trajectory of positions 88 | trajectory = [] 89 | 90 | # Start timing 91 | start_sec = time() 92 | 93 | # Loop over scans 94 | for scanno in range(nscans): 95 | 96 | if use_odometry: 97 | 98 | # Convert odometry to velocities 99 | velocities = robot.computePoseChange(odometries[scanno]) 100 | 101 | # Update SLAM with lidar and velocities 102 | slam.update(lidars[scanno], velocities) 103 | 104 | else: 105 | 106 | # Update SLAM with lidar alone 107 | slam.update(lidars[scanno]) 108 | 109 | # Get new position 110 | x_mm, y_mm, theta_degrees = slam.getpos() 111 | 112 | # Add new position to trajectory 113 | trajectory.append((x_mm, y_mm)) 114 | 115 | # Tame impatience 116 | progbar.updateAmount(scanno) 117 | stdout.write('\r%s' % str(progbar)) 118 | stdout.flush() 119 | 120 | # Report elapsed time 121 | elapsed_sec = time() - start_sec 122 | print('\n%d scans in %f sec = %f scans / sec' % (nscans, elapsed_sec, nscans/elapsed_sec)) 123 | 124 | 125 | # Create a byte array to receive the computed maps 126 | mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS) 127 | 128 | # Get final map 129 | slam.getmap(mapbytes) 130 | 131 | # Put trajectory into map as black pixels 132 | for coords in trajectory: 133 | 134 | x_mm, y_mm = coords 135 | 136 | x_pix = mm2pix(x_mm) 137 | y_pix = mm2pix(y_mm) 138 | 139 | mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0; 140 | 141 | # Save map and trajectory as PNG file 142 | image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS), mapbytes, 'raw', 'L', 0, 1) 143 | image.save('%s.png' % dataset) 144 | 145 | 146 | # Helpers --------------------------------------------------------- 147 | 148 | def mm2pix(mm): 149 | 150 | return int(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS)) 151 | 152 | 153 | main() 154 | -------------------------------------------------------------------------------- /examples/logdemo.m: -------------------------------------------------------------------------------- 1 | % 2 | % logdemo.m : BreezySLAM Matlab demo. Reads logfile with odometry and scan 3 | % data from Paris Mines Tech and displays the map and robot 4 | % position in real time. 5 | % 6 | % Usage: 7 | % 8 | % >> logdemo(dataset, [use_odometry], [random_seed]) 9 | % 10 | % Examples: 11 | % 12 | % >> logdemo('exp2') 13 | % 14 | % For details see 15 | % 16 | % @inproceedings{coreslam-2010, 17 | % author = {Bruno Steux and Oussama El Hamzaoui}, 18 | % title = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code}, 19 | % booktitle = {11th International Conference on Control, Automation, 20 | % Robotics and Vision, ICARCV 2010, Singapore, 7-10 21 | % December 2010, Proceedings}, 22 | % pages = {1975-1979}, 23 | % publisher = {IEEE}, 24 | % year = {2010} 25 | % } 26 | % 27 | % Copyright (C) 2014 Simon D. Levy 28 | % 29 | % This code is free software: you can redistribute it and/or modify 30 | % it under the terms of the GNU Lesser General Public License as 31 | % published by the Free Software Foundation, either version 3 of the 32 | % License, or (at your option) any later version. 33 | % 34 | % This code is distributed in the hope that it will be useful, 35 | % but WITHOUT ANY WARRANTY without even the implied warranty of 36 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 37 | % GNU General Public License for more details. 38 | % 39 | % You should have received a copy of the GNU Lesser General Public License 40 | % along with this code. If not, see . 41 | 42 | function logdemo(dataset, use_odometry, seed) 43 | 44 | % Params 45 | 46 | MAP_SIZE_PIXELS = 800; 47 | MAP_SIZE_METERS = 32; 48 | ROBOT_SIZE_PIXELS = 10; 49 | 50 | % Grab input args 51 | 52 | if nargin < 2 53 | use_odometry = 0; 54 | end 55 | 56 | if nargin < 3 57 | seed = 0; 58 | end 59 | 60 | % Load data from file 61 | [times, scans, odometries] = load_data(dataset); 62 | 63 | % Build a robot model if we want odometry 64 | robot = []; 65 | if use_odometry 66 | robot = MinesRover(); 67 | end 68 | 69 | % Create a CoreSLAM object with laser params and optional robot object 70 | if seed 71 | slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, seed); 72 | else 73 | slam = Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS); 74 | end 75 | 76 | % Initialize previous time for delay 77 | prevTime = 0; 78 | 79 | % Loop over scans 80 | for scanno = 1:size(scans, 1) 81 | 82 | if use_odometry 83 | 84 | % Convert odometry to velocities 85 | [velocities,robot] = robot.computePoseChange(odometries(scanno, :)); 86 | 87 | % Update SLAM with lidar and velocities 88 | slam = slam.update(scans(scanno,:), velocities); 89 | 90 | else 91 | 92 | % Update SLAM with lidar alone 93 | slam = slam.update(scans(scanno,:)); 94 | end 95 | 96 | % Get new position 97 | [x_mm, y_mm, theta_degrees] = slam.getpos(); 98 | 99 | % Get current map 100 | map = slam.getmap(); 101 | 102 | % Display map 103 | hold off 104 | image(map/4) % Keep bytes in [0,64] for colormap 105 | axis('square') 106 | colormap('gray') 107 | hold on 108 | 109 | % Generate a polyline to represent the robot 110 | [x_pix, y_pix] = robot_polyline(ROBOT_SIZE_PIXELS); 111 | 112 | % Rotate the polyline based on the robot's angular rotation 113 | theta_radians = pi * theta_degrees / 180; 114 | x_pix_r = x_pix * cos(theta_radians) - y_pix * sin(theta_radians); 115 | y_pix_r = x_pix * sin(theta_radians) + y_pix * cos(theta_radians); 116 | 117 | % Add the robot's position as offset to the polyline 118 | x_pix = x_pix_r + mm2pix(x_mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS); 119 | y_pix = y_pix_r + mm2pix(y_mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS); 120 | 121 | % Add robot image to map 122 | fill(x_pix, y_pix, 'r') 123 | drawnow 124 | 125 | % Delay based on current time in microseconds 126 | currTime = times(scanno); 127 | if scanno > 1 128 | pause((currTime-prevTime)/1e6) 129 | end 130 | prevTime = times(scanno); 131 | 132 | 133 | end 134 | 135 | % Function to generate a x, y points for a polyline to display the robot 136 | % Currently we just generate a triangle. 137 | function [x_pix, y_pix] = robot_polyline(robot_size) 138 | HEIGHT_RATIO = 1.25; 139 | x_pix = [-robot_size, robot_size, -robot_size]; 140 | y_pix = [-robot_size/HEIGHT_RATIO, 0 , robot_size/HEIGHT_RATIO]; 141 | 142 | % Function to convert millimeters to pixels ------------------------------- 143 | 144 | function pix = mm2pix(mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS) 145 | pix = floor(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS)); 146 | 147 | 148 | % Function to load all from file ------------------------------------------ 149 | % Each line in the file has the format: 150 | % 151 | % TIMESTAMP ... Q1 Q1 ... Distances 152 | % (usec) (mm) 153 | % 0 ... 2 3 ... 24 ... 154 | % 155 | %where Q1, Q2 are odometry values 156 | 157 | function [times, scans,odometries] = load_data(dataset) 158 | 159 | data = load([dataset '.dat']); 160 | 161 | times = data(:,1); 162 | scans = data(:,25:end-1); % avoid final ' ' 163 | odometries = data(:,[1,3,4]); 164 | 165 | % Function to build a Laser data structure for the Hokuyo URG04LX used for 166 | % collecting the logfile data. 167 | 168 | function laser = MinesLaser() 169 | 170 | laser.scan_size = 682; 171 | laser.scan_rate_hz = 10; 172 | laser.detection_angle_degrees = 240; 173 | laser.distance_no_detection_mm = 4000; 174 | laser.detection_margin = 70; 175 | laser.offset_mm = 145; 176 | -------------------------------------------------------------------------------- /matlab/CoreSLAM.m: -------------------------------------------------------------------------------- 1 | classdef CoreSLAM 2 | %CoreSLAM CoreSLAM abstract class for BreezySLAM 3 | % CoreSLAM is an abstract class that uses the classes Position, 4 | % Map, Scan, and Laser to run variants of the simple CoreSLAM 5 | % (tinySLAM) algorithm described in 6 | % 7 | % @inproceedings{coreslam-2010, 8 | % author = {Bruno Steux and Oussama El Hamzaoui}, 9 | % title = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code}, 10 | % booktitle = {11th International Conference on Control, Automation, 11 | % Robotics and Vision, ICARCV 2010, Singapore, 7-10 12 | % December 2010, Proceedings}, 13 | % pages = {1975-1979}, 14 | % publisher = {IEEE}, 15 | % year = {2010} 16 | % } 17 | % 18 | % 19 | % Implementing classes should provide the method 20 | % 21 | % updateMapAndPointcloud(scan_mm, velocities) 22 | % 23 | % to update the map and point-cloud (particle cloud). 24 | % 25 | % Copyright (C) 2014 Simon D. Levy 26 | % 27 | % This code is free software: you can redistribute it and/or modify 28 | % it under the terms of the GNU Lesser General Public License as 29 | % published by the Free Software Foundation, either version 3 of the 30 | % License, or (at your option) any later version. 31 | % 32 | % This code is distributed in the hope that it will be useful, 33 | % but WITHOUT ANY WARRANTY without even the implied warranty of 34 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 35 | % GNU General Public License for more details. 36 | % 37 | % You should have received a copy of the GNU Lesser General Public License 38 | % along with this code. If not, see . 39 | 40 | properties (Access = 'public') 41 | map_quality = 50; % The quality of the map (0 through 255); default = 50 42 | hole_width_mm = 600; % The width in millimeters of each "hole" in the map (essentially, wall width); default = 600 43 | end 44 | 45 | properties (Access = 'protected') 46 | laser % (internal) 47 | scan_for_distance % (internal) 48 | scan_for_mapbuild % (internal) 49 | map % (internal) 50 | velocities % (internal) 51 | end 52 | 53 | methods (Abstract, Access = 'protected') 54 | 55 | updateMapAndPointcloud(slam, velocities) 56 | 57 | end 58 | 59 | methods (Access = 'private') 60 | 61 | function scan_update(slam, scanobj, scans_mm) 62 | scanobj.update(scans_mm, slam.hole_width_mm, slam.velocities) 63 | end 64 | 65 | end 66 | 67 | methods (Access = 'protected') 68 | 69 | function slam = CoreSLAM(laser, map_size_pixels, map_size_meters) 70 | % Creates a CoreSLAM object suitable for updating with new Lidar and odometry data. 71 | % CoreSLAM(laser, map_size_pixels, map_size_meters) 72 | % laser is a Laser object representing the specifications of your Lidar unit 73 | % map_size_pixels is the size of the square map in pixels 74 | % map_size_meters is the size of the square map in meters 75 | 76 | % Store laser for later 77 | slam.laser = laser; 78 | 79 | % Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry 80 | slam.velocities = [0, 0, 0]; 81 | 82 | % Initialize a scan for computing distance to map, and one for updating map 83 | slam.scan_for_distance = Scan(laser, 1); 84 | slam.scan_for_mapbuild = Scan(laser, 3); 85 | 86 | % Initialize the map 87 | slam.map = Map(map_size_pixels, map_size_meters); 88 | 89 | end 90 | end 91 | 92 | methods (Access = 'public') 93 | 94 | function slam = update(slam, scans_mm, velocities) 95 | % Updates the scan and odometry. 96 | % Calls the the implementing class's updateMapAndPointcloud() 97 | % method with the specified velocities. 98 | % 99 | % slam = update(slam, scans_mm, [velocities]) 100 | % 101 | % scan_mm is a list of Lidar scan values, whose count is specified in the scan_size 102 | % velocities is an optional list of velocities [dxy_mm, dtheta_degrees, dt_seconds] for odometry 103 | 104 | % Build a scan for computing distance to map, and one for updating map 105 | slam.scan_update(slam.scan_for_mapbuild, scans_mm) 106 | slam.scan_update(slam.scan_for_distance, scans_mm) 107 | 108 | % Default to zero velocities 109 | if nargin < 3 110 | velocities = [0, 0, 0]; 111 | end 112 | 113 | % Update velocities 114 | velocity_factor = 0; 115 | if velocities(3) > 0 116 | velocity_factor = 1 / velocities(3); 117 | end 118 | new_dxy_mm = velocities(1) * velocity_factor; 119 | new_dtheta_degrees = velocities(2) * velocity_factor; 120 | slam.velocities = [new_dxy_mm, new_dtheta_degrees, 0]; 121 | 122 | % Implementing class updates map and pointcloud 123 | slam = slam.updateMapAndPointcloud(velocities); 124 | 125 | end 126 | 127 | function map = getmap(slam) 128 | % Returns the current map. 129 | % Map is returned as an occupancy grid (matrix of pixels). 130 | map = slam.map.get(); 131 | end 132 | 133 | end 134 | 135 | end 136 | 137 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | --------------------------------------------------------------------------------