From 6557a0af4810eab1084a3796d517ad4d73811953 Mon Sep 17 00:00:00 2001 From: simondlevy Date: Sat, 30 Jun 2018 15:01:47 -0400 Subject: [PATCH] Removed support for Matlab, Java. --- README.md | 51 +- examples/MinesRover.m | 69 --- examples/log2pgm.cpp | 439 ------------------ examples/logdemo.m | 175 ------- .../levy/breezyslam/algorithms/CoreSLAM.java | 133 ------ .../algorithms/DeterministicSLAM.java | 52 --- .../cs/levy/breezyslam/algorithms/Makefile | 90 ---- .../levy/breezyslam/algorithms/RMHCSLAM.java | 103 ---- .../algorithms/SinglePositionSLAM.java | 118 ----- .../algorithms/jnibreezyslam_algorithms.c | 67 --- .../cs/levy/breezyslam/components/Laser.java | 108 ----- .../cs/levy/breezyslam/components/Makefile | 89 ---- .../cs/levy/breezyslam/components/Map.java | 96 ---- .../breezyslam/components/PoseChange.java | 115 ----- .../levy/breezyslam/components/Position.java | 81 ---- .../cs/levy/breezyslam/components/Scan.java | 97 ---- .../levy/breezyslam/components/URG04LX.java | 29 -- .../components/jnibreezyslam_components.c | 132 ------ java/edu/wlu/cs/levy/breezyslam/jni_utils.h | 64 --- .../levy/breezyslam/robots/WheeledRobot.java | 130 ------ matlab/CoreSLAM.m | 136 ------ matlab/Deterministic_SLAM.m | 44 -- matlab/Map.m | 59 --- matlab/RMHC_SLAM.m | 72 --- matlab/Scan.m | 66 --- matlab/SinglePositionSLAM.m | 101 ---- matlab/WheeledRobot.m | 112 ----- matlab/make.m | 19 - matlab/mex_breezyslam.c | 294 ------------ matlab/mex_breezyslam.mexa64 | Bin 23466 -> 0 bytes matlab/mex_breezyslam.mexmaci64 | Bin 23440 -> 0 bytes matlab/mex_breezyslam.mexw64 | Bin 18432 -> 0 bytes python/breezyslam/algorithms.py | 2 - 33 files changed, 8 insertions(+), 3135 deletions(-) delete mode 100755 examples/MinesRover.m delete mode 100644 examples/log2pgm.cpp delete mode 100644 examples/logdemo.m delete mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java delete mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java delete mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile delete mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java delete mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java delete mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c delete mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Laser.java delete mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Makefile delete mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Map.java delete mode 100644 java/edu/wlu/cs/levy/breezyslam/components/PoseChange.java delete mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Position.java delete mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Scan.java delete mode 100644 java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java delete mode 100644 java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c delete mode 100644 java/edu/wlu/cs/levy/breezyslam/jni_utils.h delete mode 100644 java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java delete mode 100644 matlab/CoreSLAM.m delete mode 100644 matlab/Deterministic_SLAM.m delete mode 100644 matlab/Map.m delete mode 100644 matlab/RMHC_SLAM.m delete mode 100644 matlab/Scan.m delete mode 100644 matlab/SinglePositionSLAM.m delete mode 100644 matlab/WheeledRobot.m delete mode 100644 matlab/make.m delete mode 100644 matlab/mex_breezyslam.c delete mode 100755 matlab/mex_breezyslam.mexa64 delete mode 100644 matlab/mex_breezyslam.mexmaci64 delete mode 100644 matlab/mex_breezyslam.mexw64 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 0e9425666089018199330fdca8ff0426ea80a9e7..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 23466 zcmeHvdw5jU)%Qsz38{)RLDYySqYW)VNQ1^oL`#Ouz=%({P6{|A z&E~9<0ZRYR>Pv}WXx^z^&AFQYJ>_Wz#Mi&cvlFEEHAs_kqKRs+haiJ(LRa_cH z{UjB|v${=$4GPzS8>*CR)=odP>)lz8H!KQ1wcuFatuHqm+C@cB_Nn-n@wb(L>9pg- z@uw_n&b;!zN1tIpijze-Ls@fHqVRSYoRA1H?nfzbs;_+v{d-dA>(Cv`{_+(1Q&Qmj zQ`k8ph5oxK^naZKzbS>C&J_5cQrKCYLjR)_`Y)xxVMejzKlykfh5f=5_?<8?R{XUT z`oBzpuSQMU#ov*rPm)I4_bme=lmvbb3IohLyJ6h z&2@ey5cGR|ZIp9k>YADrA2${VB1V42?QUpeHc3J8wl@WpKvRpSHJ~&! zx3>C~2EP~4a0g6Efjj*{#RG>(zfEawU8XenTI(s%fD`H!Z1Q=jW(_RicAGLcH!bzG z7X(@rN_b-_AO+tgtS#7x;MNmgNW6zjpz2-jhn`il~NwO@=9qCG%_zTYQ zWzu{z8UOF2&(cAJG&bC%C?unU`ZFDruNP@K?uRBdeS>U|#^e^|ppcV(@B)%&DQ9BB zqC69un#L>e{d~dm{FYdLxo(j2{Sqr#QIz#!eIRkWz-GG0uIq40W z@N)6vugy%sf54G!PwyI~kuCOa=cO;l;d@~K5t!roY4ynQE=#>khGCFlJ&eka#bnPnS+~E#ogGo-Uba zC*vm(Pt}UHF@6H^bcsZlF#ZhU>C%W+GCrMnx+J0{jQ{uL;OSC`7BYUAc)A3lcE*21 zJazr3jq!(xr!F5=82=XW)YYRyUjmqVfOzWS(L;>iOFVV$=m6ul6F-6Y2;-k8{v6^r zGyYG+Qx}e|XZ#+}a%lKarPhB?J$@s^Jr>+`pWBgBur!E>@!uSV?r>+^TWc&|_ zr=c-g!uY$0r!E>TWW1kv>Y7nI<9|Rrb;+oW@k@!PAvCIhSHqc&0Mzicw}Mou`6H_K zzS=c%_KNh{9q2%x7xE4?TswM*`Umu9asBBg{oWCas%=+$Kboti4XFD^LM-#SZ?Mb< zg2T0{wtMt2jsxQ$y4HO+9Hltl2>w%Q(8;NYQQ*5R{P^Rr1w$9i> z+U43tBEwc2%S=)G6$XOmp^9=opg7+GdDMf^Lk({wui(Yq@WQx|yKoC@sdDW@;{qmp zl7(HNAU~C7tAa_aTwCQER~S1@Jt2V}h5faQz$OpoeCe8}pEN z(w%yHHt*Urh-zONWhhnEaKZK&PqE}~a{oLGnbA}5I$rl#Wc9yzFIs&L`pY(&7mS*J zvx@Szeh7@LRE1s-hr?G|kdI8hXX*!lPhij@_!R-Kqk4s}JSyO41iTh-xxR%6q#p?B zx&-M~A$?a!?=ne|>7al;fE%pS2GQZdlmqk;>W%RZxK(sO)&%7m>z3$`Vc9RqN9yt( zOfb)*@~8l7gKY?0^kp=CJX&oi)aXKq)KfMz z64Q__ z$WExjvk)2)c#Cf2kGcO6npo9#@b-S<6z|S9!%rz~7s0tkH;l<{fX1Wn@lIb3Ed;a@ zr%jDBJj^sRUyK=4a}^ubw0yVgm|6>mU*> z#YT{G;eGUy1nWmJyBco-r9U(JQ1Tn4@>ekXUH$dy6?`thiqS#% zeLHEQ5EF~TUArSb6{zn}r zPdU>DwX8l`RE&VqSWTpu35{|XxEwnDAe{KQ44o(dkv;14hT)O zat$o!WDInb=B7EicBbj2lbywftPjtlnX+r=Y*W+HifGtcow*jPQ^UhbpCMKbs%KQ~ z{V?5l5IsmlD`8dR(2k=NTEX}R9oh+}b<>G5&zEZ2s8Nj^=zgy56ZhI3;rZzB&-N@?zCOC438 zYm^?7o;h__E;738F0EG$kG~viZ?$hbsyoX11M54+@#9k6VOkW?uwABMNI9;(rae-P zEGV#>)}t_3Wdl>S7O4>I=Y-avU3<{32)Zq^9;LuhmmOYay1^I=;SeMp5a(=d=O#LO zh@tx&RMB<|nW@?!d%;@cZp2D+twt{D5;d+hMiU|3w5oA*VVqU0t8c@^uMKuZEZqmK zWjlID(zWizBxb8mHSF4_YTYUTB>RypL9)#A_cA>r4`$CAEYmTZAeAwQ(-v|I#s3|J zy$POq2L2831L#U`!M|Ga4q)DOP}pg_fVG0wT@Rxq zTo0jZ4Fs-r#xI%VHZG~9D!&W?{%zj%U@(H$;J+H}dsC#rS`fMW0*S^|t9y;JDW)jB zl}aCPoWo)s=IwJkoU0yxoMW#*9)bfkpTN5iyt8${*Bl=~U%+G^Xa#Bpn^|zpq3wbS zd5oZw+v|*fVJ6pH8z^43`>t8{Hl82{C+OYOCp1@QpKBdm{zq&&i#|t4$DzBk%k*(r zO0RcnzJA@cL38cXU8{SIDXSya?uRko7T2z}hVMs0XALVPA(F5)f;HTPv_l)z!_@G4 z)oNAW!kSdyO3i7k;Zs-lJfT*xT7QHzp+<65-%8p>olv_FYA(&tJ61dLM=-wjj!bdP zia7EkPQ7A-Q`@U=q0ITi+5jZ8w&!oxEBfD4NYR*S?GtDIerNtbS_Dn!(xQ&u=oD94)Up4R9$I(b$J9XD5ay#VG}k(e z*}L+0Yg@=w>n6TB^Bhz?uGMPVpsI(~tKp3KuI>ZYHE*G%o!Uny-*SYDDjnKTnddb% zTr@$|w#-Ie)$@*;cJL*#&DI@Gt#3hi@-(MbyWa5vJ;TL#)alCK;nYH%4(&_K+=a+h zu5YEx<=StFxU`+l;^BJ>=2Wd;b-pFE&S_y0bQQnSF{$h0G*^D~$b`!PU&i2-SzRBG zqFPStvx>{Q36?*!Zu-)h*4I0l-Js3x-g9qudD@7jV#ARvOPC_wwLJ}Wauy%B_fe-F z>O|Sx1y;7BYj>KfIGC4f?S73Gs`u@rQ7~y*&=+J|H$}Q5_uBc;?HN#YtM!@ko6^>t zbzWB_O}_>51I61{z0YTdnEqufCaPyU^c$Skmol;%oLYv1xZXTRHaz31#RIF}8haXc zXeTk_;^KAT0?(<4;8fM4RXhz^yTPIT1#`^@WuBeo;TgN=lBCu8d$h39ER=kcB}=mr zBjXCJ+*FNb&$}l(wSXO$yq9cxrXl4c!*>)S73x7MFk5#$t(BI5@7MtTZndv8m+beI zE+K-cG^4`vURm#Z>2y!y=fzY~x`C&w$CzE^VudEGpB#lZ`rX-v#uGN9lP4;T6+BUG zv``|xLP1Yz!$mvIl}{5}FCJASOl;(rapA|OPi$RBaVPOvnRWsdp(xri|BAdA>=vNr zGf?sA_^0uch4*$0{nh9GC(}Tv%k_?)z6HGQ3zkqB`hpT?xG_y%WzX+(6n|k2zXq_( zv&*4RyubTEh-@yJU9Qn&XRDx-EAkv&9!`GmM9Z#d7+Fy6UXr5Vn#>DP9!CED<;$JA zCpRfKl`3*CL~iFCPj1HzXmnLyWb3--hf3%w)pPw4aJc`u0wyQ#Zm`~Z@R&7c@`FNv z`=LNKBzYux50bnGyGoa^6_K;ple_9qj;>u;8`&=_EpZkb)`w@}vO(mhC0z&R#R6Rk zs>Uv3RXd5%ppEiqkMQXzho0dbwOVQEAq|!R>0Lm_i}g|;EGTJ&of>(=-v&>D(jDahCv21*z-46RaH4PHkv!N zp_3osgfTpSyY`5flBwJrHaSrFC_3l2m8?{oEPZ1&c&E0}2@+f3(nM)CbU}8d}&>H7c zJ&SkUo6#LvnMG^aBNt;dC694qDJoJ+wx6W=4-s+B`7PD_-X`Z9R-=w3DBX+N)J**& zyG6<&#^!^!H8UUM2r)J`-NH}^0kdu8rgJ(vNkYXz!j?`6fr=i$S`8)AWNLkA7nmc{ zk)akQxI@IiM9!1K)h8)F#FI(f_|0Z)tn5PQ3)P(JY*W!~Ho7ZA=S$f)yN))}lSZv$ zO_}zo>Fy)*B|JArUHK~YIWWc_4f4TOnu|rzA&9&Yc;ux!69b&DADnM9xFK*M8#rp; z8d_lIxYoM#P;L*r9%bDy1I<&Rci3K_tF^ED9^jNx`&^ww9bMP_FU8uu8Obu;1vhLH zu^M$z%_a}RTznzi!&YE`YH4jq3&o1f{0^x)^Ee)28GYom(>enVi=QoUk7zp_j~QLarvy#OR^osJ3D4zkQ(Sd(4JSky~B>%O-I^J4CkbPhr{!% z&t@!9(_EchtQKi%*N!wt@!zcvQxoZ7s#*7*-~?!Qs=bHKP(6FpN7a+3a9;`u4I7O~ zP1BQs7an|iQ?(K8S$Y+$hJQ|PMAh&%dJ%~S|(bIXm%kC!pNtB4{lXi)Tm2)ZgmoY3GaCs6jrDr+dbxaj%R|Q!*cTeS)Hm<8B$p z-$xUvd}7IbAX`>U&$2!{&~%__t)lcAwzL5)OCQIO%J~u*r=Qn*7$Omg_-$sH_9br2 z%Cx`sjeDX}y|Zrt>V%G#?)|nttM7-OW02W%!}3{usBY)nzWa$kocnPx>y79bGtYn% zh!DG}bPYCGZ^^6#Vq8X!;%4F)9!_8qq}xs06#Nz_`rZK4&drA!(wM4pDh7l~&G?r| zU$xG_3VHR#RG9JZdz5D)?q6;lJm#?GJTZBM4ScrD-!t1b%3aedR1T5KZ!RJgwd+WR zdf#W+pO20P&!AEUqt(10B|g!Y5AV_BrdUk4bmQ_^EQ)^p&qBlkSK{GCZx_a50e<*+ z!5kTjYcFOF<#eF3w#*B-cU~}b6NdY^^HCFu$v7sFG5N7TPa(ok!_aUOZW?oH5u}2H zcb|O*5AJjV?Pt@`M7j<^hB4=J#gVuvkt!S-W# zY{25mI!ojjhU|T~`2G)*s0;s*oB!>k+!dsbbcyQqYK&xloKl+o|p7`o}75kUesu2L3+$YOpTub?A$`slQI& z%T}eisMK_P5Ya086ANdYlCR4lg20LYpk(0$;Nu%D;tX@4cqu(%uf8Wujw^DeMdGK zZh1}`r(iK2OEo;P8y+Yurif+cIC!S&6`S>p`zWI5U;(@%tJ*m92p$`W|qR zx6bcrRH|C&3r2ge)y}@(gKPGw%^~~Jl|e76g@gRIUu&OQZ*QklTT{EYIbgqspa#1S z?)d|xhVTE{+`QPU?KiaA@eyFF-@eRWSKow>2(PvW{VVPG01zMH;XA-(_QqBex}vE* z$Ufkp{IK1+l)fp1psf|3BT^v%=}SQYkeWCS&CQ+ZT>RN3-zZ@~X9K4t4|A#v&E@#= z5}24e&px%rKDF%XsTEgGU1Xoy#$-5kt}bx2ZoN1E%{>>F{4D+Y7gx`0ydK_Tnpa&|VS z6%1l@tdjD7EV?a%|B|!|EpzCb_k0N2-yR)pL%Q~z(b09Feo%U9{UYcg&^JJ7ad!i{ z@HEgm>gS+6Sjn}6UWt|4I?z?15zy6G{|kLg=sM8H z@d%+G^kq;3H0xiZquCf14};DIy<`aaKyL(X2mSRYqoW%@=i-_AAn03ob&!p@|1 z7yo^96py;&c5dKyW&k>l{~XBKH_b7BAGZ7`Bb@nAR@XSo6F*6F4+YG&Dp_d67i$g`c-1K8{eX0k2Gyf14jCko4*I&qW zNd6+^U64;O%PY8?{CXSm5ablsSb6by-9wsWCj))|P3YkXQ(UhsUQQn5eUN|0EdK$Q zQ@xy!zYh6S^vzf~mGOF6Y?)jcVk$2P{g*#N-$gp6e@DE$pF(~R@;PSt@_2bqLw*AC zx6N{M9QVeGW97XE{a+m!9c6=)Y;Uwr2x_eUxQ2TnFEHC{ikFuM`JW+Yzqyh5P32kU zrNs)Qc+m5lV|X(81j>!o&m4~j(i8MoL*I?D+}+gJEOuj2Y4 zPv|#@yBDR9{d*uk1o>FkDaoIJ{58mP&E+qO*Yicl>Gj4ev%EQ8&$l6e8}hTvagF!? z&q*^6c{9*&BN(F{W9nIEQS`~58PI!kcy#nub9w1itU!{NLw*k4z+GaN-xl|$8S;sc zC-lRzu;rnQuFN~R8XG6*9_lja6`B1p_qR*q{ek-1PUwFJbIYGo>2Hnelb^?-|0?F5 z8nd72i-iXSetmo$fv+R*bp*bS!2d51SYqWhwF@fiO;ZXTPl<%&pH1TFw^npW`O8T> z{nm<(vp9+Uo|k@jK*#2poZ`bqcF5mXv3mg^bT2{2Bu-+#v!eNo4k`ci%h6WiheUap z>evwx3ET#;gWe_3!EWY3FA+RjQ-Nn|4p1zW_)%hKRH!M2NXYvqaXY23wK_D{HwTle1rdApdBR9LpUhKP(EyEg<7#^Zp zV8~unFuR~==H(&A&ANA1Q9)r*K~X-(6{R4s(if~-3L5lt+9*>ktwC?WvX)T6(oj=# z{miC%g>j8_fkvgEeq{@CavJn=$#VMLEPe*d)CA)exg-l!zqh%L@{3ekb5JQ@kNXOO z-gcz%Q{E=XTJfl_PATv5ziuVZA=5wbKn>RGd`%vB(h7q( z6|hpc#ifCOQs8Oz(NAxayUza=PvZ(*BJ#RQwLgP2j$4U@qW~Np!?eo$a=ejrn$VWl zoxJX)k=aPlSR?bx@kr7QA|sVcIx@c;mr8(;msT|)aKebbG{s(|jOyztc_ot9l&Qnlf2bE<$eh9{#f3q0pB#kw! z$Ye{R;pF^cJdyjTjM6XIOoz-bq5nW4q5PR#BIjW_FG>Am|LKhv<(KUj5&0!;mjYoR zx%^)yzfxl7>Lr}mNN74_K1u%xnK{3l2W<|X zN$N=x$^0=02$S`G$ec#@??aHjH;>nZbJmZVR*bV!O1Z)0UMfjE_l z^3$ZEz>`>JC+QMlnrq5WaMX z5^;GK7e}f8XDFxdGeg}xO*!3vvXDMT{At}q_#rXhfn|pSXTp>3UvB}PX7W>BhYbur zJ)ShLk{x+J43-^yzB|2LdLSLEUVDI(okwVrz`@>)fgZp=;eQY~duIgfu)vQCT;BK6 zI7#|eF>teYU%*ZX{C#R^#Z?0=uhR@lv@P8G0BbxI6c*Abl52oI~s-lYob3- z5E8$@%SCqU! z|1n`-G>;MyxJC50Il_Lwz+})3Vb95J`M}}vHZU<1wJDMPT#AHW#_sS_~H~e z?PxProL8jK*HYk50?!#^o$$*P`Y$m2^nL33Qs}>#0zZ}l{|464W7X>-;A6$DFa+Y`@kRTcXMMvapUVm6oqXAg${P*!)|_l@15|{?`1pTd9V{-Q=1Z~Yw)_|&VKY2 zBmctB-M&)sw}x8kLv67v_`oqxkC^${H-|{v)wFC`2;Vv)N&EG=mm;QKe?igZS6zj= z%wOoJaJgMKIB9D|7G|F2-%7Gw3fU)x^d+U+%btxg5qlQvFT%HxemA~-gaa&UtO$HP zNP8aQLqxW{VC*|emND3hinJ+rGz9|4=&kqGD=xdP;VdNikvXXe6`6r(7?;u4e#HW`mEZi|PZoPsp9Nm1&MDB*HjT=vZ z`KFWPUkFc|g>}>zEfM3qD-^f0<_1SaS*a3e^j{&pWey@}BA2JN4Q<8Zg*ao^N*VJ7 zdtjWbc2w7n>WGf{zGc8M76OeweH9CM%0*lwS+ zYi43z^M09e3GHbK_t=)2ag=I1)?TG-^Ug#y+S@Y`N!a@{E~1iHH;`jy!WOD>7gX8> gG@gh2V3N}|C3PokteTjE??IY~CG0gC7m-u{4U9jcf&c&j diff --git a/matlab/mex_breezyslam.mexmaci64 b/matlab/mex_breezyslam.mexmaci64 deleted file mode 100644 index 97fc14b98362a43e56b32a23d6f4d933e4b04536..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 23440 zcmeHve|%KcweOiQfuMvl;i`=mWk6CBz?f-K37}@k44$bo5)F}{gdrpolH2?unP^bN zD2%du9GlyBy~SRAO>f)xp}ktt+l%H}#Th^n5HV;k2JvD*Yfr=<;YT5&GWWao-e)oy zpuO*}_j#XBj-IpE+H38#*IIk+z1P`i$XjPWJ2y^IEMpW!NyD9iTN|q=A(V7oiMusL zQT+bJ?$X7)YUHC}B&tXpGL=-^RMO@52Lr7^0SkPjyf&Mly>zW83AYZj6VjH5`u)wp zmX*!%s$~7<%oe1VB?sY_I5%vS@?a{zggk$Bptdp4gqmb|r=+}vQV8L2%kXuceihBZ zrkeUyk|SB(+?xbpniOKP7J}1-E(m z{i_;=N8x<^Yz2bTlt*~7ys4%zlzxA0`Crq|j`nm7i9#VfQ6AZQt)x-m_pfXoo)zco zXP3UrPH1ELimKr95f_bvhTHG=dhWc#?eZ*^$xXB+%O$=HsVjagx@nvoa%5*z@3NN)RpS!Drjmf4_4n$ zTeGsRHMepNN>Tqm;147=_#g0>2NOCV{yDgZ^&xDWazjgV6KO+tbFi{v=FHri5}I*4 zMzkCAYqYbBXjiV`FVO+a;ks>82R3*$66iL_sk ziW5kOBx$^rw<=5^MI`B6LCQ-Y{YsKf3DT4V(hoTa+W#(BQPd4L;SCbnj7#9CmW^8l zX$X3oiuxq!a=bsG)n^H$?+Q}rAX<6!N7A(LxQQj^&c0ahPM5;P|fOm@he6hdEvq95%^u73Y{0 zVeU4m>I;J53%gMDOO!C&y;0L~GuQM20Br=!(ew}rCO8i;sX=fyU@}Y7!=$^$ywmMe zpNV>MH^DZ*G$i1mkszKljajE7ej>bjD-E2NeS-LN%pwhJ-1R8KzCq0iml#Ex z@daTrEMG%o#TY=1#&%%Ry4c5YIX_49*DRoKqsPk=@o?-mp5fBR(&%?^a2L(>7dUv$ zz}_*k%MBzFVW?}Fw?0dZ0Z; z8mjTunfgO%FOTO=XXlJ*sKcm}c{xO8A82fsu?_TrQnY)G-uaEGoayz~=mp=nj3Ya_ zyS~1f?B&YQI+btd{`w51;@Whj#r4oiP@6|aU@})p#NCXpEc<= zNgpfdO_ILWq-RQc3`2wwyIazi5nY`{8qk;5H_py_a2kKKpLYz7XTlZM!=7Mh&^`~OwQm*F&PAnhBmKNyRsQyJlfyd&~grr`k0fm{L@jj7$p2AmM( z*+vGhu+c(LggI6Sz4#(l2)$gSS0Sb%U%m=475UmVM$_+r<#N#y;TWI)$KhVS+s||N zwoK9<-ATE?ea>ahGUsxqzifxG2ZwbuTE=q#Xnh$n!uVc6p8N@F%oXLF(T&=H-+dK} zwYO7#QCy@|e4s@?Ore$IP2vnig5$LHJJaqf+o9>Hw(H2p6a?R*Ac$z}EsY(}FyfDa znGd0{0+2eTU^rh{T&%7C)CPyR`<(2whkfm0C)F*7yy5xZ_Oh5!CA%s2V08{13SJw& zBewo4ix&2dPk9*=NYT_q`?14MMvr8`0`R+8a5{O@*GI9BwF||JT_@C~=`Uh#oluuY zsh&K#ey5Eb!`%V{7U48b7;q05O#{5_tP#D+G~j1|)G1tlO~0*d;Duq_rMolHts9$Z zdOG5E8`M*@?(JA|Vx1N@i%D0!0aia>nOCd1bbV{MB#*aLY{VxZOUgj$-q4O$upaW+ljdZ|6J zxZ8FHeBtUDi9MI1k zV$V%RS*1?FcwnU`l}{{CZPp<;qGY3*{Rx^HAdmq>m`;SUHZ}VV5cZx>mkq#AbrB#t za36(3`%w`fKguB}eH{eysV%*jt56y>{ytN4C^HOp*XDUslb;9sShED1&qGpqD zbYd&%RkQa1-i?4isT$| z>02p^q)eB(Xb=YJym?NqvkRmcU->}kX5D8#P`yK}2L^kiIq$Id&b+63-*M%9%ub#; zq++D7mKDG;rdk39tzng78HW}5vPFK!;wvVchvu-ClNTd^P(y1;I zI<0Q_4lK~!5pt%x%N^RSZajfu3r`-;%AR-S?6&OIn6FcJcWLan#=?2f4zAB}N7$a| zCsQnY_Mc^kT{-*0*6~g@Z0vl_0G4v zY`eDp=wPt?&hTXu9WihI{s+c?KWz1A`Gd_q2dqud=cKyMdTW)sB2uNU?oR{ey)_N! zZlFg1tRAGtaS$G{uEE2*#-^?BwrTliTVB`LyPo`^pnZfq^+oHg(f+h5W?iGfmH8j6 z{j6+9LVqL98;YFlKiNqSJBI=MFK@+HcZR2b13M_)Gk*1VuoFINMHWo>(-N=7t4^9H z`sWnd<=bTCYI^^1v--)-?u8f;ghls4%8-@yJFHs8Av5>5OR}#3yPcAe0$H&L71=x? z8BTXF&JSP<75nK^9vtb)a-F!K17_)lSCD<}%Y!^FDhOYwv?Jk$Mae77x?PyEUBBQJm z?4C@ZSwT))`XOzMn}q<%KzOpj^`j1M76OF66+LdpM`(I|2CuckYvqCGT&_D)zwDN^Z9l-RtNBW=uZm}{f zfb^NL9oR+y*6ptF3X6+XWIAKJV7D`Wm-^%kxMSc~a0HJR;abf8T<%5naou#0n|-V? zWGor(9`&hn6hFGJP1ElpkHJ+LT7h(y|i7VYdHch{GH&0WY%F6gdJPWBgL zwJO|2zVX4s3GM=Bb8j+!!8y38wi{<*2BZ4=$)b$=>T^_@zS;%N+qzZ@Y8-{EbS> zjTAxpLv~2CB73?wqE!^C64%U_sVxsV)pgKUq&@{1}}ZalL$Xg#XdJqN@|YZQ&LjmMZyssxXly& z+>WjN*dj>iL=Pmf13k9EO7p!X{W@4m)5BDmXu^cy*B3ps40^}sH`WZsYvg|G&i`5s zzY8Zg+3V=4T^iR$#z7pEW*?yBA#bQy?9i8>z?5OiUnrfWsY7jGF{h9!#vw`R{VT4} zU*#^=x01t+HP9v$Y0a9)^?dkhxD670Ja)LOXKs|8QX~vbumMI%7MoD!nSZ0 z;DIQP6VbR6Ya$;zh9f$?1Y_jetT7D}*@kR~0w5v6lJ|#F5Bm%4POPkhOtZt?sU3Vm zE|(>YHlD}Ngvfkaz6ueR8SYNv?lJxhf>STc3?1SbuKH9y#v3HQ(3OBG{_?RDON8{F z4b>dm44{O>m2~>_fDKCyy=%MMyWP)1YemhDwim9S|B9l9wgRQI43F+hqgSvALukd9 z6I!I*o2l-yFG*4t^^Repj%ii4r60k`Czztf5DshTZDf#ikAQ`Svy_|SL$jr%G`&%k zrvDaA(7Uk}frr-K|;q(TArZX?9r#@%D+VRMW?+FIkayY!1!a(UKjq_hPqCAodyQ(1JE0*~7#I7!0#pnCfqvkYc1R*ns1 zpExi{VdE$pO;}&aKgJm{LR0Ip4X#YR3QJ>^x-4}XFV{l1<*8MGhyCHwEqLvS)I@69 z5zSG<=Cnjy%;M5hM0L@^gz6N1eDsLTWr>c^!qKBo#H-WMbjKd{ZuE-`$Ghxo^k}N% zEW5(eW9g;7ME*hg>0{NGdTMr4Z|Ka@!^Y~W`ckweLLEg1Y?dff^|8@@kc4$f_US6? ziT0;Edf4{pCmD|ItT)=9>gZ+TF2X;&AFTg0+r!?~SWCpizQMf3i!ll68!3p+ku5u1 z>We2A%$xk8`q-B=7p4A~4eEw7s9^7%)Q|+a*ol+sQtLlp8+8BC!}gxUA(Qndg!Qm! z^hmlRN^g`IaLYT<4^tiQI9Zuu=|h%6t+9Dhtk}{I^Yw)mb)Iyk=5pMYPNi`>zTMGI z>x}I?S%|6s(n;)yJtx&=d(bn=(mfq(jJho_K-u zmKH~&0~t^bj!bp*3|owe9D&up^RgrAdKL4P^$ATEz{=kpt4T6yfAmPIW51L66ibx& zqNYU?k}O(AYY!}?(rAH7B6om(k8{M9ew1@8>){#Py$}S4a$_)svJ+MX3ipxoNgsDX zt(cmmQ!DRb*vmbbSfmq`#u0qHA)PKTpLqQ6^I5xx9q_O-IGuHNTcL?`67y52tWuXmjhnf4rS3js35PJy;K98l{qK3h3?DVZ;=P>2Elq!(YBhCM z)X7}WT8%T%k$W5mzSf^XG(X@>CHe8{|Eo3n#q4kC;!` zxp9c{4$Uc-ew&L8VM4haLvltr*#T$t<5Z{PfQ#Miww!UpbL&5Pgbt#+tJKA*uVSTI zpj-bBRGk59auKAriXAk44@gkv@@ce~#n(@F=p%K*0Z#k!tP zJ{KG0iVPw&DA1kkEob!4P~>Rqe@sVsFqg^@h`O zIKe%(^joAQVu7-yhxw!}R+O@IK31@plAw@5qG*3Ar@eWjRR- zBLH@oi^p4BdVEk)r^qFON@Gr9PaYr`l`_Qqy?717;*tI!nAGfd5EO4?jqFi#_o$Cu zfoOKK+0)&j*t$trF|U3TOmIwD@4!(w3=&db<2B264D3b4aJ=z+gMLqPf%!ja+6(t5 z&i~oJ_$-g+Kce$!@%sQ_zL@_M(x-Cg(WuQtKYv&^V*dA`KK0wE$L!_vzZdbNX74BV z`Fy%_S8O*Nd}@^9TgP1-TAKP9C*Os6k7a~M-Eud?Elqu&2$=tPWyk!N@%RN_1rU!0 z`j5|hS`K96rK!(zffzDcD8!1O{%A?^VldJ_#YK*o@dZR8cOBHi4-Xp86B52!MVf)H zU^k%KZP}cTuf9=(Ufz(~7{8=ad^Ky2WgW$i5zG3BKR@PAgFipv&rkXDbN(FU&lCK4 zia$^D=MaCM<LBA%bgP{K)=${DcBj`zj1_*kLAlgg@=ykvP3Pi;~Jwek6qBXoakDz-A zx`iNmQL6S3w2+`B1kER?lA!Mr6eNf~JXAkQ&@}|L6Lc9t|3c6u1pSsEg`ignI*oZ# zy_cX*3HlR3G`Xrz5JW#RsXj;0K7!IQZwGb~bPYi-1M-zO;vWyy1Qh}*YnmH5(9&30 z9t?1BRUoJoSCrQaE)H=v4oSYd%j+u}>S`VgGzqSzy6TFehUS`JO+$TgpuDM~T4^Q` z&0rBw6U6(gA5!9WGE`OE89)du{PfmKa`K(m4_g5~uU0e^i1>P()xx`YOGXnA7s^&n@4^k^V*3ctZ9SD}kWo0#AqEOHFTkKht_Evf{*0cs{o9%0)u64w{-3 z)HmQ?SCm)O2JDRuHT6MaKxGv)k`=>d+ycT{JGY8#xh3m+x!Lk$%9=oZaIQVr zw8majU(pmOZ^l1evsX93pa*IygVmQPCH3Vifdw1v<+Zg970}$CRoh};xh5EZmta%Y z0(;g{dzNQzmTzuWu|10>i@XYcRFHYSYw=f8#~1LLwklC7{kI8@K~c8dZ`Qvi>J{bl zM@IgKe{E8T+ilWCNp*Db`ZfTGH&BlA65&tRBm9X{(%KSArENp5lb97{@YWX(<<5S0 z_ca9#udd5Ei1MFO?s#Rx^^H>R{;1$nw!PWWUOeL`Z@*^Dth;^f4=*kF`EQy!$NM%& zJ|!af`lVdu)(?x972u9whAFh@U>WECj)wU1aATIyW!YbvYOyF5OU9*E%Ty)B(q_XN z3sloRN5lNGJb1Ta zX|-8w6UL>{VlxZ1WJlfs;o0SBf=_Wyv^}0um_D{>{kSK`hEt!i{)kjnE+%I>Xe38F zEaZ5JnJ!b#g7v&T_2%b-A_%ar&1k^biPgU3$W9IknSdzfGPmD>-#Ig)QiGM~h`{opH*eB1b_B=qk) zzyDLjw2#JtUl-sbKbZ35x-}e+vKiS& zerT0?J7oRE{Ln7>re1`P^dUcNl6-IfjkuuxF9VPEg&2QI2Kkj_56PVmzBb7>K3Q(k zdPMxS;8`&!c}rsk0kk|HzjwrQ-c3?$sYHcobP7!#Y*dq zf-+H-nX;TF%UQC#NtOk&{Jty~%W}Cat7X|F%eAt6T$b&!{HZK|Da+r=vP+iRWw}?D z2V~hV%THu^N|xtjnI`|;{UljVmgRS3nJ3GO`D3(x)B~d)81=xY2SzVZ)YjCx?y z1EU@o^}whHMm_NVqX#mk+Ht(AD3=%b=}+L5SNy zN_zqRqE#jT%X|Kn_~UW-OL=Ym;(Q?%v$T`_tL+&D{<_ved_o8YTn#PwW`OTKwdGAp zyRtevfp%w0U7)F^!dH&JlGp0LIWLbG@;Jlof#AZL06LPjbIe+_Ss7?rROPM>)Zu$Y zGc_qNiAB5%%|wTLEs9t1KgefFE#=id{xyRHt(XMG1FgRD9|U}Xx`w7TMfkLXk1ptl z$kl>egum$*tfGoW;j%_vK~AH})+9exmDki#eVYY@3Vb)=TpKyo!1Zq@IM9mkXkgnU z5O`Wy-9d0wZ9@aMYct2{8Y+p@$q~^r;awcZ$0kA|9C@Ir2A|=8_7F-RY-)ow_~U%2 z=@m7r2<{`eF7AVVjt1%~8rKljkSKj7!=E+7x2}4}O+*+ZLSu^_|GKXkuB(3C0-;C^0!NT zg~a!n{1QJT@m`64B5}L?5JEqtXDa|HemgkOsB zaSD7ha1}RwXQOK?VEiACshvsqfh2q&38!x!_&?tMt0bHPFp)ks3BNQ6pOl1WCgD?) z@avNB8b|tp^$en z3AbW~C(+ zTHG{Euft8#lfD7cG^43O;ZAD_g*An89&Y;9L19dxdn<1Gm|TF{iF*O=Lfp6t=&U~r z_y0>>>1uGhv}yxf`I`3+X(*C5&??#yw()J z5mQ2Kc{6@LVv^`5B>qPE8xnuB_z?;HKt=p4gtrVfP;ZLAiU0gW{JO+M>H`nN2|@|U zRo~Eroia`nT1i?(19nXM6TPd@W1uon38b+_>~#Jg)HFB8tLq-9h{Mg*P2YO8aycVTdOFJp-!e;Bwtqas8?9&SGo6~ZM%fGUvstWnqh~1V_ zzJ&Z`Lz3+84-C=yxOg%i(s?fEGuNF$O>fuEOVifSa70w@*s>nQshQFDKfJmDn%M3qT;+vnx?X%{5Qb?4>JG& diff --git a/matlab/mex_breezyslam.mexw64 b/matlab/mex_breezyslam.mexw64 deleted file mode 100644 index 28bdc0a3788bb8904aa184b6dfab3d2f8c9fa44b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 18432 zcmeHv3wTqf2qO|3^KcL}CN{*1CD;lwIKc!%2)2=B?Ba)xECX6?K%%*nqWwieg8Fk z%T`F*uiv@fz2ALQe0$c+T5Hy;|QK>8Jm^sgrh$YZ8I zKZYGlK0V`5ocr{Q(wf>Pi_hP<*(dQlKx3_kp{Eo9`YlndMit?%b-mOOj-<7Rj3jBtxuL)SM^KB9E zu3AqG$+}+i5+`F-4=1tj*fUC_v;mfB86BU(*lb`XYgorZK$FO~#u1==JYxn*@QXsW z7X=vYr&HrVDFmxlDGI(`)-Z*u7`qEqx4WPZ3G^{`kQ%B0PHTd&jI|C+5XHYaDP*im zDE`eg<^{Yh0g#@`MnWo6-_eiQp9R{?^H)^{DjEAn36w%S#5)g9%&!oXc^VrtCV^0b zhx)J-Pt327vG6>f#whw&0V1^tLhKL4!g>BCzXwQR6WB(`bHs9qYT)zMHG`_6C7Gwb>kb*(7cgc!uS}TE zn9RErBOtKl0y`+p2?vviQTc3|Mppnqze%T$U1k*I3p)9PMpkYnp@Y)cWhOyBq?3Q4 zk$*Kz&Jg6sbu!n;59wqyz6|=J^%kAs+X8N|=91_vnFp+Sa$AR`{OGBXpWfd(tBE?D zL7mQ(d7sShFiP10D+Io&WC}SeOz7Z{Vn*Sx1(vIpvIdm#DDex(z7BK9d_dWd%9zu3 z;(1zf&X9c!KdJopslpg?(8-jSMgEM;dz1l@t+UEf7TO8gWWK>#NJ^GK$=s-t-=LpQ z=2vCzGAcWSgjFWllco5ze1`1Fg~6+H#V|DrWS3H@6G3qZ3Jl_~<|?anx_yM6K0?n^ z=Iits8r>qQ6)NMjVixStqZm+dhaAwF!v?1v{N+K(LwD+e@UDtm(xZP^8muge9?=Y0bJiMmtnSw#d zL&42^l?PFfIn09L*o?gW5DkXRFSz(Ap>oC-s%q!yRtG;S^D{~|T-tR*Hd~135<`|D z@?yxAAREgWv|)I^ydgTgM&(1~(FzUkSW&GGQw~Flaoui}9l@D2$hkBL?HjaNDDV7` zwoREE9xVW{b;!Y8)^35p2tPrxL#5UV*<+MUagxbplCm9Ei|(&OxX#|R9x&hDp!r7Yp+ZR( zqE?7W_XRddnXka4%Tpu`pP?uk(n99f^I2l8oCSeq3KW+l(^?ad?_kDY-e}#)F0~d) zrd8CZ5+f2uXv`YS7fdxwSc@_b9i!z^@fRnJ%wfSYcrYIpbW4UiSYcUXUt_;p4%PI) z)+S5^H~*uXUy`zYJuY5s#G1&PORGi7Zs}5%2<5w!`G7igoS+H?N|dQOT^gakH%Iqg z4r`h6#U!#wm+}c9>ceF~?+WErt(f8VSd=a;pM{kg&F8`tL&|S8LY^?zFSPS@qV-$5PomFHA~8NhG{nV zNQ9f8k+O@sQRz9c9?Oc!U}8!+dx611MqRhHMQH95mOBpl+p0Ls=V8vUo+Ou5z{#?71)ii+Q{@!Bl z$pb5V!A1K<`@Q!b74vb;-D@#+xu}J8vS~4ZwoWWUPO{AZNc}6cAF2}Oc|-Xw8j?e_ z?m|m8VSElJgK+t0&F~4YV9F^;uv=f4vSZq^Kv(FPFJPhpN(V7NnGJjdQiJG^^aQq8#7&VT?qWPyL3Ri=bg zuQKv;%45LMHbWc9Ln41x=H1F9M67+6Nlm>oc_a%04z$=&B3?!7u2WdGm zao&ZUugabaa>8q9h7ZV4*XItUmtYu9ICA>zJ3kT~S^R`hQ`+;dXonvZ>dPKYO|+NH zUw@I&Bur3qy3vb6#MPCfIHYHLYfosVcy^##n(jdF^55K%!;ixdj{Ken$2${_*%Qt< z^CLSZ*b`1UVV*JeoHNdxW80GKIj5Wn5xex8ctDRCq7;JS`3sL9(0Rwp1DSZ&GhOu^m6LwrN zWzbFmrin+$4xQCf!;qaPTCtR5&x#DS>$3EL3S?O3McAI!T?PBPQ^AG1fe%yu#Gtfs z2z%@mDhyI`X;9j537L(b0)1SW4nISpWS&6O&nXFmr4!p=VwYfvA7&{s3bLvZ)uN32 z`=>~I?p9URA!VT-NUN~Yq%3UEWG&iGP%5>SAaz?SkYbKWS=!{0vb0r2o8*3QVv|Hj zyhYzADJ=em_$-$n!MKRi3X~hjQ51hVW)*^e#c0vv&$nEM5>+>H!h%wQAQTxLvvKpxlmdans`DeFdiQohS!!(ht z7yS}~Vf*93CPW7~$q1WE8;^@44!DY7R66C19B|-<^Bmkx@tz&K^ATwG4-n#{(^MB~ zz+b55QJGM?ts@U}74e-rSeUVTt-Tas9ZgRPCNTdFVH`|8gvTuhoV>4?54rh!I4F@; z2drWm?k^LglD*xEfM(;OM}ZZCIUl=HHO7G^#c|~(l%UMk44Q$n{uxX{i!u-e0}b~m zBxN1S0Jc6VZiV)9Xl|AL32voCh+gpT(Pm`gdeenishP3Necx^fOjI|6X89vkq z=msQzftjzx{>bdHhQ&AI7zsZ#5bh1gCZI$izqMY9483>DTZZ$ zq<;oQuqfA^B~wz}Bg9OU4&{Do7pTjmVc4B8i7CBuz z;%2B2ZXRpr#|{(b@wWF861pST;^CyJHiZB*R>4xM%-0$b&=$&Ea!_FHz{Z(NyGE7+ zLQ9abD9>F{RX2Y@1RN^*W7=*lliQ9P<+fvq_Iw)d_6NQcjsT+sn@0yNOB+Tvb`BbL z8IY!djO~)91vs@qMr~k*KVUKt4DU<0cU@^5fvQy;qC8S{>lz< zo&SkBkBi;G$)V7*itPDR4i$WgDT-d5Rm~sJ4T|Q1E>bpRo-%{X-Kn`Cx)0J_(5uK| zk)MSRQq9kfRC`9N_IpcAwf!U2ek7`e3jUjDGFSxngJv5?>ep%YpN_3xF;f3tQ9oE@ zvV|Q%&y-+MMzAP1%KkTzpP+B8kt+E{7hMx;qX8g-%w-m4EcBzosRplrxI zJ+|JP!0Da8fGj%yl}iK2$okr?!N~m;d?(^K7yK%@&T0sT-*o15$?B|gIOLyl^S50= zY$Cxemb>9Bdzn}xrb44#%D%q`bM^&m2IicL4>+alqAnyW4q-RJ;xpJG3=77zN1WHl zX<&}e3B$j4*}_44M$n#X=dy(-g=*vA=uhNC+@51t!dFpgBX-%cC1Fvm#`$a20 zoJfY7@R8?j6-?jUiWZmj|=(tXFPDXJCyhZYA`wSUi@B)I@${2S#vJ!l%5s2 zgSSiT6EP@7mdgkqb|YhqqEiW-1$63ewJ2BZl-rIZy4u1Q#!~h{ z>wZAA)7*II?lo)HNZBul02`PGwu)V`mFSp@hD0n8`;|{*?p*HRgZ9YB(;W$e;44OZ zxD%6k=n=4D2M`Az3(buz<(33$QBc5&DMNHExKM7mjz)Je7X&R!QE9X0Ys$4s)cx;4 z@wL=n!I29BHUi`cfh-VXoj*{14i(yl)E$}m#~+xc9V4Jg0v#tXTgf?r6&X{1yxM8c z|8mD%d;X^nWVt!+2Z@TSX6$gZ6|Ni7O}ShS7O!&fk8Nr{7>ZYs0qpszvH@kH`BpCAr?_6GPZ}M?@l9k!OL+7c}7G*6eo2-cQlIIQE_WL z)D9o~3diANJEG|6g{&=~N$vjxW)~lYAt^w_v)l;PI#F1dL>7&pbOxES4l12QU@szz zqsk1@9jI%R(0QP22yM;jg(v%xN}rP$AvVmih=>^ zD+L2#Kxg|$fw9LADCphD|pUgLTgi2+DLN(M@1yzL7Ad^X?+CuZk-SR88Y8n{dTUL z(+|&$0DAqgPPmZP+vJ?fK(rD4w>!|jcD{#LOudcAVTEJL1t>-$k0_nUfkds*GT(TL zy3tECT6+kN(I$f1b;pB_`H9NY8u#mR+b4;#`Z95xL&H=XaroF(i71>z#=SJL`McV5 z!j>S0pKexTw}O$^EWoaQ4#_?Hk$Cp%Z(Xj7@5BzFkCB57fI_9jeUO1JK82%Q9~?sG*9maXh<_56h$%d z$v+^E=})Aot8gPo8S@!jC$_&O?fM+`2{G#!DB^s?Cs2pbn#zw*yznymV>`mTB7I|R zUHod~4+iNT)pqqVOkR)`TpCK92$f6=vo1s5h(6On6VpZbkt50%P&QPgO6e+%5o($o z?d-C&0b$6xE@^!y2$!WYazX1lr1TTg`b*M!OucmxpJ`Zba$-luEWCX(yGSmb= zHr(@L(U#tbTr}9;2&Lxym5!!EUFxFfL`1ntIv*VtbySF=UWQ@GJ}~0S3;p@M!PXaY zPi22a@NXe0M(}}BGu6^sET{F|FyMJ<{yFKP0(2`luU&mG^D=sri5`)q`=>|e zkF?J5Bi3)iD(oMPN{jw%CRxX^+Qd}R{Uzp-ST+=4wZdQ^V-8VVMT;mJSK)OQv!scvp*)BY1z)%}?0d zE)Lm0V~IGd1;*QXp|RLgl6a-Zop4U}bh&4qb9s*5!P60O6`APmOTr4{XZ8dskShJ! zp3v4iAn$W0bh&K^zfQo@TJGt)8LE^2&MM~iJXyqLN)YBc1as1LO4i6WFmk^p=cmP4 z7;DC!6ERg^iKuFG{Nl z`KP5YT5w|!)E<$vg3FE4(81um)P zDp2LcP8Rq*qCALsdko2h%~lWrJCKSOSZOPTq_Ci+Ek;4GJcqJ7|MO;pJAWYXw3~nK z<`?^afcMd=`9+X$k0xXoj8q|LNCpTl>7OZr45|GhDf!5fizxS9K;9PadkvuNTAbA0 ziOjH_IHK?CEj1VNbxS=30kD)oHxx&)Jse2Y?8GsE{Y!CD-A<~6(!2nj1`l}1tV%V= zI126pLy^X?5I}JO-Dy|?E_fW){!w!Zg+h-QZUIJKyhH{F%z(7pkWt=D6b+8Wi_PbW z-9>kxvp`V|TXA^MX1}bVo9~=kVa}#X&3`!av}?N8jQ%%j6sE`5!X_)HON1y z=Y6XAObX_cp#^;6CP-Nc6>KL|^M3#j3p8R8sOI0H(6;0*Fkzx;d^dtmHSY)Ed1?y& zRW<*R>g?;IhK=)7bB5qHd>>T_ zQ>dDgG|X0Dv<53hg`=8>r@c#Aj=3gdqK2>Zs9nHg*Anwf`#dc)bqhJc(2^>E{xNjt zg6SKtcH^vNl=I*9e>8k|R<3W$pCiSyFo;`NA|78?Tzw(wEeBNyPD zXAAFKOrMlCr#?R#U8r7sXNL4x7+0rmkG%jFOm2SOEneOgraPtIlqKB}a3hZZ{6Ffthr2nX~9$P&V3x;70CIWfmHcQH`c-n zAot%#pH1Yd=AYn%EyhDNKY^@+_qc3tspfq#`NxO_+;}OZ5jhZU_=!BX?JfBbdWIh` zGkfGiHoZA_{=06;(c=zIU53_NS6)HRL23yH1-Fj(X6mfjqK|V#I!~mzB3&R-2bxi5 zeG6&qXd{MMLophCVZzVJ!Qw2L$8lq@*s7d{ui8uV^s8SO4D)vyP9qp4ZFQz$iZ;`? zg7VkuqanPx$ivPTmL7|6{L|%NO#T=CGFEBA<8}DqO0E314v*;YXFB{x9k%GON{1yn zT%f~v9e!M_$@@Tuuj}x*4kze*(Jnsc)^fkr;eY6`ONYPEVYd!n*Wueb9MmDx<)`X! zvJPkKFi(diI^3kg79BpJ!=LH!pblTq;Zu4$`*irI4qJ8DqQhvvD)jQQ>+rMn@;Dt{ zTcOqXSch-x@Qe=ss}2w7Fe?8qL3b`|rM)QnTM*3<3_nyzfU$Lj0CHV`lUs%o18l?@(m zc|#+l4f62G;skULWrKS-w zx7Jn#YDQ#07RHxMX<1^KRb^?R#8=zmt!uJuCsehi9)seiQIGxxZUax_CJdzq0)36O z4FO^XvKsS4M?{%%Id)&+f65*)67+1V4wq`M*RI2tbbR>9U-+L(F5Y#Ul2gBY`=g@^ zdoSIUl6-JtSfdXty|{WqA)fH*{d-r>{=xZ|$U)=xphy9AIjo{YUnh5db-hIC*TXm| ziAhzXn6waZ!DwHcC52gP;#qoC3QI3dX6Xx!Ed4H%FD@OhY8-H5fito6^T16`VUq*N zY;u*6O)ee9CPT*L1rx%PCdIQ+aZ_3Me8#%bpe8fv>-Cq8WlLKmHn+yaW_Dz=nSm@e zv+5=`bK#9_W?UxA=$+7=Uc%TU+;B?^XEQ18MrQPlsz?HkWLw}6Wx>0NNubjF6L?N#I0DrFRN^%z;X^(X97;f6C!wzyU*U?@;?tUvLGW9GR8wj8tNNSKb9|UUK*PR**8>Wup3GzvKtmo zU^hT!diS`nm=j6yENMYJedNjFrlzu~9i!RQKnk0RwKf%NZR&za0~34A-EkUU$t1Ip znL#&KB{MVV=DWs*|8$FXp`Op7%*7vuS@jMM3HqK;A2sfgLb3$XvU+A+^! zaCN}d0Oz|ys3ns3f!kY%wd)$eEuh#9xNrLXJ-*Yd@_&1L_gX@I%W`j^#Lvo+E5ZgJ z@UCpG_xfu+#gzenZA4b}5KZjPVfl{6X2c}AF!ru48;vz=7UJq^H`TYytE#JGx3PLJ1aI*cd+Qth z+e#2UAnt=WRz#E%R0&iw;`eG5m~8O2AUtFD7^;05wKT50ytZ*ud3AGxhp`_U>UBNW z7^>?U8~v=v;Aw1PCk+T}n8!eI2V-XpO?vHaL(@O_A@6ZRUE^lvf^;8SYpBMbo8oe8 z_1#5x-JF{%RAr+Q5Ta6yRqjEBhvP+@hwH3&QFyngr*!uxz>9$#QtZ5_n2Wx${%M7EonH*ZE< zTGQCL1%ELLY@HXuX<(br3mvK(%Nr`|y-moXRzqbSY?D~t+yF(lFd5vib7O;O{ImFS zf&EcJxsLgmIfbev6q%vTmzl#$sz=M=#MO0PuaBKcEJa6a8!&lF{EZ%O6N0?sahO*{ z&3-?ID~fzCaZQ8Pr7CBO$LpggTkj70_r%rSz+WWCC%Bp%dQVII-X@v@HzwW{sJpw? zA84+uThp+$wgF=@op5egqS?0$15OP%8xXxRmO+%FN?)Ma?=5OXckuVB?=eSh69ven zHJJHT?%GZMO8>SZ3}X}fw}d;Jy<*N)H~Q=8tWnh1EUfUuc=W8a*0V(*7<)OUriq}0mFlkEhDlssT3he6H=+Ap%CQyByH+~gwguX3BJUFYn_@rwu-Kn~BUpoS z9UdQ^y$F3C24u7z$~}!RO;wn#43V6eGXb3^9;Q2g@DTh7o;>9D z18zag{}}QiY~dJ&#(v`eux#mST>*sSL%9)3#CL(<=W|A$y0QRorJG630> z%BH#XwH|+CQ)6{tuBWm7*2<>(dH3C%X~EuITkUNM+)dGAW5bfno9E?bE=^0ZSPBCE z=B9uPG1A{C&8F2u%_gs>*;$0(h_@#-QjSpTwYeDXZ~l(bZ#_Z{Cb}>K5cp$nZN)3zp}vp0LZ1+ AaR2}S 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() - -