diff --git a/README.md b/README.md index 22f5479..16f0287 100644 --- a/README.md +++ b/README.md @@ -5,19 +5,20 @@ BreezySLAM

-Simple, efficient, open-source package for Simultaneous Localization and Mapping in Python, Matlab, Java, and C++ +Simple, efficient, open-source package for Simultaneous Localization and Mapping in Python and C++ This repository contains everything you need to start working with Lidar -based SLAM -in Python, Matlab or C++. BreezySLAM works with Python 2 and 3 on Linux and Mac OS X, and -with C++ on Linux and Windows. -By using Python C extensions, we were able to get the Python and Matlab versions to run -as fast as C++. For maximum effiency on 32-bit platforms, we use Streaming -SIMD extensions (Intel) and NEON (ARMv7) in the compute-intensive part -of the code. +in Python or C++. (For those interested in Matlab or Java, there is a +[legacy](https://github.com/simondlevy/BreezySLAM/tree/legacy) branch, which I +am no longer maintaining.) BreezySLAM works with Python 2 and 3 on Linux and +Mac OS X, and with C++ on Linux and Windows. By using Python C extensions, we +were able to get the Python version to run as fast as C++. For maximum effiency +on 32-bit platforms, we use Streaming SIMD extensions (Intel) and NEON (ARMv7) +in the compute-intensive part of the code.

BreezySLAM was inspired by the Breezy approach to Graphical User Interfaces developed by my colleague @@ -120,27 +121,6 @@ To try it out, you'll also need the these instructions very helpful when I ran into trouble. -

Installing for C++

Just cd to BreezySLAM/cpp, and do @@ -166,21 +146,6 @@ the Makefile in this directory as well, if you don't use /usr/local/lib

-

Installing for Java

- -In BreezySLAM/java/edu/wlu/cs/levy/breezyslam/algorithms and -BreezySLAM/java/edu/wlu/cs/levy/breezyslam/components, -edit the JDKINC variable in the Makefile to reflect where you installed the JDK. -Then run make in these directories. - -

- -For a quick demo, you can then cd to BreezySLAM/examples and do - -

-make javatest
-
-

Notes on Windows installation

diff --git a/examples/MinesRover.m b/examples/MinesRover.m deleted file mode 100755 index a0f2378..0000000 --- a/examples/MinesRover.m +++ /dev/null @@ -1,69 +0,0 @@ -classdef MinesRover < WheeledRobot - %MinesRover Class for MinesRover custom robot - % - % Copyright (C) 2014 Simon D. Levy - % - % This code is free software: you can redistribute it and/or modify - % it under the terms of the GNU Lesser General Public License as - % published by the Free Software Foundation, either version 3 of the - % License, or (at your option) any later version. - % - % This code is distributed in the hope that it will be useful, - % but WITHOUT ANY WARRANTY without even the implied warranty of - % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - % GNU General Public License for more details. - % - % You should have received a copy of the GNU Lesser General Public License - % along with this code. If not, see . - - properties (Access = 'private') - TICKS_PER_CYCLE = 2000; - end - - methods (Access = 'private') - - function degrees = ticks_to_degrees(obj, ticks) - - degrees = ticks * (180. / obj.TICKS_PER_CYCLE); - - end - - end - - methods (Access = 'public') - - function robot = MinesRover() - - robot = robot@WheeledRobot(77, 165); - - end - - function disp(obj) - % Displays information about this MinesRover - - fprintf('<%s ticks_per_cycle=%d>\n', obj.str(), obj.TICKS_PER_CYCLE) - - end - - function [poseChange, obj] = computePoseChange(obj, odometry) - - [poseChange, obj] = computePoseChange@WheeledRobot(obj, odometry(1), odometry(2), odometry(3)); - - end - - function [timestampSeconds, leftWheelDegrees, rightWheelDegrees] = ... - extractOdometry(obj, timestamp, leftWheel, rightWheel) - - % Convert microseconds to seconds - timestampSeconds = timestamp / 1e6; - - % Convert ticks to angles - leftWheelDegrees = obj.ticks_to_degrees(leftWheel); - rightWheelDegrees = obj.ticks_to_degrees(rightWheel); - - end - - end - -end - diff --git a/examples/log2pgm.cpp b/examples/log2pgm.cpp deleted file mode 100644 index a88789d..0000000 --- a/examples/log2pgm.cpp +++ /dev/null @@ -1,439 +0,0 @@ -/* -log2pgm.cpp : BreezySLAM demo. Reads logfile with odometry and scan data from -Paris Mines Tech and produces a .PGM image file showing robot path -and final map. - -For details see - -@inproceedings{, - author = {Bruno Steux and Oussama El Hamzaoui}, - title = {SinglePositionSLAM: a SLAM Algorithm in less than 200 lines of C code}, - booktitle = {11th International Conference on Control, Automation, Robotics and Vision, ICARCV 2010, Singapore, 7-10 - December 2010, Proceedings}, - pages = {1975-1979}, - publisher = {IEEE}, - year = {2010} -} - -Copyright (C) 2014 Simon D. Levy - -This code is free software: you can redistribute it and/or modify -it under the terms of the GNU Lesser General Public License as -published by the Free Software Foundation, either version 3 of the -License, or (at your option) any later version. - -This code is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU Lesser General Public License -along with this code. If not, see . - -Change log: - -20-APR-2014 - Simon D. Levy - Get params from command line -05-JUN-2014 - SDL - get random seed from command line -*/ - -// SinglePositionSLAM params: gives us a nice-size map -static const int MAP_SIZE_PIXELS = 800; -static const double MAP_SIZE_METERS = 32; - -static const int SCAN_SIZE = 682; - -// Arbitrary maximum length of line in input logfile -#define MAXLINE 10000 - -#include -#include -using namespace std; - -#include -#include -#include -#include -#include - -#include "Position.hpp" -#include "Laser.hpp" -#include "WheeledRobot.hpp" -#include "PoseChange.hpp" -#include "algorithms.hpp" - - -// Methods to load all data from file ------------------------------------------ -// Each line in the file has the format: -// -// TIMESTAMP ... Q1 Q1 ... Distances -// (usec) (mm) -// 0 ... 2 3 ... 24 ... -// -//where Q1, Q2 are odometry values - -static void skiptok(char ** cpp) -{ - *cpp = strtok(NULL, " "); -} - -static int nextint(char ** cpp) -{ - skiptok(cpp); - - return atoi(*cpp); -} - -static void load_data( - const char * dataset, - vector & scans, - vector & odometries) -{ - char filename[256]; - - sprintf(filename, "%s.dat", dataset); - printf("Loading data from %s ... \n", filename); - - FILE * fp = fopen(filename, "rt"); - - if (!fp) - { - fprintf(stderr, "Failed to open file\n"); - exit(1); - } - - char s[MAXLINE]; - - while (fgets(s, MAXLINE, fp)) - { - char * cp = strtok(s, " "); - - long * odometry = new long [3]; - odometry[0] = atol(cp); - skiptok(&cp); - odometry[1] = nextint(&cp); - odometry[2] = nextint(&cp); - - odometries.push_back(odometry); - - // Skip unused fields - for (int k=0; k<20; ++k) - { - skiptok(&cp); - } - - int * scanvals = new int [SCAN_SIZE]; - - for (int k=0; kTICKS_PER_CYCLE); - } - -private: - - double ticksToDegrees(double ticks) - { - return ticks * (180. / this->TICKS_PER_CYCLE); - } - - static const int TICKS_PER_CYCLE = 2000; -}; - -// Progress-bar class -// Adapted from http://code.activestate.com/recipes/168639-progress-bar-class/ -// Downloaded 12 January 2014 - -class ProgressBar -{ -public: - - ProgressBar(int minValue, int maxValue, int totalWidth) - { - strcpy(this->progBar, "[]"); // This holds the progress bar string - this->min = minValue; - this->max = maxValue; - this->span = maxValue - minValue; - this->width = totalWidth; - this->amount = 0; // When amount == max, we are 100% done - this->updateAmount(0); // Build progress bar string - } - - void updateAmount(int newAmount) - { - if (newAmount < this->min) - { - newAmount = this->min; - } - if (newAmount > this->max) - { - newAmount = this->max; - } - - this->amount = newAmount; - - // Figure out the new percent done, round to an integer - float diffFromMin = float(this->amount - this->min); - int percentDone = (int)round((diffFromMin / float(this->span)) * 100.0); - - // Figure out how many hash bars the percentage should be - int allFull = this->width - 2; - int numHashes = (int)round((percentDone / 100.0) * allFull); - - - // Build a progress bar with hashes and spaces - strcpy(this->progBar, "["); - this->addToProgBar("#", numHashes); - this->addToProgBar(" ", allFull-numHashes); - strcat(this->progBar, "]"); - - // Figure out where to put the percentage, roughly centered - int percentPlace = (strlen(this->progBar) / 2) - ((int)(log10(percentDone+1)) + 1); - char percentString[5]; - sprintf(percentString, "%d%%", percentDone); - - // Put it there - for (int k=0; kprogBar[percentPlace+k] = percentString[k]; - } - - } - - char * str() - { - return this->progBar; - } - -private: - - char progBar[1000]; // more than we should ever need - int min; - int max; - int span; - int width; - int amount; - - void addToProgBar(const char * s, int n) - { - for (int k=0; kprogBar, s); - } - } -}; - -// Helpers ---------------------------------------------------------------- - -int coords2index(double x, double y) -{ - return y * MAP_SIZE_PIXELS + x; -} - - -int mm2pix(double mm) -{ - return (int)(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS)); -} - -int main( int argc, const char** argv ) -{ - // Bozo filter for input args - if (argc < 3) - { - fprintf(stderr, - "Usage: %s \n", - argv[0]); - fprintf(stderr, "Example: %s exp2 1 9999\n", argv[0]); - exit(1); - } - - // Grab input args - const char * dataset = argv[1]; - bool use_odometry = atoi(argv[2]) ? true : false; - int random_seed = argc > 3 ? atoi(argv[3]) : 0; - - // Load the Lidar and odometry data from the file - vector scans; - vector odometries; - load_data(dataset, scans, odometries); - - // Build a robot model in case we want odometry - Rover robot = Rover(); - - // Create a byte array to receive the computed maps - unsigned char * mapbytes = new unsigned char[MAP_SIZE_PIXELS * MAP_SIZE_PIXELS]; - - // Create SLAM object - MinesURG04LX laser; - SinglePositionSLAM * slam = random_seed ? - (SinglePositionSLAM*)new RMHC_SLAM(laser, MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed) : - (SinglePositionSLAM*)new Deterministic_SLAM(laser, MAP_SIZE_PIXELS, MAP_SIZE_METERS); - - // Report what we're doing - int nscans = scans.size(); - printf("Processing %d scans with%s odometry / with%s particle filter...\n", - nscans, use_odometry ? "" : "out", random_seed ? "" : "out"); - ProgressBar * progbar = new ProgressBar(0, nscans, 80); - - // Start with an empty trajectory of positions - vector trajectory; - - // Start timing - time_t start_sec = time(NULL); - - // Loop over scans - for (int scanno=0; scannoupdate(lidar, poseChange); - } - else - { - slam->update(lidar); - } - - Position position = slam->getpos(); - - // Add new coordinates to trajectory - double * v = new double[2]; - v[0] = position.x_mm; - v[1] = position.y_mm; - trajectory.push_back(v); - - // Tame impatience - progbar->updateAmount(scanno); - printf("\r%s", progbar->str()); - fflush(stdout); - } - - // Report speed - time_t elapsed_sec = time(NULL) - start_sec; - printf("\n%d scans in %ld seconds = %f scans / sec\n", - nscans, elapsed_sec, (float)nscans/elapsed_sec); - - // Get final map - slam->getmap(mapbytes); - - // Put trajectory into map as black pixels - for (int k=0; k<(int)trajectory.size(); ++k) - { - double * v = trajectory[k]; - - int x = mm2pix(v[0]); - int y = mm2pix(v[1]); - - delete v; - - mapbytes[coords2index(x, y)] = 0; - } - - // Save map and trajectory as PGM file - - char filename[100]; - sprintf(filename, "%s.pgm", dataset); - printf("\nSaving map to file %s\n", filename); - - FILE * output = fopen(filename, "wt"); - - fprintf(output, "P2\n%d %d 255\n", MAP_SIZE_PIXELS, MAP_SIZE_PIXELS); - - for (int y=0; y> logdemo(dataset, [use_odometry], [random_seed]) -% -% Examples: -% -% >> logdemo('exp2') -% -% For details see -% -% @inproceedings{coreslam-2010, -% author = {Bruno Steux and Oussama El Hamzaoui}, -% title = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code}, -% booktitle = {11th International Conference on Control, Automation, -% Robotics and Vision, ICARCV 2010, Singapore, 7-10 -% December 2010, Proceedings}, -% pages = {1975-1979}, -% publisher = {IEEE}, -% year = {2010} -% } -% -% Copyright (C) 2014 Simon D. Levy -% -% This code is free software: you can redistribute it and/or modify -% it under the terms of the GNU Lesser General Public License as -% published by the Free Software Foundation, either version 3 of the -% License, or (at your option) any later version. -% -% This code is distributed in the hope that it will be useful, -% but WITHOUT ANY WARRANTY without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -% GNU General Public License for more details. -% -% You should have received a copy of the GNU Lesser General Public License -% along with this code. If not, see . - -function logdemo(dataset, use_odometry, seed) - -% Params - -MAP_SIZE_PIXELS = 800; -MAP_SIZE_METERS = 32; -ROBOT_SIZE_PIXELS = 10; - -% Grab input args - -if nargin < 2 - use_odometry = 0; -end - -if nargin < 3 - seed = 0; -end - -% Load data from file -[times, scans, odometries] = load_data(dataset); - -% Build a robot model if we want odometry -robot = []; -if use_odometry - robot = MinesRover(); -end - -% Create a CoreSLAM object with laser params and optional robot object -if seed - slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, seed); -else - slam = Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS); -end - -% Initialize previous time for delay -prevTime = 0; - -% Loop over scans -for scanno = 1:size(scans, 1) - - if use_odometry - - % Convert odometry to velocities - [velocities,robot] = robot.computePoseChange(odometries(scanno, :)); - - % Update SLAM with lidar and velocities - slam = slam.update(scans(scanno,:), velocities); - - else - - % Update SLAM with lidar alone - slam = slam.update(scans(scanno,:)); - end - - % Get new position - [x_mm, y_mm, theta_degrees] = slam.getpos(); - - % Get current map - map = slam.getmap(); - - % Display map - hold off - image(map/4) % Keep bytes in [0,64] for colormap - axis('square') - colormap('gray') - hold on - - % Generate a polyline to represent the robot - [x_pix, y_pix] = robot_polyline(ROBOT_SIZE_PIXELS); - - % Rotate the polyline based on the robot's angular rotation - theta_radians = pi * theta_degrees / 180; - x_pix_r = x_pix * cos(theta_radians) - y_pix * sin(theta_radians); - y_pix_r = x_pix * sin(theta_radians) + y_pix * cos(theta_radians); - - % Add the robot's position as offset to the polyline - x_pix = x_pix_r + mm2pix(x_mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS); - y_pix = y_pix_r + mm2pix(y_mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS); - - % Add robot image to map - fill(x_pix, y_pix, 'r') - drawnow - - % Delay based on current time in microseconds - currTime = times(scanno); - if scanno > 1 - pause((currTime-prevTime)/1e6) - end - prevTime = times(scanno); - - -end - -% Function to generate a x, y points for a polyline to display the robot -% Currently we just generate a triangle. -function [x_pix, y_pix] = robot_polyline(robot_size) -HEIGHT_RATIO = 1.25; -x_pix = [-robot_size, robot_size, -robot_size]; -y_pix = [-robot_size/HEIGHT_RATIO, 0 , robot_size/HEIGHT_RATIO]; - -% Function to convert millimeters to pixels ------------------------------- - -function pix = mm2pix(mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS) -pix = floor(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS)); - - -% Function to load all from file ------------------------------------------ -% Each line in the file has the format: -% -% TIMESTAMP ... Q1 Q1 ... Distances -% (usec) (mm) -% 0 ... 2 3 ... 24 ... -% -%where Q1, Q2 are odometry values - -function [times, scans,odometries] = load_data(dataset) - -data = load([dataset '.dat']); - -times = data(:,1); -scans = data(:,25:end-1); % avoid final ' ' -odometries = data(:,[1,3,4]); - -% Function to build a Laser data structure for the Hokuyo URG04LX used for -% collecting the logfile data. - -function laser = MinesLaser() - -laser.scan_size = 682; -laser.scan_rate_hz = 10; -laser.detection_angle_degrees = 240; -laser.distance_no_detection_mm = 4000; -laser.detection_margin = 70; -laser.offset_mm = 145; diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java deleted file mode 100644 index a57a162..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java +++ /dev/null @@ -1,133 +0,0 @@ -/** -* -* CoreSLAM.java abstract Java class for CoreSLAM algorithm in BreezySLAM -* -* Copyright (C) 2014 Simon D. Levy - -* This code is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as -* published by the Free Software Foundation, either version 3 of the -* License, or (at your option) any later version. -* -* This code is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with this code. If not, see . -*/ - - -package edu.wlu.cs.levy.breezyslam.algorithms; - -import edu.wlu.cs.levy.breezyslam.components.Laser; -import edu.wlu.cs.levy.breezyslam.components.PoseChange; -import edu.wlu.cs.levy.breezyslam.components.Map; -import edu.wlu.cs.levy.breezyslam.components.Scan; - -/** -* CoreSLAM is an abstract class that uses the classes Position, Map, Scan, and Laser -* to run variants of the simple CoreSLAM (tinySLAM) algorithm described in -* 
-*     @inproceedings{coreslam-2010,
-*       author    = {Bruno Steux and Oussama El Hamzaoui}, 
-*       title     = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code},  
-*       booktitle = {11th International Conference on Control, Automation,   
-*                    Robotics and Vision, ICARCV 2010, Singapore, 7-10  
-*                    December 2010, Proceedings},  
-*       pages     = {1975-1979},  
-*       publisher = {IEEE},  
-*       year      = {2010}
-*     }
-*
-* Implementing classes should provide the method -* -* void updateMapAndPointcloud(int * scan_mm, PoseChange & poseChange) -* -* to update the map and point-cloud (particle cloud). -* -*/ -public abstract class CoreSLAM { - - /** - * The quality of the map (0 through 255) - */ - public int map_quality = 50; - - /** - * The width in millimeters of each "hole" in the map (essentially, wall width) - */ - public double hole_width_mm = 600; - - protected Laser laser; - - protected PoseChange poseChange; - - protected Map map; - - protected Scan scan_for_mapbuild; - protected Scan scan_for_distance; - - public CoreSLAM(Laser laser, int map_size_pixels, double map_size_meters) - { - // Set default params - this.laser = new Laser(laser); - - // Initialize poseChange (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry - this.poseChange = new PoseChange(); - - // Initialize a scan for computing distance to map, and one for updating map - this.scan_for_mapbuild = this.scan_create(3); - this.scan_for_distance = this.scan_create(1); - - // Initialize the map - this.map = new Map(map_size_pixels, map_size_meters); - } - - private Scan scan_create(int span) - { - return new Scan(this.laser, span); - } - - private void scan_update(Scan scan, int [] scan_mm) - { - scan.update(scan_mm, this.hole_width_mm, this.poseChange); - } - - - public void update(int [] scan_mm, PoseChange poseChange) - { - // Build a scan for computing distance to map, and one for updating map - this.scan_update(this.scan_for_mapbuild, scan_mm); - this.scan_update(this.scan_for_distance, scan_mm); - - // Update poseChange - this.poseChange.update(poseChange.getDxyMm(), poseChange.getDthetaDegrees(), poseChange.getDtSeconds()); - - // Implementing class updates map and pointcloud - this.updateMapAndPointcloud(poseChange); - } - - /** - * Updates the scan, and calls the the implementing class's updateMapAndPointcloud method with zero poseChange - * (no odometry). - * @param scan_mm Lidar scan values, whose count is specified in the scan_size - * attribute of the Laser object passed to the CoreSlam constructor - */ - public void update(int [] scan_mm) - { - PoseChange zero_poseChange = new PoseChange(); - - this.update(scan_mm, zero_poseChange); - } - - - protected abstract void updateMapAndPointcloud(PoseChange poseChange); - - public void getmap(byte [] mapbytes) - { - this.map.get(mapbytes); - } - -} diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java deleted file mode 100644 index 458943d..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java +++ /dev/null @@ -1,52 +0,0 @@ -/** -* DeterministicSLAM implements SinglePositionSLAM using by returning the starting position instead of searching -* on it; i.e., using odometry alone. -* -* Copyright (C) 2014 Simon D. Levy -* -* This code is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as -* published by the Free Software Foundation, either version 3 of the -* License, or (at your option) any later version. -* -* This code is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with this code. If not, see . -*/ - - -package edu.wlu.cs.levy.breezyslam.algorithms; - -import edu.wlu.cs.levy.breezyslam.components.Position; -import edu.wlu.cs.levy.breezyslam.components.Laser; - -public class DeterministicSLAM extends SinglePositionSLAM -{ - - /** - * Creates a DeterministicSLAM object. - * @param laser a Laser object containing parameters for your Lidar equipment - * @param map_size_pixels the size of the desired map (map is square) - * @param map_size_meters the size of the area to be mapped, in meters - * @return a new CoreSLAM object - */ - public DeterministicSLAM(Laser laser, int map_size_pixels, double map_size_meters) - { - super(laser, map_size_pixels, map_size_meters); - } - - /** - * Returns a new position identical to the starting position. Called automatically by - * SinglePositionSLAM::updateMapAndPointcloud() - * @param start_pos the starting position - */ - protected Position getNewPosition(Position start_position) - { - return new Position(start_position.x_mm, start_position.y_mm, start_position.theta_degrees); - } - -} // DeterministicSLAM diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile b/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile deleted file mode 100644 index 45b5a43..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile +++ /dev/null @@ -1,90 +0,0 @@ -# Makefile for BreezySLAM algorithms in Java -# -# Copyright (C) 2014 Simon D. Levy -# -# This code is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as -# published by the Free Software Foundation, either version 3 of the -# License, or (at your option) any later version. -# -# This code is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. - -# You should have received a copy of the GNU Lesser General Public License -# along with this code. If not, see . - -BASEDIR = ../../../../../../.. -JAVADIR = $(BASEDIR)/java -CDIR = $(BASEDIR)/c -JFLAGS = -Xlint -JDKINC = -I /usr/lib/jvm/java-7-openjdk-amd64/include -#JDKINC = -I /opt/jdk1.7.0_67/include -I /opt/jdk1.7.0_67/include/linux - -# Set library extension based on OS -ifeq ("$(shell uname)","Darwin") - LIBEXT = dylib -else ifeq ("$(shell uname)","Linux") - CFLAGS = -fPIC - LIBEXT = so -else - LIBEXT = dll -endif - -# Set SIMD compile params based on architecture -ifeq ("$(ARCH)","armv7l") - SIMD_FLAGS = -mfpu=neon -else ifeq ("$(ARCH)","i686") - SIMD_FLAGS = -msse3 -else - ARCH = sisd -endif - - -ALL = libjnibreezyslam_algorithms.$(LIBEXT) CoreSLAM.class SinglePositionSLAM.class DeterministicSLAM.class RMHCSLAM.class - -all: $(ALL) - -libjnibreezyslam_algorithms.$(LIBEXT): jnibreezyslam_algorithms.o coreslam.o random.o ziggurat.o coreslam_$(ARCH).o - gcc -shared -Wl,-soname,libjnibreezyslam_algorithms.so -o libjnibreezyslam_algorithms.so jnibreezyslam_algorithms.o \ - coreslam.o coreslam_$(ARCH).o random.o ziggurat.o - -jnibreezyslam_algorithms.o: jnibreezyslam_algorithms.c RMHCSLAM.h ../jni_utils.h - gcc $(JDKINC) -fPIC -c jnibreezyslam_algorithms.c - - -CoreSLAM.class: CoreSLAM.java - javac -classpath $(JAVADIR):. CoreSLAM.java - - -SinglePositionSLAM.class: SinglePositionSLAM.java - javac -classpath $(JAVADIR):. SinglePositionSLAM.java - -DeterministicSLAM.class: DeterministicSLAM.java - javac -classpath $(JAVADIR):. DeterministicSLAM.java - -RMHCSLAM.class: RMHCSLAM.java - javac -classpath $(JAVADIR):. RMHCSLAM.java - -RMHCSLAM.h: RMHCSLAM.class - javah -o RMHCSLAM.h -classpath $(JAVADIR) -jni edu.wlu.cs.levy.breezyslam.algorithms.RMHCSLAM - -coreslam.o: $(CDIR)/coreslam.c $(CDIR)/coreslam.h - gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/coreslam.c - -coreslam_$(ARCH).o: $(CDIR)/coreslam_$(ARCH).c $(CDIR)/coreslam.h - gcc -O3 -c -Wall $(CFLAGS) $(SIMD_FLAGS) $(CDIR)/coreslam_$(ARCH).c - -random.o: $(CDIR)/random.c - gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/random.c - -ziggurat.o: $(CDIR)/ziggurat.c - gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/ziggurat.c - -clean: - rm -f *.class *.o *.h *.$(LIBEXT) *~ - -backup: - cp *.java bak - cp Makefile bak diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java deleted file mode 100644 index 06269ab..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java +++ /dev/null @@ -1,103 +0,0 @@ -/** -* RMHCSLAM implements SinglePositionSLAM using random-mutation hill-climbing search on the starting position. -* -* Copyright (C) 2014 Simon D. Levy -* -* This code is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as -* published by the Free Software Foundation, either version 3 of the -* License, or (at your option) any later version. -* -* This code is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with this code. If not, see . -*/ - -package edu.wlu.cs.levy.breezyslam.algorithms; - -import edu.wlu.cs.levy.breezyslam.components.Position; -import edu.wlu.cs.levy.breezyslam.components.Laser; -import edu.wlu.cs.levy.breezyslam.components.PoseChange; -import edu.wlu.cs.levy.breezyslam.components.Map; -import edu.wlu.cs.levy.breezyslam.components.Scan; - -public class RMHCSLAM extends SinglePositionSLAM -{ - private static final double DEFAULT_SIGMA_XY_MM = 100; - private static final double DEFAULT_SIGMA_THETA_DEGREES = 20; - private static final int DEFAULT_MAX_SEARCH_ITER = 1000; - - static - { - System.loadLibrary("jnibreezyslam_algorithms"); - } - - private native void init(int random_seed); - - private native Object positionSearch( - Position start_pos, - Map map, - Scan scan, - double sigma_xy_mm, - double sigma_theta_degrees, - int max_search_iter); - - - private long native_ptr; - - /** - * Creates an RMHCSLAM object. - * @param laser a Laser object containing parameters for your Lidar equipment - * @param map_size_pixels the size of the desired map (map is square) - * @param map_size_meters the size of the area to be mapped, in meters - * @param random_seed seed for psuedorandom number generator in particle filter - * @return a new CoreSLAM object - */ - public RMHCSLAM(Laser laser, int map_size_pixels, double map_size_meters, int random_seed) - { - super(laser, map_size_pixels, map_size_meters); - - this.init(random_seed); - } - - /** - * The standard deviation in millimeters of the Gaussian distribution of - * the (X,Y) component of position in the particle filter; default = 100 - */ - public double sigma_xy_mm = DEFAULT_SIGMA_XY_MM;; - - /** - * The standard deviation in degrees of the Gaussian distribution of - * the angular rotation component of position in the particle filter; default = 20 - */ - public double sigma_theta_degrees = DEFAULT_SIGMA_THETA_DEGREES; - - /** - * The maximum number of iterations for particle-filter search; default = 1000 - */ - public int max_search_iter = DEFAULT_MAX_SEARCH_ITER; - - /** - * Returns a new position based on RMHC search from a starting position. Called automatically by - * SinglePositionSLAM::updateMapAndPointcloud() - * @param start_position the starting position - */ - protected Position getNewPosition(Position start_position) - { - Position newpos = - (Position)positionSearch( - start_position, - this.map, - this.scan_for_distance, - this.sigma_xy_mm, - this.sigma_theta_degrees, - this.max_search_iter); - - return newpos; - } - -} // RMHCSLAM diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java deleted file mode 100644 index 085064b..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java +++ /dev/null @@ -1,118 +0,0 @@ -/** -* SinglePositionSLAM is an abstract class that implements CoreSLAM using a point-cloud -* with a single point (particle, position). Implementing classes should provide the method -* -* Position getNewPosition(Position start_position) -* -* to compute a new position based on searching from a starting position. -* -* Copyright (C) 2014 Simon D. Levy -* -* This code is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as -* published by the Free Software Foundation, either version 3 of the -* License, or (at your option) any later version. -* -* This code is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with this code. If not, see . -*/ - -package edu.wlu.cs.levy.breezyslam.algorithms; - -import edu.wlu.cs.levy.breezyslam.components.Position; -import edu.wlu.cs.levy.breezyslam.components.PoseChange; -import edu.wlu.cs.levy.breezyslam.components.Laser; - - -public abstract class SinglePositionSLAM extends CoreSLAM -{ - /** - * Returns the current position. - * @return the current position as a Position object. - */ - public Position getpos() - { - return this.position; - } - - /** - * Creates a SinglePositionSLAM object. - * @param laser a Laser object containing parameters for your Lidar equipment - * @param map_size_pixels the size of the desired map (map is square) - * @param map_size_meters the size of the area to be mapped, in meters - * @return a new SinglePositionSLAM object - */ - protected SinglePositionSLAM(Laser laser, int map_size_pixels, double map_size_meters) - { - super(laser, map_size_pixels, map_size_meters); - - this.position = new Position(this.init_coord_mm(), this.init_coord_mm(), 0); - } - - - /** - * Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM::update() - * @param poseChange poseChange for odometry - */ - protected void updateMapAndPointcloud(PoseChange poseChange) - { - // Start at current position - Position start_pos = new Position(this.position); - - // Add effect of poseChange - start_pos.x_mm += poseChange.getDxyMm() * this.costheta(); - start_pos.y_mm += poseChange.getDxyMm() * this.sintheta(); - start_pos.theta_degrees += poseChange.getDthetaDegrees(); - - // Add offset from laser - start_pos.x_mm += this.laser.getOffsetMm() * this.costheta(); - start_pos.y_mm += this.laser.getOffsetMm() * this.sintheta(); - - // Get new position from implementing class - Position new_position = this.getNewPosition(start_pos); - - // Update the map with this new position - this.map.update(this.scan_for_mapbuild, new_position, this.map_quality, this.hole_width_mm); - - // Update the current position with this new position, adjusted by laser offset - this.position = new Position(new_position); - this.position.x_mm -= this.laser.getOffsetMm() * this.costheta(); - this.position.y_mm -= this.laser.getOffsetMm() * this.sintheta(); - } - - /** - * Returns a new position based on searching from a starting position. Called automatically by - * SinglePositionSLAM::updateMapAndPointcloud() - * @param start_pos the starting position - */ - - protected abstract Position getNewPosition(Position start_pos); - - private Position position; - - private double theta_radians() - { - return java.lang.Math.toRadians(this.position.theta_degrees); - } - - private double costheta() - { - return java.lang.Math.cos(this.theta_radians()); - } - - private double sintheta() - { - return java.lang.Math.sin(this.theta_radians()); - } - - private double init_coord_mm() - { - // Center of map - return 500 * this.map.sizeMeters(); - } -} diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c b/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c deleted file mode 100644 index e2e8d8f..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c +++ /dev/null @@ -1,67 +0,0 @@ -/* - * jnibreezyslam_algorithms.c Java Native Interface code for BreezySLAM algorithms - * - * Copyright (C) 2014 Simon D. Levy - * - * This code is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as - * published by the Free Software Foundation, either version 3 of the - * License, or (at your option) any later version. - * - * This code is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public License - * along with this code. If not, see . - */ - - -#include "../../../../../../../c/random.h" -#include "../jni_utils.h" - -#include - - -// RMHC_SLAM methods ----------------------------------------------------------------------------------------- - -JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_algorithms_RMHCSLAM_init (JNIEnv *env, jobject thisobject, jint random_seed) -{ - ptr_to_obj(env, thisobject, random_new(random_seed)); -} - -JNIEXPORT jobject JNICALL Java_edu_wlu_cs_levy_breezyslam_algorithms_RMHCSLAM_positionSearch (JNIEnv *env, jobject thisobject, - jobject startpos_object, - jobject map_object, - jobject scan_object, - jdouble sigma_xy_mm, - jdouble sigma_theta_degrees, - jint max_search_iter) -{ - position_t startpos; - - startpos.x_mm = get_double_field(env, startpos_object, "x_mm"); - startpos.y_mm = get_double_field(env, startpos_object, "y_mm"); - startpos.theta_degrees = get_double_field(env, startpos_object, "theta_degrees"); - - void * random = ptr_from_obj(env, thisobject); - - position_t newpos = - rmhc_position_search( - startpos, - cmap_from_jmap(env, map_object), - cscan_from_jscan(env, scan_object), - sigma_xy_mm, - sigma_theta_degrees, - max_search_iter, - random); - - jclass cls = (*env)->FindClass(env, "edu/wlu/cs/levy/breezyslam/components/Position"); - - jmethodID constructor = (*env)->GetMethodID(env, cls, "", "(DDD)V"); - - jobject newpos_object = (*env)->NewObject(env, cls, constructor, newpos.x_mm, newpos.y_mm, newpos.theta_degrees); - - return newpos_object; -} diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Laser.java b/java/edu/wlu/cs/levy/breezyslam/components/Laser.java deleted file mode 100644 index aa77425..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/components/Laser.java +++ /dev/null @@ -1,108 +0,0 @@ -/** -* -* BreezySLAM: Simple, efficient SLAM in Java -* -* Laser.java - Java code for Laser model class -* -* Copyright (C) 2014 Simon D. Levy -* -* This code is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as -* published by the Free Software Foundation, either version 3 of the -* License, or (at your option) any later version. -* -* This code is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with this code. If not, see . -*/ - -package edu.wlu.cs.levy.breezyslam.components; - -/** -* A class for scanning laser rangefinder (Lidar) parameters. -*/ -public class Laser -{ - - protected int scan_size; - protected double scan_rate_hz; - protected double detection_angle_degrees; - protected double distance_no_detection_mm; - protected int detection_margin; - protected double offset_mm; - - /** - * Builds a Laser object from parameters based on the specifications for your - * Lidar unit. - * @param scan_size number of rays per scan - * @param scan_rate_hz laser scan rate in Hertz - * @param detection_angle_degrees detection angle in degrees (e.g. 240, 360) - * @param detection_margin number of rays at edges of scan to ignore - * @param offset_mm forward/backward offset of laser motor from robot center - * @return a new Laser object - * - */ - public Laser( - int scan_size, - double scan_rate_hz, - double detection_angle_degrees, - double distance_no_detection_mm, - int detection_margin, - double offset_mm) - { - this.scan_size = scan_size; - this.scan_rate_hz = scan_rate_hz; - this.detection_angle_degrees = detection_angle_degrees; - this.distance_no_detection_mm = distance_no_detection_mm; - this.detection_margin = detection_margin; - this.offset_mm = offset_mm; - } - - /** - * Builds a Laser object by copying another Laser object. - * Lidar unit. - * @param laser the other Laser object - * - */ - public Laser(Laser laser) - { - this.scan_size = laser.scan_size; - this.scan_rate_hz = laser.scan_rate_hz; - this.detection_angle_degrees = laser.detection_angle_degrees; - this.distance_no_detection_mm = laser.distance_no_detection_mm; - this.detection_margin = laser.detection_margin; - this.offset_mm = laser.offset_mm; - } - - /** - * Returns a string representation of this Laser object. - */ - - public String toString() - { - String format = "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"; - - return String.format(format, this.scan_size, this.scan_rate_hz, - this.detection_angle_degrees, - this.distance_no_detection_mm, - this.detection_margin, - this.offset_mm); - - } - - /** - * Returns the offset of the laser in mm, from the center of the robot. - * - */ - public double getOffsetMm() - { - return this.offset_mm; - } -} diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Makefile b/java/edu/wlu/cs/levy/breezyslam/components/Makefile deleted file mode 100644 index 35a37bc..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/components/Makefile +++ /dev/null @@ -1,89 +0,0 @@ -# Makefile for BreezySLAM components in Java -# -# Copyright (C) 2014 Simon D. Levy -# -# This code is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as -# published by the Free Software Foundation, either version 3 of the -# License, or (at your option) any later version. -# -# This code is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. - -# You should have received a copy of the GNU Lesser General Public License -# along with this code. If not, see . - -BASEDIR = ../../../../../../.. -JAVADIR = $(BASEDIR)/java -CDIR = $(BASEDIR)/c -JFLAGS = -Xlint -JDKINC = -I /usr/lib/jvm/java-7-openjdk-amd64/include -#JDKINC = -I /opt/jdk1.7.0_67/include -I /opt/jdk1.7.0_67/include/linux - -# Set library extension based on OS -ifeq ("$(shell uname)","Darwin") - LIBEXT = dylib -else ifeq ("$(shell uname)","Linux") - CFLAGS = -fPIC - LIBEXT = so -else - LIBEXT = dll -endif - -ARCH = $(shell uname -m) - -# Set SIMD compile params based on architecture -ifeq ("$(ARCH)","armv7l") - SIMD_FLAGS = -mfpu=neon -else ifeq ("$(ARCH)","i686") - SIMD_FLAGS = -msse3 -else - ARCH = sisd -endif - -ALL = libjnibreezyslam_components.$(LIBEXT) Laser.class Position.class PoseChange.class URG04LX.class - -all: $(ALL) - -libjnibreezyslam_components.$(LIBEXT): jnibreezyslam_components.o coreslam.o coreslam_$(ARCH).o - gcc -shared -Wl,-soname,libjnibreezyslam_components.so -o libjnibreezyslam_components.so jnibreezyslam_components.o \ - coreslam.o coreslam_$(ARCH).o - -jnibreezyslam_components.o: jnibreezyslam_components.c Map.h Scan.h ../jni_utils.h - gcc $(JDKINC) -fPIC -c jnibreezyslam_components.c - -coreslam.o: $(CDIR)/coreslam.c $(CDIR)/coreslam.h - gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/coreslam.c - -coreslam_$(ARCH).o: $(CDIR)/coreslam_$(ARCH).c $(CDIR)/coreslam.h - gcc -O3 -c -Wall $(CFLAGS) $(SIMD_FLAGS) $(CDIR)/coreslam_$(ARCH).c - -Map.h: Map.class - javah -o Map.h -classpath $(JAVADIR) -jni edu.wlu.cs.levy.breezyslam.components.Map - -Map.class: Map.java Scan.class Position.class - javac $(JFLAGS) -classpath $(JAVADIR) Map.java - -Scan.h: Scan.class - javah -o Scan.h -classpath $(JAVADIR) -jni edu.wlu.cs.levy.breezyslam.components.Scan - -Scan.class: Scan.java Laser.class - javac $(JFLAGS) -classpath $(JAVADIR) Scan.java - -Laser.class: Laser.java - javac $(JFLAGS) Laser.java - -URG04LX.class: URG04LX.java Laser.class - javac $(JFLAGS) -classpath $(JAVADIR) URG04LX.java - -PoseChange.class: PoseChange.java - javac $(JFLAGS) PoseChange.java - - -Position.class: Position.java - javac $(JFLAGS) Position.java - -clean: - rm -f *.so *.class *.o *.h *~ diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Map.java b/java/edu/wlu/cs/levy/breezyslam/components/Map.java deleted file mode 100644 index 4bfa2f6..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/components/Map.java +++ /dev/null @@ -1,96 +0,0 @@ -/** -* -* BreezySLAM: Simple, efficient SLAM in Java -* -* Map.java - Java code for Map class -* -* Copyright (C) 2014 Simon D. Levy -* -* This code is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as -* published by the Free Software Foundation, either version 3 of the -* License, or (at your option) any later version. -* -* This code is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with this code. If not, see . -*/ - -package edu.wlu.cs.levy.breezyslam.components; - -/** -* A class for maps used in SLAM. -*/ -public class Map -{ - static - { - System.loadLibrary("jnibreezyslam_components"); - } - - private native void init(int size_pixels, double size_meters); - - private double size_meters; - - private native void update( - Scan scan, - double position_x_mm, - double position_y_mm, - double position_theta_degrees, - int quality, - double hole_width_mm); - - private long native_ptr; - - /** - * Returns a string representation of this Map object. - */ - public native String toString(); - - /** - * Builds a square Map object. - * @param size_pixels size in pixels - * @param size_meters size in meters - * - */ - public Map(int size_pixels, double size_meters) - { - this.init(size_pixels, size_meters); - - // for public accessor - this.size_meters = size_meters; - } - - /** - * Puts current map values into bytearray, which should of which should be of - * this.size map_size_pixels ^ 2. - * @param bytes byte array that gets the map values - */ - public native void get(byte [] bytes); - - /** - * Updates this map object based on new data. - * @param scan a new scan - * @param position a new postion - * @param quality speed with which scan is integerate into map (0 through 255) - * @param hole_width_mm hole width in millimeters - * - */ - public void update(Scan scan, Position position, int quality, double hole_width_mm) - { - this.update(scan, position.x_mm, position.y_mm, position.theta_degrees, quality, hole_width_mm); - } - - /** - * Returns the size of this map in meters. - */ - public double sizeMeters() - { - return this.size_meters; - } - -} diff --git a/java/edu/wlu/cs/levy/breezyslam/components/PoseChange.java b/java/edu/wlu/cs/levy/breezyslam/components/PoseChange.java deleted file mode 100644 index 7682724..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/components/PoseChange.java +++ /dev/null @@ -1,115 +0,0 @@ -/** -* -* BreezySLAM: Simple, efficient SLAM in Java -* -* PoseChange.java - Java code for PoseChange class, encoding triple -* (dxy_mm, dtheta_degrees, dt_seconds) -* -* Copyright (C) 2014 Simon D. Levy -* -* This code is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as -* published by the Free Software Foundation, either version 3 of the -* License, or (at your option) any later version. -* -* This code is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with this code. If not, see . -*/ - -package edu.wlu.cs.levy.breezyslam.components; - -/** -* A class representing the forward and angular velocities of a robot as determined by odometry. -*/ -public class PoseChange -{ - - /** - * Creates a new PoseChange object with specified velocities. - */ - public PoseChange(double dxy_mm, double dtheta_degrees, double dtSeconds) - { - this.dxy_mm = dxy_mm; - this.dtheta_degrees = dtheta_degrees; - this.dt_seconds = dtSeconds; - } - - /** - * Creates a new PoseChange object with zero velocities. - */ - public PoseChange() - { - this.dxy_mm = 0; - this.dtheta_degrees = 0; - this.dt_seconds = 0; - } - - /** - * Updates this PoseChange object. - * @param dxy_mm new forward distance traveled in millimeters - * @param dtheta_degrees new angular rotation in degrees - * @param dtSeconds time in seconds since last velocities - */ - public void update(double dxy_mm, double dtheta_degrees, double dtSeconds) - { - double velocity_factor = (dtSeconds > 0) ? (1 / dtSeconds) : 0; - - this.dxy_mm = dxy_mm * velocity_factor; - - this.dtheta_degrees = dtheta_degrees * velocity_factor; - } - - /** - * Returns a string representation of this PoseChange object. - */ - public String toString() - { - return String.format(". - */ - -package edu.wlu.cs.levy.breezyslam.components; - -/** - * A class representing the position of a robot. - */ -public class Position -{ - - /** - * Distance of robot from left edge of map, in millimeters. - */ - public double x_mm; - - /** - * Distance of robot from top edge of map, in millimeters. - */ - public double y_mm; - - /** - * Clockwise rotation of robot with respect to three o'clock (east), in degrees. - */ - public double theta_degrees; - - /** - * Constructs a new position. - * @param x_mm X coordinate in millimeters - * @param y_mm Y coordinate in millimeters - * @param theta_degrees rotation angle in degrees - */ - public Position(double x_mm, double y_mm, double theta_degrees) - { - this.x_mm = x_mm; - this.y_mm = y_mm; - this.theta_degrees = theta_degrees; - } - - /** - * Constructs a new Position object by copying another. - * @param the other Positon object - */ - - public Position(Position position) - { - this.x_mm = position.x_mm; - this.y_mm = position.y_mm; - this.theta_degrees = position.theta_degrees; - } - - /** - * Returns a string representation of this Position object. - */ - public String toString() - { - String format = ""; - - return String.format(format, this.x_mm, this.y_mm, this.theta_degrees); - - } -} diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Scan.java b/java/edu/wlu/cs/levy/breezyslam/components/Scan.java deleted file mode 100644 index 9f898a8..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/components/Scan.java +++ /dev/null @@ -1,97 +0,0 @@ -/** -* -* BreezySLAM: Simple, efficient SLAM in Java -* -* Scan.java - Java code for Scan class -* -* Copyright (C) 2014 Simon D. Levy -* -* This code is free software: you can redistribute it and/or modify -* it under the terms of the GNU Lesser General Public License as -* published by the Free Software Foundation, either version 3 of the -* License, or (at your option) any later version. -* -* This code is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU Lesser General Public License -* along with this code. If not, see . -*/ - -package edu.wlu.cs.levy.breezyslam.components; - -/** -* A class for Lidar scans. -*/ -public class Scan -{ - static - { - System.loadLibrary("jnibreezyslam_components"); - } - - private native void init( - int span, - int scan_size, - double scan_rate_hz, - double detection_angle_degrees, - double distance_no_detection_mm, - int detection_margin, - double offset_mm); - - private long native_ptr; - - public native String toString(); - - /** - * Returns a string representation of this Scan object. - */ - public native void update( - int [] lidar_mm, - double hole_width_mm, - double poseChange_dxy_mm, - double poseChange_dtheta_degrees); - - - /** - * Builds a Scan object. - * @param laser laser parameters - * @param span supports spanning laser scan to cover the space better. - * - */ - public Scan(Laser laser, int span) - { - this.init(span, - laser.scan_size, - laser.scan_rate_hz, - laser.detection_angle_degrees, - laser.distance_no_detection_mm, - laser.detection_margin, - laser.offset_mm); - } - - /** - * Builds a Scan object. - * @param laser laser parameters - * - */ - public Scan(Laser laser) - { - this(laser, 1); - } - - /** - * Updates this Scan object with new values from a Lidar scan. - * @param scanvals_mm scanned Lidar distance values in millimeters - * @param hole_width_millimeters hole width in millimeters - * @param poseChange forward velocity and angular velocity of robot at scan time - * - */ - public void update(int [] scanvals_mm, double hole_width_millimeters, PoseChange poseChange) - { - this.update(scanvals_mm, hole_width_millimeters, poseChange.dxy_mm, poseChange.dtheta_degrees); - } -} - diff --git a/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java b/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java deleted file mode 100644 index 053b174..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java +++ /dev/null @@ -1,29 +0,0 @@ -package edu.wlu.cs.levy.breezyslam.components; - -/** - * A class for the Hokuyo URG-04LX laser. - */ -public class URG04LX extends Laser -{ - - /** - * Builds a URG04LX object. - * Lidar unit. - * @param detection_margin number of rays at edges of scan to ignore - * @param offset_mm forward/backward offset of laser motor from robot center - * @return a new URG04LX object - * - */ - public URG04LX(int detection_margin, double offset_mm) - { - super(682, 10, 240, 4000, detection_margin, offset_mm); - } - - /** - * Builds a URG04LX object with zero detection margin, offset mm. - */ - public URG04LX() - { - this(0, 0); - } -} diff --git a/java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c b/java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c deleted file mode 100644 index 6e4d8fa..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c +++ /dev/null @@ -1,132 +0,0 @@ -/* - * jnibreezyslam_components.c Java Native Interface code for BreezySLAM components - * - * Copyright (C) 2014 Simon D. Levy - * - * This code is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as - * published by the Free Software Foundation, either version 3 of the - * License, or (at your option) any later version. - * - * This code is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public License - * along with this code. If not, see . - */ - -#include "../jni_utils.h" - -#include "Map.h" -#include "Scan.h" - -#include -#include - -#define MAXSTR 100 - -// Map methods ----------------------------------------------------------------------------------------- - -JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_init (JNIEnv *env, jobject thisobject, jint size_pixels, jdouble size_meters) -{ - map_t * map = (map_t *)malloc(sizeof(map_t)); - map_init(map, (int)size_pixels, (double)size_meters); - ptr_to_obj(env, thisobject, map); -} - -JNIEXPORT jstring JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_toString (JNIEnv *env, jobject thisobject) -{ - map_t * map = cmap_from_jmap(env, thisobject); - - char str[MAXSTR]; - - map_string(*map, str); - - return (*env)->NewStringUTF(env, str); -} - -JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_get (JNIEnv *env, jobject thisobject, jbyteArray bytes) -{ - map_t * map = cmap_from_jmap(env, thisobject); - - jbyte * ptr = (*env)->GetByteArrayElements(env, bytes, NULL); - - map_get(map, ptr); - - (*env)->ReleaseByteArrayElements(env, bytes, ptr, 0); -} - -JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_update (JNIEnv *env, jobject thisobject, - jobject scanobject, - jdouble position_x_mm, - jdouble position_y_mm, - jdouble position_theta_degrees, - jint quality, - jdouble hole_width_mm) -{ - map_t * map = cmap_from_jmap(env, thisobject); - - scan_t * scan = cscan_from_jscan(env, scanobject); - - position_t position; - - position.x_mm = position_x_mm; - position.y_mm = position_y_mm; - position.theta_degrees = position_theta_degrees; - - map_update(map, scan, position, quality, hole_width_mm); -} - - -// Scan methods ----------------------------------------------------------------------------------------- - -JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Scan_init (JNIEnv *env, jobject thisobject, - jint span, - jint scan_size, - jdouble scan_rate_hz, - jdouble detection_angle_degrees, - jdouble distance_no_detection_mm, - jint detection_margin, - jdouble offset_mm) - { - scan_t * scan = (scan_t *)malloc(sizeof(scan_t)); - - scan_init(scan, - span, - scan_size, - scan_rate_hz, - detection_angle_degrees, - distance_no_detection_mm, - detection_margin, - offset_mm); - - ptr_to_obj(env, thisobject, scan); -} - -JNIEXPORT jstring JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Scan_toString (JNIEnv *env, jobject thisobject) -{ - scan_t * scan = cscan_from_jscan(env, thisobject); - - char str[MAXSTR]; - - scan_string(*scan, str); - - return (*env)->NewStringUTF(env, str); -} - -JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Scan_update (JNIEnv *env, jobject thisobject, - jintArray lidar_mm, - jdouble hole_width_mm, - jdouble velocities_dxy_mm, - jdouble velocities_dtheta_degrees) -{ - scan_t * scan = cscan_from_jscan(env, thisobject); - - jint * lidar_mm_c = (*env)->GetIntArrayElements(env, lidar_mm, 0); - - scan_update(scan, lidar_mm_c, hole_width_mm, velocities_dxy_mm, velocities_dtheta_degrees); - - (*env)->ReleaseIntArrayElements(env, lidar_mm, lidar_mm_c, 0); -} diff --git a/java/edu/wlu/cs/levy/breezyslam/jni_utils.h b/java/edu/wlu/cs/levy/breezyslam/jni_utils.h deleted file mode 100644 index acaa3a5..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/jni_utils.h +++ /dev/null @@ -1,64 +0,0 @@ -/* - * jni_utils.h Utilities for Java Native Interface code - * - * Copyright (C) 2014 Simon D. Levy - * - * This code is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as - * published by the Free Software Foundation, either version 3 of the - * License, or (at your option) any later version. - * - * This code is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public License - * along with this code. If not, see . - */ - - -#include -#include -#include "../../../../../../c/coreslam.h" - -#include -#include - -static jfieldID get_fid(JNIEnv *env, jobject object, const char * fieldname, const char * fieldsig) -{ - jclass cls = (*env)->GetObjectClass(env, object); - return (*env)->GetFieldID(env, cls, fieldname, fieldsig); -} - -static jfieldID get_this_fid(JNIEnv *env, jobject thisobject) -{ - return get_fid(env, thisobject, "native_ptr", "J"); -} - -static void * ptr_from_obj(JNIEnv *env, jobject thisobject) -{ - return (void *)(*env)->GetLongField (env, thisobject, get_this_fid(env, thisobject)); -} - -static void ptr_to_obj(JNIEnv *env, jobject thisobject, void * ptr) -{ - (*env)->SetLongField (env, thisobject, get_this_fid(env, thisobject), (long)ptr); -} - -static scan_t * cscan_from_jscan(JNIEnv *env, jobject thisobject) -{ - return (scan_t *)ptr_from_obj(env, thisobject); -} - -static map_t * cmap_from_jmap(JNIEnv *env, jobject thisobject) -{ - return (map_t *)ptr_from_obj(env, thisobject); -} - -static double get_double_field(JNIEnv *env, jobject object, const char * fieldname) -{ - return (*env)->GetDoubleField (env, object, get_fid(env, object, fieldname, "D")); -} - - diff --git a/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java b/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java deleted file mode 100644 index f2b2822..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java +++ /dev/null @@ -1,130 +0,0 @@ -/** - * - * BreezySLAM: Simple, efficient SLAM in Java - * - * WheeledRobot.java - Java class for wheeled robots - * - * Copyright (C) 2014 Simon D. Levy - * - * This code is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as - * published by the Free Software Foundation, either version 3 of the - * License, or (at your option) any later version. - * - * This code is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public License - * along with this code. If not, see . - */ - - -package edu.wlu.cs.levy.breezyslam.robots; - -import edu.wlu.cs.levy.breezyslam.components.PoseChange; - - -/** - * An abstract class for wheeled robots. Supports computation of forward and angular - * poseChange based on odometry. Your subclass should should implement the - * extractOdometry method. - */ -public abstract class WheeledRobot -{ - - /** - * Builds a WheeledRobot object. Parameters should be based on the specifications for - * your robot. - * @param wheel_radius_mm radius of each odometry wheel, in meters - * @param half_axle_length_mm half the length of the axle between the odometry wheels, in meters - * @return a new WheeledRobot object - */ - protected WheeledRobot(double wheel_radius_mm, double half_axle_length_mm) - { - this.wheel_radius_mm = wheel_radius_mm; - this.half_axle_length_mm = half_axle_length_mm; - - this.timestamp_seconds_prev = 0; - this.left_wheel_degrees_prev = 0; - this.right_wheel_degrees_prev = 0; - } - - public String toString() - { - - return String.format("", - this.wheel_radius_mm, this.half_axle_length_mm, this.descriptorString()); - } - - /** - * Computes forward and angular poseChange based on odometry. - * @param timestamp time stamp, in whatever units your robot uses - * @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses - * @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses - * @return poseChange object representing poseChange for these odometry values - */ - - public PoseChange computePoseChange( double timestamp, double left_wheel_odometry, double right_wheel_odometry) - { - WheelOdometry odometry = this.extractOdometry(timestamp, left_wheel_odometry, right_wheel_odometry); - - double dxy_mm = 0; - double dtheta_degrees = 0; - double dt_seconds = 0; - - if (this.timestamp_seconds_prev > 0) - { - double left_diff_degrees = odometry.left_wheel_degrees - this.left_wheel_degrees_prev; - double right_diff_degrees = odometry.right_wheel_degrees - this.right_wheel_degrees_prev; - - dxy_mm = this.wheel_radius_mm * (java.lang.Math.toRadians(left_diff_degrees) + java.lang.Math.toRadians(right_diff_degrees)); - - dtheta_degrees = this.wheel_radius_mm / this.half_axle_length_mm * (right_diff_degrees - left_diff_degrees); - - dt_seconds = odometry.timestamp_seconds - this.timestamp_seconds_prev; - } - - // Store current odometry for next time - this.timestamp_seconds_prev = odometry.timestamp_seconds; - this.left_wheel_degrees_prev = odometry.left_wheel_degrees; - this.right_wheel_degrees_prev = odometry.right_wheel_degrees; - - return new PoseChange(dxy_mm, dtheta_degrees, dt_seconds); - } - - /** - * Extracts usable odometry values from your robot's odometry. - * @param timestamp time stamp, in whatever units your robot uses - * @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses - * @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses - * @return WheelOdometry object containing timestamp in seconds, left wheel degrees, right wheel degrees - */ - protected abstract WheelOdometry extractOdometry(double timestamp, double left_wheel_odometry, double right_wheel_odometry); - - protected abstract String descriptorString(); - - protected class WheelOdometry - { - public WheelOdometry(double timestamp_seconds, double left_wheel_degrees, double right_wheel_degrees) - { - this.timestamp_seconds = timestamp_seconds; - this.left_wheel_degrees = left_wheel_degrees; - this.right_wheel_degrees = right_wheel_degrees; - } - - public double timestamp_seconds; - public double left_wheel_degrees; - public double right_wheel_degrees; - - } - - - private double wheel_radius_mm; - private double half_axle_length_mm; - - private double timestamp_seconds_prev; - private double left_wheel_degrees_prev; - private double right_wheel_degrees_prev; -} diff --git a/matlab/CoreSLAM.m b/matlab/CoreSLAM.m deleted file mode 100644 index ad17c74..0000000 --- a/matlab/CoreSLAM.m +++ /dev/null @@ -1,136 +0,0 @@ -classdef CoreSLAM - %CoreSLAM CoreSLAM abstract class for BreezySLAM - % CoreSLAM is an abstract class that uses the classes Position, - % Map, Scan, and Laser to run variants of the simple CoreSLAM - % (tinySLAM) algorithm described in - % - % @inproceedings{coreslam-2010, - % author = {Bruno Steux and Oussama El Hamzaoui}, - % title = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code}, - % booktitle = {11th International Conference on Control, Automation, - % Robotics and Vision, ICARCV 2010, Singapore, 7-10 - % December 2010, Proceedings}, - % pages = {1975-1979}, - % publisher = {IEEE}, - % year = {2010} - % } - % - % - % Implementing classes should provide the method - % - % updateMapAndPointcloud(scan_mm, velocities) - % - % to update the map and point-cloud (particle cloud). - % - % Copyright (C) 2014 Simon D. Levy - % - % This code is free software: you can redistribute it and/or modify - % it under the terms of the GNU Lesser General Public License as - % published by the Free Software Foundation, either version 3 of the - % License, or (at your option) any later version. - % - % This code is distributed in the hope that it will be useful, - % but WITHOUT ANY WARRANTY without even the implied warranty of - % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - % GNU General Public License for more details. - % - % You should have received a copy of the GNU Lesser General Public License - % along with this code. If not, see . - - properties (Access = 'public') - map_quality = 50; % The quality of the map (0 through 255); default = 50 - hole_width_mm = 600; % The width in millimeters of each "hole" in the map (essentially, wall width); default = 600 - end - - properties (Access = 'protected') - laser % (internal) - scan_for_distance % (internal) - scan_for_mapbuild % (internal) - map % (internal) - velocities % (internal) - end - - methods (Abstract, Access = 'protected') - - updateMapAndPointcloud(slam, velocities) - - end - - methods (Access = 'private') - - function scan_update(slam, scanobj, scans_mm) - scanobj.update(scans_mm, slam.hole_width_mm, slam.velocities) - end - - end - - methods (Access = 'protected') - - function slam = CoreSLAM(laser, map_size_pixels, map_size_meters) - % Creates a CoreSLAM object suitable for updating with new Lidar and odometry data. - % CoreSLAM(laser, map_size_pixels, map_size_meters) - % laser is a Laser object representing the specifications of your Lidar unit - % map_size_pixels is the size of the square map in pixels - % map_size_meters is the size of the square map in meters - - % Store laser for later - slam.laser = laser; - - % Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry - slam.velocities = [0, 0, 0]; - - % Initialize a scan for computing distance to map, and one for updating map - slam.scan_for_distance = Scan(laser, 1); - slam.scan_for_mapbuild = Scan(laser, 3); - - % Initialize the map - slam.map = Map(map_size_pixels, map_size_meters); - - end - end - - methods (Access = 'public') - - function slam = update(slam, scans_mm, velocities) - % Updates the scan and odometry. - % Calls the the implementing class's updateMapAndPointcloud() - % method with the specified velocities. - % - % slam = update(slam, scans_mm, [velocities]) - % - % scan_mm is a list of Lidar scan values, whose count is specified in the scan_size - % velocities is an optional list of velocities [dxy_mm, dtheta_degrees, dt_seconds] for odometry - - % Build a scan for computing distance to map, and one for updating map - slam.scan_update(slam.scan_for_mapbuild, scans_mm) - slam.scan_update(slam.scan_for_distance, scans_mm) - - % Default to zero velocities - if nargin < 3 - velocities = [0, 0, 0]; - end - - % Update velocities - velocity_factor = 0; - if velocities(3) > 0 - velocity_factor = 1 / velocities(3); - end - new_dxy_mm = velocities(1) * velocity_factor; - new_dtheta_degrees = velocities(2) * velocity_factor; - slam.velocities = [new_dxy_mm, new_dtheta_degrees, 0]; - - % Implementing class updates map and pointcloud - slam = slam.updateMapAndPointcloud(velocities); - - end - - function map = getmap(slam) - % Returns the current map. - % Map is returned as an occupancy grid (matrix of pixels). - map = slam.map.get(); - end - - end - -end - diff --git a/matlab/Deterministic_SLAM.m b/matlab/Deterministic_SLAM.m deleted file mode 100644 index 2b7c8e7..0000000 --- a/matlab/Deterministic_SLAM.m +++ /dev/null @@ -1,44 +0,0 @@ -classdef Deterministic_SLAM < SinglePositionSLAM - %Deterministic_SLAM SLAM with no Monte Carlo search - % Implements the getNewPosition() method of SinglePositionSLAM - % by copying the start position. - % - % Copyright (C) 2014 Simon D. Levy - % - % This code is free software: you can redistribute it and/or modify - % it under the terms of the GNU Lesser General Public License as - % published by the Free Software Foundation, either version 3 of the - % License, or (at your option) any later version. - % - % This code is distributed in the hope that it will be useful, - % but WITHOUT ANY WARRANTY without even the implied warranty of - % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - % GNU General Public License for more details. - % - % You should have received a copy of the GNU Lesser General Public License - % along with this code. If not, see . - - methods - - function slam = Deterministic_SLAM(laser, map_size_pixels, map_size_meters) - %Creates a Deterministic_SLAM object suitable for updating with new Lidar and odometry data. - % slam = Deterministic_SLAM(laser, map_size_pixels, map_size_meters) - % laser is a Laser object representing the specifications of your Lidar unit - % map_size_pixels is the size of the square map in pixels - % map_size_meters is the size of the square map in meters - - slam = slam@SinglePositionSLAM(laser, map_size_pixels, map_size_meters); - - end - - function new_pos = getNewPosition(~, start_pos) - % Implements the _getNewPosition() method of SinglePositionSLAM. - % new_pos = getNewPosition(~, start_pos) simply returns start_pos - - new_pos = start_pos; - end - - end - -end - diff --git a/matlab/Map.m b/matlab/Map.m deleted file mode 100644 index e3f960e..0000000 --- a/matlab/Map.m +++ /dev/null @@ -1,59 +0,0 @@ -classdef Map - %A class for maps (occupancy grids) used in SLAM - % - % Copyright (C) 2014 Simon D. Levy - % - % This code is free software: you can redistribute it and/or modify - % it under the terms of the GNU Lesser General Public License as - % published by the Free Software Foundation, either version 3 of the - % License, or (at your option) any later version. - % - % This code is distributed in the hope that it will be useful, - % but WITHOUT ANY WARRANTY without even the implied warranty of - % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - % GNU General Public License for more details. - % - % You should have received a copy of the GNU Lesser General Public License - % along with this code. If not, see . - - properties (Access = {?RMHC_SLAM}) - - c_map - end - - methods (Access = 'public') - - function map = Map(size_pixels, size_meters) - % Map creates an empty square map - % map = Map(size_pixels, size_meters) - map.c_map = mex_breezyslam('Map_init', size_pixels, size_meters); - - end - - function disp(map) - % Displays data about this map - mex_breezyslam('Map_disp', map.c_map) - - end - - function update(map, scan, new_position, map_quality, hole_width_mm) - % Updates this map with a new scan and position - % - % update(map, scan, new_position, map_quality, hole_width_mm) - mex_breezyslam('Map_update', map.c_map, scan.c_scan, new_position, int32(map_quality), hole_width_mm) - - end - - function bytes = get(map) - % Returns occupancy grid matrix of bytes for this map - % - % bytes = get(map) - - % Transpose for uniformity with Python, C++ versions - bytes = mex_breezyslam('Map_get', map.c_map)'; - - end - - end % methods - -end % classdef diff --git a/matlab/RMHC_SLAM.m b/matlab/RMHC_SLAM.m deleted file mode 100644 index 0062574..0000000 --- a/matlab/RMHC_SLAM.m +++ /dev/null @@ -1,72 +0,0 @@ -classdef RMHC_SLAM < SinglePositionSLAM - %RMHC_SLAM Random Mutation Hill-Climbing SLAM - % Implements the getNewPosition() method of SinglePositionSLAM - % using Random-Mutation Hill-Climbing search. Uses its own internal - % pseudorandom-number generator for efficiency. - % - % Copyright (C) 2014 Simon D. Levy - % - % This code is free software: you can redistribute it and/or modify - % it under the terms of the GNU Lesser General Public License as - % published by the Free Software Foundation, either version 3 of the - % License, or (at your option) any later version. - % - % This code is distributed in the hope that it will be useful, - % but WITHOUT ANY WARRANTY without even the implied warranty of - % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - % GNU General Public License for more details. - % - % You should have received a copy of the GNU Lesser General Public License - % along with this code. If not, see . - - properties (Access = 'public') - sigma_xy_mm = 100; % std. dev. of X/Y component of search - sigma_theta_degrees = 20; % std. dev. of angular component of search - max_search_iter = 1000; % max. # of search iterations per update - end - - properties (Access = 'private') - c_randomizer - end - - methods - - function slam = RMHC_SLAM(laser, map_size_pixels, map_size_meters, random_seed) - %Creates an RMHC_SLAM object suitable for updating with new Lidar and odometry data. - % slam = RMHC_SLAM(laser, map_size_pixels, map_size_meters, random_seed) - % laser is a Laser object representing the specifications of your Lidar unit - % map_size_pixels is the size of the square map in pixels - % map_size_meters is the size of the square map in meters - % random_seed supports reproducible results; defaults to system time if unspecified - - slam = slam@SinglePositionSLAM(laser, map_size_pixels, map_size_meters); - - if nargin < 3 - random_seed = floor(cputime) & hex2dec('FFFF'); - end - - slam.c_randomizer = mex_breezyslam('Randomizer_init', random_seed); - - end - - function new_pos = getNewPosition(slam, start_pos) - % Implements the _getNewPosition() method of SinglePositionSLAM. - % Uses Random-Mutation Hill-Climbing search to look for a - % better position based on a starting position. - - [new_pos.x_mm,new_pos.y_mm,new_pos.theta_degrees] = ... - mex_breezyslam('rmhcPositionSearch', ... - start_pos, ... - slam.map.c_map, ... - slam.scan_for_distance.c_scan, ... - slam.laser,... - slam.sigma_xy_mm,... - slam.sigma_theta_degrees,... - slam.max_search_iter,... - slam.c_randomizer); - end - - end - -end - diff --git a/matlab/Scan.m b/matlab/Scan.m deleted file mode 100644 index 4004572..0000000 --- a/matlab/Scan.m +++ /dev/null @@ -1,66 +0,0 @@ -classdef Scan - %A class for Lidar scans used in SLAM - % - % Copyright (C) 2014 Simon D. Levy - % - % This code is free software: you can redistribute it and/or modify - % it under the terms of the GNU Lesser General Public License as - % published by the Free Software Foundation, either version 3 of the - % License, or (at your option) any later version. - % - % This code is distributed in the hope that it will be useful, - % but WITHOUT ANY WARRANTY without even the implied warranty of - % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - % GNU General Public License for more details. - % - % You should have received a copy of the GNU Lesser General Public License - % along with this code. If not, see . - - properties (Access = {?Map, ?RMHC_SLAM}) - - c_scan - end - - methods - - function scan = Scan(laser, span) - % Creates a new scan object - % scan = Scan(laser, [span]) - % laser is a structure with the fields: - % scan_size - % scan_rate_hz - % detection_angle_degrees - % distance_no_detection_mm - % distance_no_detection_mm - % detection_margin - % offset_mm = offset_mm - % span (default=1) supports spanning laser scan to cover the space better - - if nargin < 2 - span = 1; - end - - scan.c_scan = mex_breezyslam('Scan_init', laser, span); - - end - - function disp(scan) - % Displays information about this Scan - - mex_breezyslam('Scan_disp', scan.c_scan) - - end - - function update(scan, scans_mm, hole_width_mm, velocities) - % Updates scan with new lidar and velocity data - % update(scans_mm, hole_width_mm, [velocities]) - % scans_mm is a list of integers representing scanned distances in mm. - % hole_width_mm is the width of holes (obstacles, walls) in millimeters. - % velocities is an optional list[dxy_mm, dtheta_degrees] - % i.e., robot's (forward, rotational velocity) for improving the quality of the scan. - mex_breezyslam('Scan_update', scan.c_scan, int32(scans_mm), hole_width_mm, velocities) - end - - end - -end diff --git a/matlab/SinglePositionSLAM.m b/matlab/SinglePositionSLAM.m deleted file mode 100644 index 787753c..0000000 --- a/matlab/SinglePositionSLAM.m +++ /dev/null @@ -1,101 +0,0 @@ -classdef SinglePositionSLAM < CoreSLAM - %SinglePositionSLAM Abstract class for CoreSLAM with single-point cloud - % Implementing classes should provide the method - % - % getNewPosition(self, start_position) - % - % to compute a new position based on searching from a starting position. - % - % Copyright (C) 2014 Simon D. Levy - % - % This code is free software: you can redistribute it and/or modify - % it under the terms of the GNU Lesser General Public License as - % published by the Free Software Foundation, either version 3 of the - % License, or (at your option) any later version. - % - % This code is distributed in the hope that it will be useful, - % but WITHOUT ANY WARRANTY without even the implied warranty of - % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - % GNU General Public License for more details. - % - % You should have received a copy of the GNU Lesser General Public License - % along with this code. If not, see . - - properties (Access = 'private') - position - end - - methods (Abstract) - getNewPosition(slam, start_pos) - end - - methods (Access = 'private') - - function c = costheta(slam) - c = cosd(slam.position.theta_degrees); - end - - function s = sintheta(slam) - s = sind(slam.position.theta_degrees); - end - - end - - methods (Access = 'public') - - function slam = SinglePositionSLAM(laser, map_size_pixels, map_size_meters) - - slam = slam@CoreSLAM(laser, map_size_pixels, map_size_meters); - - % Initialize the position (x, y, theta) - init_coord_mm = 500 * map_size_meters; % center of map - slam.position.x_mm = init_coord_mm; - slam.position.y_mm = init_coord_mm; - slam.position.theta_degrees = 0; - - end - - function [x_mm, y_mm, theta_degrees] = getpos(slam) - % Returns the current position. - % [x_mm, y_mm, theta_degrees] = getpos(slam) - - x_mm = slam.position.x_mm; - y_mm = slam.position.y_mm; - theta_degrees = slam.position.theta_degrees; - end - - end - - methods (Access = 'protected') - - function slam = updateMapAndPointcloud(slam, velocities) - - % Start at current position - start_pos = slam.position; - - % Add effect of velocities - start_pos.x_mm = start_pos.x_mm + velocities(1) * slam.costheta(); - start_pos.y_mm = start_pos.y_mm + velocities(1) * slam.sintheta(); - start_pos.theta_degrees = start_pos.theta_degrees + velocities(2); - - % Add offset from laser - start_pos.x_mm = start_pos.x_mm + slam.laser.offset_mm * slam.costheta(); - start_pos.y_mm = start_pos.y_mm + slam.laser.offset_mm * slam.sintheta(); - - % Get new position from implementing class - new_position = slam.getNewPosition(start_pos); - - % Update the map with this new position - slam.map.update(slam.scan_for_mapbuild, new_position, slam.map_quality, slam.hole_width_mm) - - % Update the current position with this new position, adjusted by laser offset - slam.position = new_position; - slam.position.x_mm = slam.position.x_mm - slam.laser.offset_mm * slam.costheta(); - slam.position.y_mm = slam.position.y_mm - slam.laser.offset_mm * slam.sintheta(); - - end - - end - -end - diff --git a/matlab/WheeledRobot.m b/matlab/WheeledRobot.m deleted file mode 100644 index ac155c7..0000000 --- a/matlab/WheeledRobot.m +++ /dev/null @@ -1,112 +0,0 @@ -classdef WheeledRobot - %WheeledRobot An abstract class supporting ododmetry for wheeled robots. - % Your implementing class should provide the method: - % - % extractOdometry(obj, timestamp, leftWheel, rightWheel) --> - % (timestampSeconds, leftWheelDegrees, rightWheelDegrees) - % - % Copyright (C) 2014 Simon D. Levy - % - % This code is free software: you can redistribute it and/or modify - % it under the terms of the GNU Lesser General Public License as - % published by the Free Software Foundation, either version 3 of the - % License, or (at your option) any later version. - % - % This code is distributed in the hope that it will be useful, - % but WITHOUT ANY WARRANTY without even the implied warranty of - % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - % GNU General Public License for more details. - % - % You should have received a copy of the GNU Lesser General Public License - % along with this code. If not, see . - - properties (Access = 'private') - - wheelRadiusMillimeters - halfAxleLengthMillimeters - - timestampSecondsPrev - leftWheelDegreesPrev - rightWheelDegreesPrev - - end - - methods (Access = 'protected') - - function s = str(obj) - % Returns a string representation of this WheeledRobot - s = sprintf('', ... - obj.wheelRadiusMillimeters, obj.halfAxleLengthMillimeters); - end - - function robot = WheeledRobot(wheelRadiusMillimeters, halfAxleLengthMillimeters) - % Constructs a WheeledRobot object - % robot = WheeledRobot(wheelRadiusMillimeters, halfAxleLengthMillimeters) - robot.wheelRadiusMillimeters = wheelRadiusMillimeters; - robot.halfAxleLengthMillimeters = halfAxleLengthMillimeters; - - end - - function r = deg2rad(~, d) - % Converts degrees to radians - r = d * pi / 180; - end - - end - - methods (Abstract) - - extractOdometry(obj, timestamp, leftWheel, rightWheel) - - end - - methods (Access = 'public') - - function [poseChange, obj] = computePoseChange(obj, timestamp, leftWheelOdometry, rightWheelOdometry) - % Computes forward and angular poseChange based on odometry. - % - % Parameters: - - % timestamp time stamp, in whatever units your robot uses - % leftWheelOdometry odometry for left wheel, in whatever form your robot uses - % rightWheelOdometry odometry for right wheel, in whatever form your robot uses - % - % Returns a tuple (dxyMillimeters, dthetaDegrees, dtSeconds) - - % dxyMillimeters forward distance traveled, in millimeters - % dthetaDegrees change in angular position, in degrees - % dtSeconds elapsed time since previous odometry, in seconds - - dxyMillimeters = 0; - dthetaDegrees = 0; - dtSeconds = 0; - - [timestampSecondsCurr, leftWheelDegreesCurr, rightWheelDegreesCurr] = ... - obj.extractOdometry(timestamp, leftWheelOdometry, rightWheelOdometry); - - if obj.timestampSecondsPrev - - leftDiffDegrees = leftWheelDegreesCurr - obj.leftWheelDegreesPrev; - rightDiffDegrees = rightWheelDegreesCurr - obj.rightWheelDegreesPrev; - - dxyMillimeters = obj.wheelRadiusMillimeters * ... - (obj.deg2rad(leftDiffDegrees) + obj.deg2rad(rightDiffDegrees)); - - dthetaDegrees = (obj.wheelRadiusMillimeters / obj.halfAxleLengthMillimeters) * ... - (rightDiffDegrees - leftDiffDegrees); - - dtSeconds = timestampSecondsCurr - obj.timestampSecondsPrev; - end - - % Store current odometry for next time - obj.timestampSecondsPrev = timestampSecondsCurr; - obj.leftWheelDegreesPrev = leftWheelDegreesCurr; - obj.rightWheelDegreesPrev = rightWheelDegreesCurr; - - % Return linear velocity, angular velocity, time difference - poseChange = [dxyMillimeters, dthetaDegrees, dtSeconds]; - - end - end -end - diff --git a/matlab/make.m b/matlab/make.m deleted file mode 100644 index b00df47..0000000 --- a/matlab/make.m +++ /dev/null @@ -1,19 +0,0 @@ -% Script for building BreezySLAM C extensions - -% Copyright (C) 2014 Simon D. Levy -% -% This code is free software: you can redistribute it and/or modify -% it under the terms of the GNU Lesser General Public License as -% published by the Free Software Foundation, either version 3 of the -% License, or (at your option) any later version. -% -% This code is distributed in the hope that it will be useful, -% but WITHOUT ANY WARRANTY without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -% GNU General Public License for more details. -% -% You should have received a copy of the GNU Lesser General Public License -% along with this code. If not, see . - - -mex mex_breezyslam.c ../c/coreslam.c ../c/coreslam_sisd.c ../c/random.c ../c/ziggurat.c diff --git a/matlab/mex_breezyslam.c b/matlab/mex_breezyslam.c deleted file mode 100644 index b74cbf3..0000000 --- a/matlab/mex_breezyslam.c +++ /dev/null @@ -1,294 +0,0 @@ -/* - * mex_breezyslam.c : C extensions for BreezySLAM in Matlab - * - * Copyright (C) 2014 Simon D. Levy - * - * This code is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as - * published by the Free Software Foundation, either version 3 of the - * License, or (at your option) any later version. - * - * This code is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public License - * along with this code. If not, see . - */ - -#include "mex.h" - -#include "../c/coreslam.h" -#include "../c/random.h" - -#define MAXSTR 100 - -/* Helpers ------------------------------------------------------------- */ - -static int _streq(char * s, const char * t) -{ - return !strcmp(s, t); -} - -static void _insert_obj_lhs(mxArray *plhs[], void * obj, int pos) -{ - long * outptr = NULL; - - plhs[pos] = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); - - outptr = (long *) mxGetPr(plhs[pos]); - - mexMakeMemoryPersistent(obj); - - *outptr = (long)obj; -} - -static double _get_field(const mxArray * pm, const char * fieldname) -{ - mxArray * field_array_ptr = mxGetField(pm, 0, fieldname); - - return mxGetScalar(field_array_ptr); -} - -static long _rhs2ptr(const mxArray * prhs[], int index) -{ - long * inptr = (long *) mxGetPr(prhs[index]); - - return *inptr; -} - -static scan_t * _rhs2scan(const mxArray * prhs[], int index) -{ - long inptr = _rhs2ptr(prhs, index); - - return (scan_t *)inptr; -} - -static map_t * _rhs2map(const mxArray * prhs[], int index) -{ - long inptr = _rhs2ptr(prhs, index); - - return (map_t *)inptr; -} - -static position_t _rhs2pos(const mxArray * prhs[], int index) -{ - position_t position; - - position.x_mm = _get_field(prhs[index], "x_mm"); - position.y_mm = _get_field(prhs[index], "y_mm"); - position.theta_degrees = _get_field(prhs[index], "theta_degrees"); - - return position; -} - -/* Class methods ------------------------------------------------------- */ - -static void _map_init(mxArray *plhs[], const mxArray * prhs[]) -{ - - int size_pixels = (int)mxGetScalar(prhs[1]); - - double size_meters = mxGetScalar(prhs[2]); - - map_t * map = (map_t *)mxMalloc(sizeof(map_t)); - - map_init(map, size_pixels, size_meters); - - _insert_obj_lhs(plhs, map, 0); -} - -static void _map_disp(const mxArray * prhs[]) -{ - char str[MAXSTR]; - - map_t * map = _rhs2map(prhs, 1); - - map_string(*map, str); - - printf("%s\n", str); -} - -static void _map_update(const mxArray * prhs[]) -{ - map_t * map = _rhs2map(prhs, 1); - - scan_t * scan = _rhs2scan(prhs, 2); - - position_t position = _rhs2pos(prhs, 3); - - int map_quality = (int)mxGetScalar(prhs[4]); - - double hole_width_mm = mxGetScalar(prhs[5]); - - map_update(map, scan, position, map_quality, hole_width_mm); -} - -static void _map_get(mxArray *plhs[], const mxArray * prhs[]) -{ - map_t * map = _rhs2map(prhs, 1); - - unsigned char * pointer = NULL; - - plhs[0] = mxCreateNumericMatrix(map->size_pixels, map->size_pixels, - mxUINT8_CLASS, mxREAL); - - pointer = (unsigned char *)mxGetPr(plhs[0]); - - map_get(map, pointer); -} - - -static void _scan_init(mxArray *plhs[], const mxArray * prhs[]) -{ - - scan_t * scan = (scan_t *)mxMalloc(sizeof(scan_t)); - - int span = (int)mxGetScalar(prhs[2]); - - const mxArray * laser = prhs[1]; - int scan_size = (int)_get_field(laser, "scan_size"); - double scan_rate_hz = _get_field(laser, "scan_rate_hz"); - double detection_angle_degrees = _get_field(laser, "detection_angle_degrees"); - double distance_no_detection_mm = _get_field(laser, "distance_no_detection_mm"); - int detection_margin = (int)_get_field(laser, "detection_margin"); - double offset_mm = _get_field(laser, "offset_mm"); - - scan_init( - scan, - span, - scan_size, - scan_rate_hz, - detection_angle_degrees, - distance_no_detection_mm, - detection_margin, - offset_mm); - - _insert_obj_lhs(plhs, scan, 0); -} - -static void _scan_disp(const mxArray * prhs[]) -{ - char str[MAXSTR]; - - scan_t * scan = _rhs2scan(prhs, 1); - - scan_string(*scan, str); - - printf("%s\n", str); -} - -static void _scan_update(const mxArray * prhs[]) -{ - scan_t * scan = _rhs2scan(prhs, 1); - - int scansize = (int)mxGetNumberOfElements(prhs[2]); - - int * lidar_mm = (int *)mxGetPr(prhs[2]); - - double hole_width_mm = mxGetScalar(prhs[3]); - - double * velocities = mxGetPr(prhs[4]); - - scan_update(scan, lidar_mm, hole_width_mm, velocities[0], velocities[1]); -} - -static void _randomizer_init(mxArray *plhs[], const mxArray * prhs[]) -{ - int seed = (int)mxGetScalar(prhs[1]); - - void * r = mxMalloc(random_size()); - - random_init(r, seed); - - _insert_obj_lhs(plhs, r, 0); -} - -static void _rmhcPositionSearch(mxArray *plhs[], const mxArray * prhs[]) -{ - position_t start_pos = _rhs2pos(prhs, 1); - - map_t * map = _rhs2map(prhs, 2); - - scan_t * scan = _rhs2scan(prhs, 3); - - position_t new_pos; - - double sigma_xy_mm = mxGetScalar(prhs[5]); - - double sigma_theta_degrees = mxGetScalar(prhs[6]); - - int max_search_iter = (int)mxGetScalar(prhs[7]); - - void * randomizer = (void *)(long)mxGetScalar(prhs[8]); - - new_pos = rmhc_position_search( - start_pos, - map, - scan, - sigma_xy_mm, - sigma_theta_degrees, - max_search_iter, - randomizer); - - plhs[0] = mxCreateDoubleScalar(new_pos.x_mm); - plhs[1] = mxCreateDoubleScalar(new_pos.y_mm); - plhs[2] = mxCreateDoubleScalar(new_pos.theta_degrees); -} - -/* The gateway function ------------------------------------------------ */ -void mexFunction( int nlhs, mxArray *plhs[], - int nrhs, const mxArray * prhs[]) -{ - - char methodname[MAXSTR]; - - mxGetString(prhs[0], methodname, 100); - - if (_streq(methodname, "Map_init")) - { - _map_init(plhs, prhs); - } - - else if (_streq(methodname, "Map_disp")) - { - _map_disp(prhs); - } - - else if (_streq(methodname, "Map_update")) - { - _map_update(prhs); - } - - else if (_streq(methodname, "Map_get")) - { - _map_get(plhs, prhs); - } - - else if (_streq(methodname, "Scan_init")) - { - _scan_init(plhs, prhs); - } - - else if (_streq(methodname, "Scan_disp")) - { - _scan_disp(prhs); - } - - else if (_streq(methodname, "Scan_update")) - { - _scan_update(prhs); - } - - else if (_streq(methodname, "Randomizer_init")) - { - _randomizer_init(plhs, prhs); - } - - else if (_streq(methodname, "rmhcPositionSearch")) - { - _rmhcPositionSearch(plhs, prhs); - } -} - diff --git a/matlab/mex_breezyslam.mexa64 b/matlab/mex_breezyslam.mexa64 deleted file mode 100755 index 0e94256..0000000 Binary files a/matlab/mex_breezyslam.mexa64 and /dev/null differ diff --git a/matlab/mex_breezyslam.mexmaci64 b/matlab/mex_breezyslam.mexmaci64 deleted file mode 100644 index 97fc14b..0000000 Binary files a/matlab/mex_breezyslam.mexmaci64 and /dev/null differ diff --git a/matlab/mex_breezyslam.mexw64 b/matlab/mex_breezyslam.mexw64 deleted file mode 100644 index 28bdc0a..0000000 Binary files a/matlab/mex_breezyslam.mexw64 and /dev/null differ diff --git a/python/breezyslam/algorithms.py b/python/breezyslam/algorithms.py index f9a7b7d..5a823e1 100644 --- a/python/breezyslam/algorithms.py +++ b/python/breezyslam/algorithms.py @@ -308,5 +308,3 @@ class Deterministic_SLAM(SinglePositionSLAM): ''' return start_position.copy() - -