vato007/breezyslam
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Restored Matlab, Java

Browse Source
This commit is contained in:
simondlevy
2018-06-30 22:36:15 -04:00
parent 16ee48385f
commit 0ac92380c5
33 changed files with 3033 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
README.md
View File

@@ -5,8 +5,6 @@ BreezySLAM
<p><p><p> <p><p><p>
<b>NOTE: This branch (legacy) contains legacy support for Matlab, and Java</b>, 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.
<b>Simple, efficient, open-source package for Simultaneous Localization and Mapping in Python and C++</b> <b>Simple, efficient, open-source package for Simultaneous Localization and Mapping in Python and C++</b>
<a href="https://github.com/simondlevy/BreezySLAM">This repository</a> contains everything you need to <a href="https://github.com/simondlevy/BreezySLAM">This repository</a> contains everything you need to

363
examples/Log2PGM.java Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
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<n; ++k)
{
this.progBar += s;
}
}
}
public Log2PGM(boolean use_odometry, int random_seed)
{
MinesURG04LX laser = new MinesURG04LX();
this.robot = new Rover();
this.slam = random_seed > 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<int []> scans, Vector<long []> 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<double []> trajectory = new Vector<double []>();
for (int scanno=0; scanno<nscans; ++scanno)
{
int [] scan = scans.elementAt(scanno);
// Update with/out odometry
if (this.use_odometry)
{
long [] odometry = odometries.elementAt(scanno);
PoseChange poseChange = this.robot.computePoseChange(odometry[0], odometry[1], odometry[2]);
slam.update(scan, poseChange);
}
else
{
slam.update(scan);
}
Position position = slam.getpos();
// Add new coordinates to trajectory
double [] v = new double[2];
v[0] = position.x_mm;
v[1] = position.y_mm;
trajectory.add(v);
progbar.updateAmount(scanno);
System.out.printf("\r%s", progbar.str());
}
// Get final map
byte [] mapbytes = new byte [MAP_SIZE_PIXELS*MAP_SIZE_PIXELS];
slam.getmap(mapbytes);
// Put trajectory into map as black pixels
for (int k=0; k<trajectory.size(); ++k)
{
double [] v = trajectory.elementAt(k);
int x = mm2pix(v[0]);
int y = mm2pix(v[1]);
mapbytes[coords2index(x, y)] = 0;
}
return mapbytes;
}
// main -------------------------------------------------------------------------------------------------------------
public static void main(String[] argv)
{
// Bozo filter for input args
if (argv.length < 3)
{
System.err.println("Usage: java log2pgm <dataset> <use_odometry> <random_seed>");
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<int []> scans = new Vector<int []>();
Vector<long []> odometries = new Vector<long []>();;
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<SCAN_SIZE; ++k)
{
scan[k] = Integer.parseInt(toks[k+24]);
}
scans.add(scan);
}
input.close();
}
catch (IOException e)
{
System.err.println("Error: " + e.getMessage());
}
// Create a Log2PGM object to process the data
Log2PGM log2pgm = new Log2PGM(use_odometry, random_seed);
// Process the scan and odometry data, returning a map
byte [] mapbytes = log2pgm.processData(scans, odometries);
// Save map and trajectory as PGM file
filename = dataset + ".pgm";
System.out.println("\nSaving map to file " + filename);
BufferedWriter output = null;
try
{
FileWriter fstream = new FileWriter(filename);
output = new BufferedWriter(fstream);
output.write(String.format("P2\n%d %d 255\n", MAP_SIZE_PIXELS, MAP_SIZE_PIXELS));
for (int y=0; y<MAP_SIZE_PIXELS; y++)
{
for (int x=0; x<MAP_SIZE_PIXELS; x++)
{
// Output unsigned byte value
output.write(String.format("%d ", (int)mapbytes[coords2index(x, y)] & 0xFF));
}
output.write("\n");
}
output.close();
}
catch (IOException e)
{
System.err.println("Error: " + e.getMessage());
}
}
}

69
examples/MinesRover.m Executable file
View File

@@ -0,0 +1,69 @@
classdef MinesRover < WheeledRobot
%MinesRover Class for MinesRover custom robot
%
% Copyright (C) 2014 Simon D. Levy
%
% This code is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as
% published by the Free Software Foundation, either version 3 of the
% License, or (at your option) any later version.
%
% This code is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with this code. If not, see <http:#www.gnu.org/licenses/>.
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

175
examples/logdemo.m Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
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;

28
examples/rpslam.py
View File

@@ -30,6 +30,9 @@ from rplidar import RPLidar as Lidar
from pltslamshow import SlamShow from pltslamshow import SlamShow
from scipy.interpolate import interp1d
import numpy as np
if __name__ == '__main__': if __name__ == '__main__':
# Connect to Lidar unit # Connect to Lidar unit
@@ -52,36 +55,33 @@ if __name__ == '__main__':
while True: while True:
try:
# Extrat (quality, angle, distance) triples from current scan # Extrat (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)] items = [item for item in next(iterator)]
# Extract distances and angles from triples # Extract distances and angles from triples
distances = [item[2] for item in items] distances = [item[2] for item in items]
angles = [item[1] for item in items] angles = [item[1] for item in items]
print(angles)
except KeyboardInterrupt: # Interpolate to get 360 angles from 0 through 359, and corresponding distances
break 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 # 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 # Get current robot position
#x, y, theta = slam.getpos() x, y, theta = slam.getpos()
# Get current map bytes as grayscale # 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 # Exit on window close
#key = display.refresh() if not display.refresh():
#if key != None and (key&0x1A): exit(0)
# exit(0)
lidar.stop() lidar.stop()
lidar.disconnect() lidar.disconnect()

133
java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
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
* <pre>
* @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}
* }
* </pre>
* 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 <tt>scan_size</tt>
* 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);
}
}

52
java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
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

90
java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
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

103
java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
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

118
java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
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();
}
}

67
java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c Normal file
View File

@@ -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 <http:#www.gnu.org/licenses/>.
*/
#include "../../../../../../../c/random.h"
#include "../jni_utils.h"
#include <jni.h>
// 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, "<init>", "(DDD)V");
jobject newpos_object = (*env)->NewObject(env, cls, constructor, newpos.x_mm, newpos.y_mm, newpos.theta_degrees);
return newpos_object;
}

108
java/edu/wlu/cs/levy/breezyslam/components/Laser.java Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
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;
}
}

89
java/edu/wlu/cs/levy/breezyslam/components/Makefile Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
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 *~

96
java/edu/wlu/cs/levy/breezyslam/components/Map.java Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
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;
}
}

115
java/edu/wlu/cs/levy/breezyslam/components/PoseChange.java Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
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("<dxy=%7.0f mm dtheta = %+3.3f degrees dt = %f s",
this.dxy_mm, this.dtheta_degrees, this.dt_seconds);
}
/**
* Returns the forward component of this PoseChange object.
*/
public double getDxyMm()
{
return this.dxy_mm;
}
/**
* Returns the angular component of this PoseChange object.
*/
public double getDthetaDegrees()
{
return this.dtheta_degrees;
}
/**
* Returns the time component of this PoseChange object.
*/
public double getDtSeconds()
{
return this.dt_seconds;
}
/**
* Forward component of velocity, in mm to be divided by time in seconds.
*/
protected double dxy_mm;
/**
* Angular component of velocity, in mm to be divided by time in seconds.
*/
protected double dtheta_degrees;
/**
* Time in seconds between successive velocity measurements.
*/
protected double dt_seconds;
}

81
java/edu/wlu/cs/levy/breezyslam/components/Position.java Normal file
View File

@@ -0,0 +1,81 @@
/**
*
* BreezySLAM: Simple, efficient SLAM in Java
*
* Position.java - Java code for Position 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 <http://www.gnu.org/licenses/>.
*/
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 = "<x = %7.0f mm y = %7.0f mm theta = %+3.3f degrees>";
return String.format(format, this.x_mm, this.y_mm, this.theta_degrees);
}
}

97
java/edu/wlu/cs/levy/breezyslam/components/Scan.java Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
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);
}
}

29
java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java Normal file
View File

@@ -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);
}
}

132
java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c Normal file
View File

@@ -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 <http:#www.gnu.org/licenses/>.
*/
#include "../jni_utils.h"
#include "Map.h"
#include "Scan.h"
#include <jni.h>
#include <stdlib.h>
#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);
}

64
java/edu/wlu/cs/levy/breezyslam/jni_utils.h Normal file
View File

@@ -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 <http:#www.gnu.org/licenses/>.
*/
#include <stdlib.h>
#include <stdio.h>
#include "../../../../../../c/coreslam.h"
#include <jni.h>
#include <stdlib.h>
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"));
}

130
java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
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("<Wheel radius=%f m Half axle Length=%f m | %s>",
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;
}

136
matlab/CoreSLAM.m Normal file
View File

@@ -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 <http:#www.gnu.org/licenses/>.
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

44
matlab/Deterministic_SLAM.m Normal file
View File

@@ -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 <http:#www.gnu.org/licenses/>.
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

59
matlab/Map.m Normal file
View File

@@ -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 <http:#www.gnu.org/licenses/>.
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

72
matlab/RMHC_SLAM.m Normal file
View File

@@ -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 <http:#www.gnu.org/licenses/>.
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

66
matlab/Scan.m Normal file
View File

@@ -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 <http:#www.gnu.org/licenses/>.
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

101
matlab/SinglePositionSLAM.m Normal file
View File

@@ -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 <http:#www.gnu.org/licenses/>.
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

112
matlab/WheeledRobot.m Normal file
View File

@@ -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 <http:#www.gnu.org/licenses/>.
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('<Wheel radius=%f mm Half axle Length=%f mm>', ...
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

19
matlab/make.m Normal file
View File

@@ -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 <http:#www.gnu.org/licenses/>.
mex mex_breezyslam.c ../c/coreslam.c ../c/coreslam_sisd.c ../c/random.c ../c/ziggurat.c

294
matlab/mex_breezyslam.c Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
#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);
}
}

BIN
matlab/mex_breezyslam.mexa64 Executable file
View File

Binary file not shown.

BIN
matlab/mex_breezyslam.mexmaci64 Normal file
View File

Binary file not shown.

BIN
matlab/mex_breezyslam.mexw64 Normal file
View File

Binary file not shown.