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

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.