From 0ac92380c503e18124bc76e8f5880e941f1ef15d Mon Sep 17 00:00:00 2001 From: simondlevy Date: Sat, 30 Jun 2018 22:36:15 -0400 Subject: [PATCH] Restored Matlab, Java --- README.md | 2 - examples/Log2PGM.java | 363 ++++++++++++++++++ examples/MinesRover.m | 69 ++++ examples/logdemo.m | 175 +++++++++ examples/rpslam.py | 38 +- .../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 0 -> 23466 bytes matlab/mex_breezyslam.mexmaci64 | Bin 0 -> 23440 bytes matlab/mex_breezyslam.mexw64 | Bin 0 -> 18432 bytes 33 files changed, 3033 insertions(+), 21 deletions(-) create mode 100644 examples/Log2PGM.java create mode 100755 examples/MinesRover.m create mode 100644 examples/logdemo.m create mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile create mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Laser.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Makefile create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Map.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/PoseChange.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Position.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Scan.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c create mode 100644 java/edu/wlu/cs/levy/breezyslam/jni_utils.h create mode 100644 java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java create mode 100644 matlab/CoreSLAM.m create mode 100644 matlab/Deterministic_SLAM.m create mode 100644 matlab/Map.m create mode 100644 matlab/RMHC_SLAM.m create mode 100644 matlab/Scan.m create mode 100644 matlab/SinglePositionSLAM.m create mode 100644 matlab/WheeledRobot.m create mode 100644 matlab/make.m create mode 100644 matlab/mex_breezyslam.c create mode 100755 matlab/mex_breezyslam.mexa64 create mode 100644 matlab/mex_breezyslam.mexmaci64 create mode 100644 matlab/mex_breezyslam.mexw64 diff --git a/README.md b/README.md index 179405a..8222f00 100644 --- a/README.md +++ b/README.md @@ -5,8 +5,6 @@ BreezySLAM

-NOTE: This branch (legacy) contains legacy support for Matlab, and Java, languages that I am no longer supporting in the [master](https://github.com/simondlevy/BreezySLAM) branch. If you're working in Python or C++, you should be using the master branch. - Simple, efficient, open-source package for Simultaneous Localization and Mapping in Python and C++ This repository contains everything you need to diff --git a/examples/Log2PGM.java b/examples/Log2PGM.java new file mode 100644 index 0000000..47d2100 --- /dev/null +++ b/examples/Log2PGM.java @@ -0,0 +1,363 @@ +/* + Log2PGM.java : 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 . + */ + +import edu.wlu.cs.levy.breezyslam.components.Map; +import edu.wlu.cs.levy.breezyslam.components.Scan; +import edu.wlu.cs.levy.breezyslam.components.Position; +import edu.wlu.cs.levy.breezyslam.components.URG04LX; +import edu.wlu.cs.levy.breezyslam.components.PoseChange; + +import edu.wlu.cs.levy.breezyslam.robots.WheeledRobot; + +import edu.wlu.cs.levy.breezyslam.algorithms.RMHCSLAM; +import edu.wlu.cs.levy.breezyslam.algorithms.DeterministicSLAM; +import edu.wlu.cs.levy.breezyslam.algorithms.SinglePositionSLAM; + +import java.io.BufferedReader; +import java.io.FileReader; +import java.io.BufferedWriter; +import java.io.FileWriter; +import java.io.IOException; +import java.util.Vector; + +public class Log2PGM +{ + private static int MAP_SIZE_PIXELS = 800; + private static double MAP_SIZE_METERS = 32; + private static int SCAN_SIZE = 682; + + private SinglePositionSLAM slam; + + private boolean use_odometry; + private int random_seed; + private Rover robot; + + static int coords2index(double x, double y) + { + return (int)(y * MAP_SIZE_PIXELS + x); + } + + int mm2pix(double mm) + { + return (int)(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS)); + } + + + // Class for Mines verison of URG-04LX Lidar ----------------------------------- + + private class MinesURG04LX extends URG04LX + { + public MinesURG04LX() + { + super( 70, // detectionMargin + 145); // offsetMillimeters + } + } + + // Class for MinesRover custom robot ------------------------------------------- + + private class Rover extends WheeledRobot + { + public Rover() + { + super(77, // wheelRadiusMillimeters + 165); // halfAxleLengthMillimeters + } + + protected WheeledRobot.WheelOdometry extractOdometry( + double timestamp, + double leftWheelOdometry, + double rightWheelOdometry) + { + // Convert microseconds to seconds, ticks to angles + return new WheeledRobot.WheelOdometry( + timestamp / 1e6, + ticksToDegrees(leftWheelOdometry), + ticksToDegrees(rightWheelOdometry)); + } + + protected String descriptorString() + { + return String.format("ticks_per_cycle=%d", this.TICKS_PER_CYCLE); + } + + + private double ticksToDegrees(double ticks) + { + return ticks * (180. / this.TICKS_PER_CYCLE); + } + + private int TICKS_PER_CYCLE = 2000; + } + + // Progress-bar class ---------------------------------------------------------------- + // Adapted from http://code.activestate.com/recipes/168639-progress-bar-class/ + // Downloaded 12 January 2014 + + private class ProgressBar + { + public ProgressBar(int minValue, int maxValue, int totalWidth) + { + this.progBar = "[]"; + 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)java.lang.Math.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)java.lang.Math.round((percentDone / 100.0) * allFull); + + // Build a progress bar with hashes and spaces + this.progBar = "["; + this.addToProgBar("#", numHashes); + this.addToProgBar(" ", allFull-numHashes); + this.progBar += "]"; + + // Figure out where to put the percentage, roughly centered + int percentPlace = (this.progBar.length() / 2) - ((int)(java.lang.Math.log10(percentDone+1)) + 1); + String percentString = String.format("%d%%", percentDone); + + // Put it there + this.progBar = this.progBar.substring(0,percentPlace) + percentString + this.progBar.substring(percentPlace+percentString.length()); + } + + String str() + { + return this.progBar; + } + + private String progBar; + private int min; + private int max; + private int span; + private int width; + private int amount; + + private void addToProgBar(String s, int n) + { + for (int k=0; k 0 ? + new RMHCSLAM(laser, MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed) : + new DeterministicSLAM(laser, MAP_SIZE_PIXELS, MAP_SIZE_METERS); + + this.use_odometry = use_odometry; + this.random_seed = random_seed; + } + + public byte [] processData(Vector scans, Vector odometries) + { + // Report what we're doing + int nscans = scans.size(); + ProgressBar progbar = new ProgressBar(0, nscans, 80); + + System.out.printf("Processing %d scans with%s odometry / with%s particle filter...\n", + nscans, this.use_odometry ? "" : "out", this.random_seed > 0 ? "" : "out"); + + Vector trajectory = new Vector(); + + for (int scanno=0; scanno "); + System.err.println("Example: java log2pgm exp2 1 9999"); + System.exit(1); + } + + // Grab input args + String dataset = argv[0]; + boolean use_odometry = Integer.parseInt(argv[1]) == 0 ? false : true; + int random_seed = argv.length > 2 ? Integer.parseInt(argv[2]) : 0; + + // Load the data from the file + + Vector scans = new Vector(); + Vector odometries = new Vector();; + + String filename = dataset + ".dat"; + System.out.printf("Loading data from %s ... \n", filename); + + BufferedReader input = null; + + try + { + FileReader fstream = new FileReader(filename); + input = new BufferedReader(fstream); + while (true) + { + String line = input.readLine(); + if (line == null) + { + break; + } + + String [] toks = line.split(" +"); + + long [] odometry = new long [3]; + odometry[0] = Long.parseLong(toks[0]); + odometry[1] = Long.parseLong(toks[2]); + odometry[2] = Long.parseLong(toks[3]); + odometries.add(odometry); + + int [] scan = new int [SCAN_SIZE]; + for (int k=0; k. + + 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/logdemo.m b/examples/logdemo.m new file mode 100644 index 0000000..c59e336 --- /dev/null +++ b/examples/logdemo.m @@ -0,0 +1,175 @@ +% +% logdemo.m : BreezySLAM Matlab demo. Reads logfile with odometry and scan +% data from Paris Mines Tech and displays the map and robot +% position in real time. +% +% Usage: +% +% >> 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/examples/rpslam.py b/examples/rpslam.py index 84b7140..f1099ed 100755 --- a/examples/rpslam.py +++ b/examples/rpslam.py @@ -30,6 +30,9 @@ from rplidar import RPLidar as Lidar from pltslamshow import SlamShow +from scipy.interpolate import interp1d +import numpy as np + if __name__ == '__main__': # Connect to Lidar unit @@ -52,36 +55,33 @@ if __name__ == '__main__': while True: - try: + # Extrat (quality, angle, distance) triples from current scan + items = [item for item in next(iterator)] - # Extrat (quality, angle, distance) triples from current scan - items = [item for item in next(iterator)] + # Extract distances and angles from triples + distances = [item[2] for item in items] + angles = [item[1] for item in items] - # Extract distances and angles from triples - distances = [item[2] for item in items] - angles = [item[1] for item in items] - print(angles) - - except KeyboardInterrupt: - break + # Interpolate to get 360 angles from 0 through 359, and corresponding distances + f = interp1d(angles, distances, fill_value='extrapolate') + distances = list(f(np.arange(360))) # Update SLAM with current Lidar scan, using third element of (quality, angle, distance) triples - #slam.update([item[2] for item in next(iterator)]) + slam.update(distances) # Get current robot position - #x, y, theta = slam.getpos() + x, y, theta = slam.getpos() # Get current map bytes as grayscale - #slam.getmap(mapbytes) + slam.getmap(mapbytes) - #display.displayMap(mapbytes) + display.displayMap(mapbytes) - #display.setPose(x, y, theta) + display.setPose(x, y, theta) - # Exit on ESCape - #key = display.refresh() - #if key != None and (key&0x1A): - # exit(0) + # Exit on window close + if not display.refresh(): + exit(0) lidar.stop() lidar.disconnect() diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java new file mode 100644 index 0000000..a57a162 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java @@ -0,0 +1,133 @@ +/** +* +* 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 new file mode 100644 index 0000000..458943d --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java @@ -0,0 +1,52 @@ +/** +* 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 new file mode 100644 index 0000000..45b5a43 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile @@ -0,0 +1,90 @@ +# 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 new file mode 100644 index 0000000..06269ab --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java @@ -0,0 +1,103 @@ +/** +* 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 new file mode 100644 index 0000000..085064b --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java @@ -0,0 +1,118 @@ +/** +* 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 new file mode 100644 index 0000000..e2e8d8f --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c @@ -0,0 +1,67 @@ +/* + * 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 new file mode 100644 index 0000000..aa77425 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/Laser.java @@ -0,0 +1,108 @@ +/** +* +* 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 new file mode 100644 index 0000000..35a37bc --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/Makefile @@ -0,0 +1,89 @@ +# 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 new file mode 100644 index 0000000..4bfa2f6 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/Map.java @@ -0,0 +1,96 @@ +/** +* +* 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 new file mode 100644 index 0000000..7682724 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/PoseChange.java @@ -0,0 +1,115 @@ +/** +* +* 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 new file mode 100644 index 0000000..9f898a8 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/Scan.java @@ -0,0 +1,97 @@ +/** +* +* 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 new file mode 100644 index 0000000..053b174 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java @@ -0,0 +1,29 @@ +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 new file mode 100644 index 0000000..6e4d8fa --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c @@ -0,0 +1,132 @@ +/* + * 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 new file mode 100644 index 0000000..acaa3a5 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/jni_utils.h @@ -0,0 +1,64 @@ +/* + * 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 new file mode 100644 index 0000000..f2b2822 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java @@ -0,0 +1,130 @@ +/** + * + * 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 new file mode 100644 index 0000000..ad17c74 --- /dev/null +++ b/matlab/CoreSLAM.m @@ -0,0 +1,136 @@ +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 new file mode 100644 index 0000000..2b7c8e7 --- /dev/null +++ b/matlab/Deterministic_SLAM.m @@ -0,0 +1,44 @@ +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 new file mode 100644 index 0000000..e3f960e --- /dev/null +++ b/matlab/Map.m @@ -0,0 +1,59 @@ +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 new file mode 100644 index 0000000..0062574 --- /dev/null +++ b/matlab/RMHC_SLAM.m @@ -0,0 +1,72 @@ +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 new file mode 100644 index 0000000..4004572 --- /dev/null +++ b/matlab/Scan.m @@ -0,0 +1,66 @@ +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 new file mode 100644 index 0000000..787753c --- /dev/null +++ b/matlab/SinglePositionSLAM.m @@ -0,0 +1,101 @@ +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 new file mode 100644 index 0000000..ac155c7 --- /dev/null +++ b/matlab/WheeledRobot.m @@ -0,0 +1,112 @@ +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 new file mode 100644 index 0000000..b00df47 --- /dev/null +++ b/matlab/make.m @@ -0,0 +1,19 @@ +% 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 new file mode 100644 index 0000000..b74cbf3 --- /dev/null +++ b/matlab/mex_breezyslam.c @@ -0,0 +1,294 @@ +/* + * 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 new file mode 100755 index 0000000000000000000000000000000000000000..0e9425666089018199330fdca8ff0426ea80a9e7 GIT binary patch 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 literal 0 HcmV?d00001 diff --git a/matlab/mex_breezyslam.mexmaci64 b/matlab/mex_breezyslam.mexmaci64 new file mode 100644 index 0000000000000000000000000000000000000000..97fc14b98362a43e56b32a23d6f4d933e4b04536 GIT binary patch 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& literal 0 HcmV?d00001 diff --git a/matlab/mex_breezyslam.mexw64 b/matlab/mex_breezyslam.mexw64 new file mode 100644 index 0000000000000000000000000000000000000000..28bdc0a3788bb8904aa184b6dfab3d2f8c9fa44b GIT binary patch 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 literal 0 HcmV?d00001