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

Added matlab support.

Browse Source 
Modified some c and cpp code to help.
This commit is contained in:
Simon D. Levy
2014-09-15 18:27:38 -04:00
parent e3f41e3599
commit d25f5372f7
16 changed files with 1265 additions and 310 deletions
Download Patch File Download Diff File Expand all files Collapse all files

502
c/coreslam.c
View File

@@ -1,33 +1,33 @@
/* /*
coreslam.c adapted from CoreSLAM.c, CoreSLAM_state.c, and CoreSLAM.h * coreslam.c adapted from CoreSLAM.c, CoreSLAM_state.c, and CoreSLAM.h
downloaded from openslam.org on 01 January 2014. Contains implementations * downloaded from openslam.org on 01 January 2014. Contains implementations
of scan and map methods. * of scan and map methods.
*
Copyright (C) 2014 Simon D. Levy * Copyright (C) 2014 Simon D. Levy
*
This code is free software: you can redistribute it and/or modify * This code is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as * it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation, either version 3 of the * published by the Free Software Foundation, either version 3 of the
License, or (at your option) any later version. * License, or (at your option) any later version.
*
This code is distributed in the hope that it will be useful, * This code is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY without even the implied warranty of * but WITHOUT ANY WARRANTY without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. * GNU General Public License for more details.
*
You should have received a copy of the GNU Lesser General Public License * 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/>. * along with this code. If not, see <http:#www.gnu.org/licenses/>.
*
Change log * Change log
*
07-FEB-2014 : Simon D. Levy - Initial release * 07-FEB-2014 : Simon D. Levy - Initial release
01-MAR-2014 : SDL - Converted millimeters to meters for API * 01-MAR-2014 : SDL - Converted millimeters to meters for API
21-JUN-2014 : SDL - Added support for SSE and NEON * 21-JUN-2014 : SDL - Added support for SSE and NEON
10-JUL-2014 : SDL - Changed Laser scan rate and angles from int to double * 10-JUL-2014 : SDL - Changed Laser scan rate and angles from int to double
21-JUL-2014 : SDL - Made RMHC position search avoid looping when max count is zero * 21-JUL-2014 : SDL - Made RMHC position search avoid looping when max count is zero
23-JUL-2014 : SDL - Simplified laser detection angle min, max to total angle * 23-JUL-2014 : SDL - Simplified laser detection angle min, max to total angle
07-SEP-2014 : SDL - Migrated to github * 07-SEP-2014 : SDL - Migrated to github
*/ */
#include <stdlib.h> #include <stdlib.h>
@@ -69,7 +69,7 @@ static float * float_alloc(int size)
static void static void
swap(int * a, int * b) swap(int * a, int * b)
{ {
int tmp = *a; int tmp = *a;
*a = *b; *a = *b;
@@ -84,7 +84,7 @@ static int roundup(double x)
static int static int
out_of_bounds(int value, int bound) out_of_bounds(int value, int bound)
{ {
return value < 0 || value >= bound; return value < 0 || value >= bound;
} }
@@ -92,7 +92,7 @@ out_of_bounds(int value, int bound)
static int static int
clip(int *xyc, int * yxc, int xy, int yx, int map_size) clip(int *xyc, int * yxc, int xy, int yx, int map_size)
{ {
if (*xyc < 0) if (*xyc < 0)
@@ -120,129 +120,130 @@ clip(int *xyc, int * yxc, int xy, int yx, int map_size)
static void static void
map_laser_ray( map_laser_ray(
pixel_t * map_pixels, pixel_t * map_pixels,
int map_size, int map_size,
int x1, int x1,
int y1, int y1,
int x2, int x2,
int y2, int y2,
int xp, int xp,
int yp, int yp,
int value, int value,
int alpha) int alpha)
{ {
int x2c = x2;
int y2c = y2;
if (out_of_bounds(x1, map_size) || out_of_bounds(y1, map_size)) if (out_of_bounds(x1, map_size) || out_of_bounds(y1, map_size))
{ {
return; return;
} }
int x2c = x2; if (!(clip(&x2c, &y2c, x1, y1, map_size) || clip(&y2c, &x2c, y1, x1, map_size)))
int y2c = y2;
if (clip(&x2c, &y2c, x1, y1, map_size) || clip(&y2c, &x2c, y1, x1, map_size))
{ {
return; int dx = abs(x2 - x1);
} int dy = abs(y2 - y1);
int dxc = abs(x2c - x1);
int dyc = abs(y2c - y1);
int incptrx = (x2 > x1) ? 1 : -1;
int incptry = (y2 > y1) ? map_size : -map_size;
int sincv = (value > NO_OBSTACLE) ? 1 : -1;
int dx = abs(x2 - x1); int derrorv = 0;
int dy = abs(y2 - y1);
int dxc = abs(x2c - x1);
int dyc = abs(y2c - y1);
int incptrx = (x2 > x1) ? 1 : -1;
int incptry = (y2 > y1) ? map_size : -map_size;
int sincv = (value > NO_OBSTACLE) ? 1 : -1;
int derrorv = 0; if (dx > dy)
if (dx > dy)
{
derrorv = abs(xp - x2);
}
else
{
swap(&dx, &dy);
swap(&dxc, &dyc);
swap(&incptrx, &incptry);
derrorv = abs(yp - y2);
}
if (!derrorv)
{ /* XXX should probably throw an exception */
fprintf(stderr, "map_update: No error gradient: try increasing hole width\n");
exit(1);
}
int error = 2 * dyc - dxc;
int horiz = 2 * dyc;
int diago = 2 * (dyc - dxc);
int errorv = derrorv / 2;
int incv = (value - NO_OBSTACLE) / derrorv;
int incerrorv = value - NO_OBSTACLE - derrorv * incv;
pixel_t * ptr = map_pixels + y1 * map_size + x1;
int pixval = NO_OBSTACLE;
int x = 0;
for (x = 0; x <= dxc; x++, ptr += incptrx)
{
if (x > dx - 2 * derrorv)
{ {
if (x <= dx - derrorv) derrorv = abs(xp - x2);
{ }
pixval += incv; else
errorv += incerrorv; {
if (errorv > derrorv) swap(&dx, &dy);
{ swap(&dxc, &dyc);
pixval += sincv; swap(&incptrx, &incptry);
errorv -= derrorv; derrorv = abs(yp - y2);
}
}
else
{
pixval -= incv;
errorv -= incerrorv;
if (errorv < 0)
{
pixval -= sincv;
errorv += derrorv;
}
}
} }
/* Integration into the map */ if (!derrorv)
*ptr = ((256 - alpha) * (*ptr) + alpha * pixval) >> 8; { /* XXX should probably throw an exception */
fprintf(stderr, "map_update: No error gradient: try increasing hole width\n");
exit(1);
}
if (error > 0) else
{ {
ptr += incptry; int error = 2 * dyc - dxc;
error += diago; int horiz = 2 * dyc;
} else int diago = 2 * (dyc - dxc);
{ int errorv = derrorv / 2;
error += horiz;
int incv = (value - NO_OBSTACLE) / derrorv;
int incerrorv = value - NO_OBSTACLE - derrorv * incv;
pixel_t * ptr = map_pixels + y1 * map_size + x1;
int pixval = NO_OBSTACLE;
int x = 0;
for (x = 0; x <= dxc; x++, ptr += incptrx)
{
if (x > dx - 2 * derrorv)
{
if (x <= dx - derrorv)
{
pixval += incv;
errorv += incerrorv;
if (errorv > derrorv)
{
pixval += sincv;
errorv -= derrorv;
}
}
else
{
pixval -= incv;
errorv -= incerrorv;
if (errorv < 0)
{
pixval -= sincv;
errorv += derrorv;
}
}
}
/* Integration into the map */
*ptr = ((256 - alpha) * (*ptr) + alpha * pixval) >> 8;
if (error > 0)
{
ptr += incptry;
error += diago;
} else
{
error += horiz;
}
}
} }
} }
} }
static void static void
scan_update_xy( scan_update_xy(
scan_t * scan, scan_t * scan,
int offset, int offset,
int distance, int distance,
int scanval, int scanval,
laser_t laser, laser_t laser,
double horz_mm, double horz_mm,
double rotation) double rotation)
{ {
int j; int j;
for (j=0; j<scan->span; ++j) for (j=0; j<scan->span; ++j)
{ {
double k = (double)(offset*scan->span+j) * laser.detection_angle_degrees / double k = (double)(offset*scan->span+j) * laser.detection_angle_degrees /
(laser.scan_size * scan->span - 1); (laser.scan_size * scan->span - 1);
double angle = radians(-laser.detection_angle_degrees/2 + k * rotation); double angle = radians(-laser.detection_angle_degrees/2 + k * rotation);
double x = distance * cos(angle) - k * horz_mm; double x = distance * cos(angle) - k * horz_mm;
@@ -260,23 +261,24 @@ scan_update_xy(
/* Exported functions --------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/
int * int *
int_alloc( int_alloc(
int size) int size)
{ {
return (int *)safe_malloc(size * sizeof(int)); return (int *)safe_malloc(size * sizeof(int));
} }
void void
map_init( map_init(
map_t * map, map_t * map,
int size_pixels, int size_pixels,
double size_meters) double size_meters)
{ {
int npix = size_pixels * size_pixels; int npix = size_pixels * size_pixels;
int k = 0;
map->pixels = (pixel_t *)safe_malloc(npix * sizeof(pixel_t)); map->pixels = (pixel_t *)safe_malloc(npix * sizeof(pixel_t));
int k;
for (k=0; k<npix; ++k) for (k=0; k<npix; ++k)
{ {
map->pixels[k] = (OBSTACLE + NO_OBSTACLE) / 2; map->pixels[k] = (OBSTACLE + NO_OBSTACLE) / 2;
@@ -290,27 +292,27 @@ map_init(
} }
void void
map_free( map_free(
map_t * map) map_t * map)
{ {
free(map->pixels); free(map->pixels);
} }
void map_string( void map_string(
map_t map, map_t map,
char * str) char * str)
{ {
sprintf(str, "size = %d x %d pixels | = %f meters", sprintf(str, "size = %d x %d pixels | = %f meters",
map.size_pixels, map.size_pixels, map.size_meters); map.size_pixels, map.size_pixels, map.size_meters);
} }
void void
map_update( map_update(
map_t * map, map_t * map,
scan_t * scan, scan_t * scan,
position_t position, position_t position,
int map_quality, int map_quality,
double hole_width_mm) double hole_width_mm)
{ {
double position_theta_radians = radians(position.theta_degrees); double position_theta_radians = radians(position.theta_degrees);
@@ -329,47 +331,47 @@ map_update(
int xp = roundup((position.x_mm + x2p) * map->scale_pixels_per_mm); int xp = roundup((position.x_mm + x2p) * map->scale_pixels_per_mm);
int yp = roundup((position.y_mm + y2p) * map->scale_pixels_per_mm); int yp = roundup((position.y_mm + y2p) * map->scale_pixels_per_mm);
double dist = sqrt(x2p * x2p + y2p * y2p); double dist = sqrt(x2p * x2p + y2p * y2p);
double add = hole_width_mm / 2 / dist; double add = hole_width_mm / 2 / dist;
x2p *= map->scale_pixels_per_mm * (1 + add); x2p *= map->scale_pixels_per_mm * (1 + add);
y2p *= map->scale_pixels_per_mm * (1 + add); y2p *= map->scale_pixels_per_mm * (1 + add);
int x2 = roundup(position.x_mm * map->scale_pixels_per_mm + x2p);
int y2 = roundup(position.y_mm * map->scale_pixels_per_mm + y2p);
int value = OBSTACLE;
int q = map_quality;
if (scan->value[i] == NO_OBSTACLE)
{ {
q = map_quality / 4; int x2 = roundup(position.x_mm * map->scale_pixels_per_mm + x2p);
value = NO_OBSTACLE; int y2 = roundup(position.y_mm * map->scale_pixels_per_mm + y2p);
}
map_laser_ray(map->pixels, map->size_pixels, x1, y1, x2, y2, xp, yp, value, q); int value = OBSTACLE;
int q = map_quality;
if (scan->value[i] == NO_OBSTACLE)
{
q = map_quality / 4;
value = NO_OBSTACLE;
}
map_laser_ray(map->pixels, map->size_pixels, x1, y1, x2, y2, xp, yp, value, q);
}
} }
} }
void void
map_get( map_get(
map_t * map, map_t * map,
char * bytes) char * bytes)
{ {
int k; int k;
for (k=0; k<map->size_pixels*map->size_pixels; ++k) for (k=0; k<map->size_pixels*map->size_pixels; ++k)
{ {
bytes[k] = map->pixels[k] >> 8; bytes[k] = map->pixels[k] >> 8;
} }
} }
void void
map_set( map_set(
map_t * map, map_t * map,
char * bytes) char * bytes)
{ {
int k; int k;
for (k=0; k<map->size_pixels*map->size_pixels; ++k) for (k=0; k<map->size_pixels*map->size_pixels; ++k)
@@ -380,10 +382,10 @@ map_set(
} }
void void
scan_init( scan_init(
scan_t * scan, scan_t * scan,
int size, int size,
int span) int span)
{ {
scan->x_mm = double_alloc(size*span); scan->x_mm = double_alloc(size*span);
scan->y_mm = double_alloc(size*span); scan->y_mm = double_alloc(size*span);
@@ -400,8 +402,8 @@ scan_init(
void void
scan_free( scan_free(
scan_t * scan) scan_t * scan)
{ {
free(scan->x_mm); free(scan->x_mm);
free(scan->y_mm); free(scan->y_mm);
@@ -412,31 +414,32 @@ scan_free(
} }
void scan_string( void scan_string(
scan_t scan, scan_t scan,
char * str) char * str)
{ {
sprintf(str, "%d obstacle points | %d free points", scan.obst_npoints, scan.npoints-scan.obst_npoints); sprintf(str, "%d obstacle points | %d free points", scan.obst_npoints, scan.npoints-scan.obst_npoints);
} }
void void
scan_update( scan_update(
scan_t * scan, scan_t * scan,
int * lidar_mm, int * lidar_mm,
laser_t laser, laser_t laser,
double hole_width_mm, double hole_width_mm,
double velocities_dxy_mm, double velocities_dxy_mm,
double velocities_dtheta_degrees) double velocities_dtheta_degrees)
{ {
scan->npoints = 0;
scan->obst_npoints = 0;
/* Take velocity into account */ /* Take velocity into account */
int degrees_per_second = laser.scan_rate_hz * 360; int degrees_per_second = (int)(laser.scan_rate_hz * 360);
double horz_mm = velocities_dxy_mm / degrees_per_second; double horz_mm = velocities_dxy_mm / degrees_per_second;
double rotation = 1 + velocities_dtheta_degrees / degrees_per_second; double rotation = 1 + velocities_dtheta_degrees / degrees_per_second;
/* Span the laser scans to better cover the space */ /* Span the laser scans to better cover the space */
int i; int i = 0;
scan->npoints = 0;
scan->obst_npoints = 0;
for (i=laser.detection_margin+1; i<laser.scan_size-laser.detection_margin; ++i) for (i=laser.detection_margin+1; i<laser.scan_size-laser.detection_margin; ++i)
{ {
int lidar_value_mm = lidar_mm[i]; int lidar_value_mm = lidar_mm[i];
@@ -444,7 +447,7 @@ scan_update(
/* No obstacle */ /* No obstacle */
if (lidar_value_mm == 0) if (lidar_value_mm == 0)
{ {
scan_update_xy(scan, i, laser.distance_no_detection_mm, NO_OBSTACLE, laser, horz_mm, rotation); scan_update_xy(scan, i, (int)laser.distance_no_detection_mm, NO_OBSTACLE, laser, horz_mm, rotation);
} }
/* Obstacle */ /* Obstacle */
@@ -452,16 +455,17 @@ scan_update(
{ {
int oldstart = scan->npoints; int oldstart = scan->npoints;
int j = 0;
scan_update_xy(scan, i, lidar_value_mm, OBSTACLE, laser, horz_mm, rotation); scan_update_xy(scan, i, lidar_value_mm, OBSTACLE, laser, horz_mm, rotation);
/* Store obstacles separately for SSE */ /* Store obstacles separately for SSE */
int j;
for (j=oldstart; j<scan->npoints; ++j) for (j=oldstart; j<scan->npoints; ++j)
{ {
if (scan->value[j] == OBSTACLE) if (scan->value[j] == OBSTACLE)
{ {
scan->obst_x_mm[scan->obst_npoints] = scan->x_mm[j]; scan->obst_x_mm[scan->obst_npoints] = (float)scan->x_mm[j];
scan->obst_y_mm[scan->obst_npoints] = scan->y_mm[j]; scan->obst_y_mm[scan->obst_npoints] = (float)scan->y_mm[j];
scan->obst_npoints++; scan->obst_npoints++;
} }
} }
@@ -470,78 +474,78 @@ scan_update(
} }
void void
laser_string( laser_string(
laser_t laser, laser_t laser,
char * str) char * str)
{ {
sprintf(str, sprintf(str,
"scan_size=%d | scan_rate=%3.3f hz | detection_angle=%3.3f deg | " "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", "distance_no_detection=%7.4f mm | detection_margin=%d | offset=%4.4f m",
laser.scan_size, laser.scan_size,
laser.scan_rate_hz, laser.scan_rate_hz,
laser.detection_angle_degrees, laser.detection_angle_degrees,
laser.distance_no_detection_mm, laser.distance_no_detection_mm,
laser.detection_margin, laser.detection_margin,
laser.offset_mm laser.offset_mm
); );
} }
position_t position_t
rmhc_position_search( rmhc_position_search(
position_t start_pos, position_t start_pos,
map_t * map, map_t * map,
scan_t * scan, scan_t * scan,
laser_t laser, laser_t laser,
double sigma_xy_mm, double sigma_xy_mm,
double sigma_theta_degrees, double sigma_theta_degrees,
int max_search_iter, int max_search_iter,
void * randomizer) void * randomizer)
{ {
position_t currentpos = start_pos; position_t currentpos = start_pos;
position_t bestpos = start_pos; position_t bestpos = start_pos;
position_t lastbestpos = start_pos; position_t lastbestpos = start_pos;
int current_distance = distance_scan_to_map(map, scan, currentpos); int current_distance = distance_scan_to_map(map, scan, currentpos);
int lowest_distance = current_distance; int lowest_distance = current_distance;
int last_lowest_distance = current_distance; int last_lowest_distance = current_distance;
int counter = 0; int counter = 0;
while (counter < max_search_iter) while (counter < max_search_iter)
{ {
currentpos = lastbestpos; currentpos = lastbestpos;
currentpos.x_mm = random_normal(randomizer, currentpos.x_mm, sigma_xy_mm); currentpos.x_mm = random_normal(randomizer, currentpos.x_mm, sigma_xy_mm);
currentpos.y_mm = random_normal(randomizer, currentpos.y_mm, sigma_xy_mm); currentpos.y_mm = random_normal(randomizer, currentpos.y_mm, sigma_xy_mm);
currentpos.theta_degrees = random_normal(randomizer, currentpos.theta_degrees, sigma_theta_degrees); currentpos.theta_degrees = random_normal(randomizer, currentpos.theta_degrees, sigma_theta_degrees);
current_distance = distance_scan_to_map(map, scan, currentpos); current_distance = distance_scan_to_map(map, scan, currentpos);
/* -1 indicates infinity */ /* -1 indicates infinity */
if ((current_distance > -1) && (current_distance < lowest_distance)) if ((current_distance > -1) && (current_distance < lowest_distance))
{ {
lowest_distance = current_distance; lowest_distance = current_distance;
bestpos = currentpos; bestpos = currentpos;
} }
else else
{ {
counter++; counter++;
} }
if (counter > max_search_iter / 3) if (counter > max_search_iter / 3)
{ {
if (lowest_distance < last_lowest_distance) if (lowest_distance < last_lowest_distance)
{ {
lastbestpos = bestpos; lastbestpos = bestpos;
last_lowest_distance = lowest_distance; last_lowest_distance = lowest_distance;
counter = 0; counter = 0;
sigma_xy_mm *= 0.5; sigma_xy_mm *= 0.5;
sigma_theta_degrees *= 0.5; sigma_theta_degrees *= 0.5;
} }
} }
} }
return bestpos; return bestpos;
} }

3
c/coreslam_internals.h
View File

@@ -17,8 +17,11 @@ 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/>. along with this code. If not, see <http:#www.gnu.org/licenses/>.
*/ */
#ifdef _MSC_VER #ifdef _MSC_VER
typedef __int64 int64_t; /* Define it from MSVC's internal type */ typedef __int64 int64_t; /* Define it from MSVC's internal type */
#define _USE_MATH_DEFINES
#include <math.h>
#else #else
#include <stdint.h> /* Use the C99 official header */ #include <stdint.h> /* Use the C99 official header */
#endif #endif

24
c/random.c
View File

@@ -19,6 +19,7 @@ along with this code. If not, see <http:#www.gnu.org/licenses/>.
*/ */
#include "ziggurat.h" #include "ziggurat.h"
#include "random.h"
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
@@ -32,18 +33,33 @@ typedef struct random_t
} random_t; } random_t;
void * random_init(int seed) size_t random_size(void)
{
return sizeof(random_t);
}
void * random_new(int seed)
{ {
random_t * r = (random_t *)malloc(sizeof(random_t)); random_t * r = (random_t *)malloc(sizeof(random_t));
r->seed = seed; random_init(r, seed);
r4_nor_setup (r->kn, r->fn, r->wn );
return r; return r;
} }
void random_init(void * v, int seed)
{
random_t * r = (random_t *)v;
r->seed = seed;
r4_nor_setup (r->kn, r->fn, r->wn );
}
double random_normal(void * v, double mu, double sigma) double random_normal(void * v, double mu, double sigma)
{ {
random_t * r = (random_t *)v; random_t * r = (random_t *)v;

10
c/random.h
View File

@@ -20,12 +20,20 @@ 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/>. along with this code. If not, see <http:#www.gnu.org/licenses/>.
*/ */
#include "stdlib.h"
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
/* Returns size of random-number generator in bytes */
size_t random_size(void);
/* Creates and initializes a new random-number generator */ /* Creates and initializes a new random-number generator */
void * random_init(int seed); void * random_new(int seed);
/* Initializes a random-number generator */
void random_init(void * r, int seed);
/* Make a copy of the specified random-number generator */ /* Make a copy of the specified random-number generator */
void * random_copy(void * r); void * random_copy(void * r);

2
cpp/algorithms.cpp
View File

@@ -190,7 +190,7 @@ SinglePositionSLAM(laser, map_size_pixels, map_size_meters)
this->max_search_iter = DEFAULT_MAX_SEARCH_ITER; this->max_search_iter = DEFAULT_MAX_SEARCH_ITER;
this->randomizer = random_init(random_seed); this->randomizer = random_new(random_seed);
} }
RMHC_SLAM::~RMHC_SLAM(void) RMHC_SLAM::~RMHC_SLAM(void)

131
matlab/CoreSLAM.m Normal file
View File

@@ -0,0 +1,131 @@
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 a 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)
% 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_coreslam('Map_init', size_pixels, size_meters);
end
function disp(map)
% Displays data about this map
mex_coreslam('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_coreslam('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_coreslam('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_coreslam('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_coreslam('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

19
matlab/Randomizer.m Normal file
View File

@@ -0,0 +1,19 @@
classdef Randomizer
%Randomizer Internal class for random-number generation
% Matlab has its own random-number generators, but this class
% provides consistency with our Python and C++ SLAM implementations.
properties (Access = 'private')
c_randomizer
end
methods
function r = Randomizer(seed)
r.c_randomizer = mex_coreslam('Randomizer_init', seed);
end
end
end

67
matlab/Scan.m Normal file
View File

@@ -0,0 +1,67 @@
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
c_laser
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, scan.c_laser] = mex_coreslam('Scan_init', laser, span);
end
function disp(scan)
% Displays information about this Scan
mex_coreslam('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_coreslam('Scan_update', scan.c_scan, scan.c_laser, 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 [velocities, obj] = computeVelocities(obj, timestamp, leftWheelOdometry, rightWheelOdometry)
% Computes forward and angular velocities 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
velocities = [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_coreslam.c ../c/coreslam.c ../c/coreslam_sisd.c ../c/random.c ../c/ziggurat.c

300
matlab/mex_coreslam.c Normal file
View File

@@ -0,0 +1,300 @@
/*
* mex_coreslam.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;
}
static void _rhs2laser(laser_t * laser, const mxArray * prhs[], int index)
{
laser->scan_size = (int)_get_field(prhs[index], "scan_size");
laser->scan_rate_hz = _get_field(prhs[index], "scan_rate_hz");
laser->detection_angle_degrees = _get_field(prhs[index], "detection_angle_degrees");
laser->distance_no_detection_mm = _get_field(prhs[index], "distance_no_detection_mm");
laser->detection_margin = (int)_get_field(prhs[index], "detection_margin");
laser->offset_mm = _get_field(prhs[index], "offset_mm");
}
/* 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[])
{
laser_t * laser = (laser_t *)mxMalloc(sizeof(laser_t));
scan_t * scan = (scan_t *)mxMalloc(sizeof(scan_t));
int span = (int)mxGetScalar(prhs[2]);
_rhs2laser(laser, prhs, 1);
scan_init(scan, laser->scan_size, span);
_insert_obj_lhs(plhs, scan, 0);
_insert_obj_lhs(plhs, laser, 1);
}
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);
laser_t * laser = (laser_t *)_rhs2ptr(prhs, 2);
int scansize = (int)mxGetNumberOfElements(prhs[3]);
int * lidar_mm = (int *)mxGetPr(prhs[3]);
double hole_width_mm = mxGetScalar(prhs[4]);
double * velocities = mxGetPr(prhs[5]);
scan_update(scan, lidar_mm, *laser, 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);
laser_t laser;
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]);
_rhs2laser(&laser, prhs, 4);
new_pos = rmhc_position_search(
start_pos,
map,
scan,
laser,
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);
}
}

2
python/pybreezyslam.c
View File

@@ -673,7 +673,7 @@ Randomizer_init(Randomizer *self, PyObject *args, PyObject *kwds)
return error_on_raise_argument_exception("Randomizer"); return error_on_raise_argument_exception("Randomizer");
} }
self->randomizer = random_init(seed); self->randomizer = random_new(seed);
return 0; return 0;
} }