diff --git a/c/coreslam.c b/c/coreslam.c index 74be4f3..c0e5fd8 100644 --- a/c/coreslam.c +++ b/c/coreslam.c @@ -1,33 +1,33 @@ /* -coreslam.c adapted from CoreSLAM.c, CoreSLAM_state.c, and CoreSLAM.h -downloaded from openslam.org on 01 January 2014. Contains implementations -of scan and map methods. - -Copyright (C) 2014 Simon D. Levy - -This code is free software: you can redistribute it and/or modify -it under the terms of the GNU Lesser General Public License as -published by the Free Software Foundation, either version 3 of the -License, or (at your option) any later version. - -This code is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU Lesser General Public License -along with this code. If not, see . - -Change log - -07-FEB-2014 : Simon D. Levy - Initial release -01-MAR-2014 : SDL - Converted millimeters to meters for API -21-JUN-2014 : SDL - Added support for SSE and NEON -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 -23-JUL-2014 : SDL - Simplified laser detection angle min, max to total angle -07-SEP-2014 : SDL - Migrated to github -*/ + * coreslam.c adapted from CoreSLAM.c, CoreSLAM_state.c, and CoreSLAM.h + * downloaded from openslam.org on 01 January 2014. Contains implementations + * of scan and map methods. + * + * Copyright (C) 2014 Simon D. Levy + * + * This code is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This code is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this code. If not, see . + * + * Change log + * + * 07-FEB-2014 : Simon D. Levy - Initial release + * 01-MAR-2014 : SDL - Converted millimeters to meters for API + * 21-JUN-2014 : SDL - Added support for SSE and NEON + * 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 + * 23-JUL-2014 : SDL - Simplified laser detection angle min, max to total angle + * 07-SEP-2014 : SDL - Migrated to github + */ #include @@ -68,8 +68,8 @@ static float * float_alloc(int size) } -static void -swap(int * a, int * b) +static void + swap(int * a, int * b) { int tmp = *a; *a = *b; @@ -83,21 +83,21 @@ static int roundup(double x) } -static int -out_of_bounds(int value, int bound) +static int + out_of_bounds(int value, int bound) { return value < 0 || value >= bound; } -static int -clip(int *xyc, int * yxc, int xy, int yx, int map_size) +static int + clip(int *xyc, int * yxc, int xy, int yx, int map_size) { - if (*xyc < 0) + if (*xyc < 0) { - if (*xyc == xy) + if (*xyc == xy) { return 1; } @@ -107,7 +107,7 @@ clip(int *xyc, int * yxc, int xy, int yx, int map_size) if (*xyc >= map_size) { - if (*xyc == xy) + if (*xyc == xy) { return 1; } @@ -120,130 +120,131 @@ clip(int *xyc, int * yxc, int xy, int yx, int map_size) static void -map_laser_ray( - pixel_t * map_pixels, - int map_size, - int x1, - int y1, - int x2, - int y2, - int xp, - int yp, - int value, - int alpha) -{ - - if (out_of_bounds(x1, map_size) || out_of_bounds(y1, map_size)) - { - return; - } + map_laser_ray( + pixel_t * map_pixels, + int map_size, + int x1, + int y1, + int x2, + int y2, + int xp, + int yp, + int value, + int alpha) +{ - int x2c = x2; + int x2c = x2; int y2c = y2; - if (clip(&x2c, &y2c, x1, y1, map_size) || clip(&y2c, &x2c, y1, x1, map_size)) + if (out_of_bounds(x1, map_size) || out_of_bounds(y1, 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; + if (!(clip(&x2c, &y2c, x1, y1, map_size) || clip(&y2c, &x2c, y1, x1, map_size))) + { + 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 derrorv = 0; - - 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 derrorv = 0; - 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 (dx > dy) { - if (x <= dx - derrorv) + 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); + } + + else + { + 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) { - pixval += incv; - errorv += incerrorv; - if (errorv > derrorv) + if (x > dx - 2 * derrorv) { - pixval += sincv; - errorv -= 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; + } + } } - } - else - { - pixval -= incv; - errorv -= incerrorv; - if (errorv < 0) + + /* Integration into the map */ + *ptr = ((256 - alpha) * (*ptr) + alpha * pixval) >> 8; + + if (error > 0) { - pixval -= sincv; - errorv += derrorv; + ptr += incptry; + error += diago; + } else + { + error += horiz; } } - } - - /* Integration into the map */ - *ptr = ((256 - alpha) * (*ptr) + alpha * pixval) >> 8; - - if (error > 0) - { - ptr += incptry; - error += diago; - } else - { - error += horiz; } - } + } } -static void -scan_update_xy( - scan_t * scan, - int offset, - int distance, - int scanval, - laser_t laser, - double horz_mm, - double rotation) +static void + scan_update_xy( + scan_t * scan, + int offset, + int distance, + int scanval, + laser_t laser, + double horz_mm, + double rotation) { int j; for (j=0; jspan; ++j) - { - double k = (double)(offset*scan->span+j) * laser.detection_angle_degrees / - (laser.scan_size * scan->span - 1); - double angle = radians(-laser.detection_angle_degrees/2 + k * rotation); + { + double k = (double)(offset*scan->span+j) * laser.detection_angle_degrees / + (laser.scan_size * scan->span - 1); + double angle = radians(-laser.detection_angle_degrees/2 + k * rotation); double x = distance * cos(angle) - k * horz_mm; double y = distance * sin(angle); @@ -259,59 +260,60 @@ scan_update_xy( /* Exported functions --------------------------------------------------------*/ -int * -int_alloc( - int size) +int * + int_alloc( + int size) { return (int *)safe_malloc(size * sizeof(int)); } -void -map_init( - map_t * map, - int size_pixels, - double size_meters) -{ +void + map_init( + map_t * map, + int size_pixels, + double size_meters) +{ int npix = size_pixels * size_pixels; + + int k = 0; map->pixels = (pixel_t *)safe_malloc(npix * sizeof(pixel_t)); - int k; for (k=0; kpixels[k] = (OBSTACLE + NO_OBSTACLE) / 2; - } + } map->size_pixels = size_pixels; - map->size_meters = size_meters; + map->size_meters = size_meters; /* precompute scale for efficiency */ - map->scale_pixels_per_mm = size_pixels / (size_meters * 1000); + map->scale_pixels_per_mm = size_pixels / (size_meters * 1000); } void -map_free( - map_t * map) + map_free( + map_t * map) { free(map->pixels); } void map_string( - map_t map, - char * str) + map_t map, + char * str) { - sprintf(str, "size = %d x %d pixels | = %f meters", - map.size_pixels, map.size_pixels, map.size_meters); + sprintf(str, "size = %d x %d pixels | = %f meters", + map.size_pixels, map.size_pixels, map.size_meters); } -void -map_update( - map_t * map, - scan_t * scan, - position_t position, - int map_quality, - double hole_width_mm) -{ +void + map_update( + map_t * map, + scan_t * scan, + position_t position, + int map_quality, + double hole_width_mm) +{ double position_theta_radians = radians(position.theta_degrees); double costheta = cos(position_theta_radians); @@ -321,56 +323,56 @@ map_update( int y1 = roundup(position.y_mm * map->scale_pixels_per_mm); int i = 0; - for (i = 0; i != scan->npoints; i++) - { + for (i = 0; i != scan->npoints; i++) + { double x2p = costheta * scan->x_mm[i] - sintheta * scan->y_mm[i]; double y2p = sintheta * scan->x_mm[i] + costheta * scan->y_mm[i]; int xp = roundup((position.x_mm + x2p) * 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 add = hole_width_mm / 2 / dist; + 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; - value = NO_OBSTACLE; + { + 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; + value = NO_OBSTACLE; + } + + map_laser_ray(map->pixels, map->size_pixels, x1, y1, x2, y2, xp, yp, value, q); } - - map_laser_ray(map->pixels, map->size_pixels, x1, y1, x2, y2, xp, yp, value, q); - } -} + } +} void -map_get( - map_t * map, - char * bytes) -{ + map_get( + map_t * map, + char * bytes) +{ int k; for (k=0; ksize_pixels*map->size_pixels; ++k) { bytes[k] = map->pixels[k] >> 8; } - } void -map_set( - map_t * map, - char * bytes) -{ + map_set( + map_t * map, + char * bytes) +{ int k; for (k=0; ksize_pixels*map->size_pixels; ++k) { @@ -379,14 +381,14 @@ map_set( } } -void -scan_init( - scan_t * scan, - int size, - int span) +void + scan_init( + scan_t * scan, + int size, + int span) { scan->x_mm = double_alloc(size*span); - scan->y_mm = double_alloc(size*span); + scan->y_mm = double_alloc(size*span); scan->value = int_alloc(size*span); scan->span = span; @@ -399,9 +401,9 @@ scan_init( } -void -scan_free( - scan_t * scan) +void + scan_free( + scan_t * scan) { free(scan->x_mm); free(scan->y_mm); @@ -412,31 +414,32 @@ scan_free( } void scan_string( - scan_t scan, - char * str) + scan_t scan, + 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 -scan_update( - scan_t * scan, - int * lidar_mm, - laser_t laser, - double hole_width_mm, - double velocities_dxy_mm, - double velocities_dtheta_degrees) -{ +void + scan_update( + scan_t * scan, + int * lidar_mm, + laser_t laser, + double hole_width_mm, + double velocities_dxy_mm, + double velocities_dtheta_degrees) +{ + /* Take velocity into account */ + int degrees_per_second = (int)(laser.scan_rate_hz * 360); + double horz_mm = velocities_dxy_mm / degrees_per_second; + double rotation = 1 + velocities_dtheta_degrees / degrees_per_second; + + /* Span the laser scans to better cover the space */ + int i = 0; + scan->npoints = 0; scan->obst_npoints = 0; - /* Take velocity into account */ - int degrees_per_second = laser.scan_rate_hz * 360; - double horz_mm = velocities_dxy_mm / degrees_per_second; - double rotation = 1 + velocities_dtheta_degrees / degrees_per_second; - - /* Span the laser scans to better cover the space */ - int i; for (i=laser.detection_margin+1; i hole_width_mm / 2) { int oldstart = scan->npoints; - + + int j = 0; + scan_update_xy(scan, i, lidar_value_mm, OBSTACLE, laser, horz_mm, rotation); /* Store obstacles separately for SSE */ - int j; for (j=oldstart; jnpoints; ++j) { if (scan->value[j] == OBSTACLE) - { - scan->obst_x_mm[scan->obst_npoints] = scan->x_mm[j]; - scan->obst_y_mm[scan->obst_npoints] = scan->y_mm[j]; + { + scan->obst_x_mm[scan->obst_npoints] = (float)scan->x_mm[j]; + scan->obst_y_mm[scan->obst_npoints] = (float)scan->y_mm[j]; scan->obst_npoints++; } } } - } + } } -void -laser_string( - laser_t laser, - char * str) +void + laser_string( + laser_t laser, + char * str) { - sprintf(str, - "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", - laser.scan_size, - laser.scan_rate_hz, - laser.detection_angle_degrees, - laser.distance_no_detection_mm, - laser.detection_margin, - laser.offset_mm - ); -} - -position_t -rmhc_position_search( - position_t start_pos, - map_t * map, - scan_t * scan, - laser_t laser, - double sigma_xy_mm, - double sigma_theta_degrees, - int max_search_iter, - void * randomizer) -{ - position_t currentpos = start_pos; - position_t bestpos = start_pos; - position_t lastbestpos = start_pos; - - int current_distance = distance_scan_to_map(map, scan, currentpos); - - int lowest_distance = current_distance; - int last_lowest_distance = current_distance; - - int counter = 0; - - while (counter < max_search_iter) - { - currentpos = lastbestpos; - - 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.theta_degrees = random_normal(randomizer, currentpos.theta_degrees, sigma_theta_degrees); - - current_distance = distance_scan_to_map(map, scan, currentpos); - - /* -1 indicates infinity */ - if ((current_distance > -1) && (current_distance < lowest_distance)) - { - lowest_distance = current_distance; - bestpos = currentpos; - } - else - { - counter++; - } - - if (counter > max_search_iter / 3) - { - if (lowest_distance < last_lowest_distance) - { - lastbestpos = bestpos; - last_lowest_distance = lowest_distance; - counter = 0; - sigma_xy_mm *= 0.5; - sigma_theta_degrees *= 0.5; - } - } - - } - - return bestpos; + sprintf(str, + "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", + laser.scan_size, + laser.scan_rate_hz, + laser.detection_angle_degrees, + laser.distance_no_detection_mm, + laser.detection_margin, + laser.offset_mm + ); +} + +position_t + rmhc_position_search( + position_t start_pos, + map_t * map, + scan_t * scan, + laser_t laser, + double sigma_xy_mm, + double sigma_theta_degrees, + int max_search_iter, + void * randomizer) +{ + position_t currentpos = start_pos; + position_t bestpos = start_pos; + position_t lastbestpos = start_pos; + + int current_distance = distance_scan_to_map(map, scan, currentpos); + + int lowest_distance = current_distance; + int last_lowest_distance = current_distance; + + int counter = 0; + + while (counter < max_search_iter) + { + currentpos = lastbestpos; + + 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.theta_degrees = random_normal(randomizer, currentpos.theta_degrees, sigma_theta_degrees); + + current_distance = distance_scan_to_map(map, scan, currentpos); + + /* -1 indicates infinity */ + if ((current_distance > -1) && (current_distance < lowest_distance)) + { + lowest_distance = current_distance; + bestpos = currentpos; + } + else + { + counter++; + } + + if (counter > max_search_iter / 3) + { + if (lowest_distance < last_lowest_distance) + { + lastbestpos = bestpos; + last_lowest_distance = lowest_distance; + counter = 0; + sigma_xy_mm *= 0.5; + sigma_theta_degrees *= 0.5; + } + } + + } + + return bestpos; } diff --git a/c/coreslam_internals.h b/c/coreslam_internals.h index 6023020..b0a7aaa 100644 --- a/c/coreslam_internals.h +++ b/c/coreslam_internals.h @@ -17,8 +17,11 @@ You should have received a copy of the GNU Lesser General Public License along with this code. If not, see . */ + #ifdef _MSC_VER typedef __int64 int64_t; /* Define it from MSVC's internal type */ +#define _USE_MATH_DEFINES +#include #else #include /* Use the C99 official header */ #endif diff --git a/c/random.c b/c/random.c index 231c40a..6dadbba 100644 --- a/c/random.c +++ b/c/random.c @@ -19,6 +19,7 @@ along with this code. If not, see . */ #include "ziggurat.h" +#include "random.h" #include #include @@ -32,18 +33,33 @@ typedef struct 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)); - r->seed = seed; - - r4_nor_setup (r->kn, r->fn, r->wn ); + random_init(r, seed); 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) { random_t * r = (random_t *)v; diff --git a/c/random.h b/c/random.h index 3ac2cba..81300ba 100644 --- a/c/random.h +++ b/c/random.h @@ -20,12 +20,20 @@ You should have received a copy of the GNU Lesser General Public License along with this code. If not, see . */ +#include "stdlib.h" + #ifdef __cplusplus extern "C" { #endif + +/* Returns size of random-number generator in bytes */ +size_t random_size(void); /* 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 */ void * random_copy(void * r); diff --git a/cpp/algorithms.cpp b/cpp/algorithms.cpp index 911c67f..f2fae2f 100644 --- a/cpp/algorithms.cpp +++ b/cpp/algorithms.cpp @@ -190,7 +190,7 @@ SinglePositionSLAM(laser, map_size_pixels, map_size_meters) 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) diff --git a/matlab/CoreSLAM.m b/matlab/CoreSLAM.m new file mode 100644 index 0000000..8ed823e --- /dev/null +++ b/matlab/CoreSLAM.m @@ -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 . + + 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 + diff --git a/matlab/Deterministic_SLAM.m b/matlab/Deterministic_SLAM.m new file mode 100644 index 0000000..2b7c8e7 --- /dev/null +++ b/matlab/Deterministic_SLAM.m @@ -0,0 +1,44 @@ +classdef Deterministic_SLAM < SinglePositionSLAM + %Deterministic_SLAM SLAM with no Monte Carlo search + % Implements the getNewPosition() method of SinglePositionSLAM + % by copying the start position. + % + % Copyright (C) 2014 Simon D. Levy + % + % This code is free software: you can redistribute it and/or modify + % it under the terms of the GNU Lesser General Public License as + % published by the Free Software Foundation, either version 3 of the + % License, or (at your option) any later version. + % + % This code is distributed in the hope that it will be useful, + % but WITHOUT ANY WARRANTY without even the implied warranty of + % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + % GNU General Public License for more details. + % + % You should have received a copy of the GNU Lesser General Public License + % along with this code. If not, see . + + methods + + function slam = Deterministic_SLAM(laser, map_size_pixels, map_size_meters) + %Creates a Deterministic_SLAM object suitable for updating with new Lidar and odometry data. + % slam = Deterministic_SLAM(laser, map_size_pixels, map_size_meters) + % laser is a Laser object representing the specifications of your Lidar unit + % map_size_pixels is the size of the square map in pixels + % map_size_meters is the size of the square map in meters + + slam = slam@SinglePositionSLAM(laser, map_size_pixels, map_size_meters); + + end + + function new_pos = getNewPosition(~, start_pos) + % Implements the _getNewPosition() method of SinglePositionSLAM. + % new_pos = getNewPosition(~, start_pos) simply returns start_pos + + new_pos = start_pos; + end + + end + +end + diff --git a/matlab/Map.m b/matlab/Map.m new file mode 100644 index 0000000..b36713b --- /dev/null +++ b/matlab/Map.m @@ -0,0 +1,59 @@ +classdef Map + %A class for maps (occupancy grids) used in SLAM + % + % Copyright (C) 2014 Simon D. Levy + % + % This code is free software: you can redistribute it and/or modify + % it under the terms of the GNU Lesser General Public License as + % published by the Free Software Foundation, either version 3 of the + % License, or (at your option) any later version. + % + % This code is distributed in the hope that it will be useful, + % but WITHOUT ANY WARRANTY without even the implied warranty of + % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + % GNU General Public License for more details. + % + % You should have received a copy of the GNU Lesser General Public License + % along with this code. If not, see . + + properties (Access = {?RMHC_SLAM}) + + c_map + end + + methods (Access = 'public') + + function map = Map(size_pixels, size_meters) + % Map creates an empty square map + % map = Map(size_pixels, size_meters) + map.c_map = mex_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 diff --git a/matlab/RMHC_SLAM.m b/matlab/RMHC_SLAM.m new file mode 100644 index 0000000..d146de9 --- /dev/null +++ b/matlab/RMHC_SLAM.m @@ -0,0 +1,72 @@ +classdef RMHC_SLAM < SinglePositionSLAM + %RMHC_SLAM Random Mutation Hill-Climbing SLAM + % Implements the getNewPosition() method of SinglePositionSLAM + % using Random-Mutation Hill-Climbing search. Uses its own internal + % pseudorandom-number generator for efficiency. + % + % Copyright (C) 2014 Simon D. Levy + % + % This code is free software: you can redistribute it and/or modify + % it under the terms of the GNU Lesser General Public License as + % published by the Free Software Foundation, either version 3 of the + % License, or (at your option) any later version. + % + % This code is distributed in the hope that it will be useful, + % but WITHOUT ANY WARRANTY without even the implied warranty of + % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + % GNU General Public License for more details. + % + % You should have received a copy of the GNU Lesser General Public License + % along with this code. If not, see . + + properties (Access = 'public') + sigma_xy_mm = 100; % std. dev. of X/Y component of search + sigma_theta_degrees = 20; % std. dev. of angular component of search + max_search_iter = 1000; % max. # of search iterations per update + end + + properties (Access = 'private') + c_randomizer + end + + methods + + function slam = RMHC_SLAM(laser, map_size_pixels, map_size_meters, random_seed) + %Creates an RMHC_SLAM object suitable for updating with new Lidar and odometry data. + % slam = RMHC_SLAM(laser, map_size_pixels, map_size_meters, random_seed) + % laser is a Laser object representing the specifications of your Lidar unit + % map_size_pixels is the size of the square map in pixels + % map_size_meters is the size of the square map in meters + % random_seed supports reproducible results; defaults to system time if unspecified + + slam = slam@SinglePositionSLAM(laser, map_size_pixels, map_size_meters); + + if nargin < 3 + random_seed = floor(cputime) & hex2dec('FFFF'); + end + + slam.c_randomizer = mex_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 + diff --git a/matlab/Randomizer.m b/matlab/Randomizer.m new file mode 100644 index 0000000..58ea96b --- /dev/null +++ b/matlab/Randomizer.m @@ -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 + diff --git a/matlab/Scan.m b/matlab/Scan.m new file mode 100644 index 0000000..17fad78 --- /dev/null +++ b/matlab/Scan.m @@ -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 . + + 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 diff --git a/matlab/SinglePositionSLAM.m b/matlab/SinglePositionSLAM.m new file mode 100644 index 0000000..787753c --- /dev/null +++ b/matlab/SinglePositionSLAM.m @@ -0,0 +1,101 @@ +classdef SinglePositionSLAM < CoreSLAM + %SinglePositionSLAM Abstract class for CoreSLAM with single-point cloud + % Implementing classes should provide the method + % + % getNewPosition(self, start_position) + % + % to compute a new position based on searching from a starting position. + % + % Copyright (C) 2014 Simon D. Levy + % + % This code is free software: you can redistribute it and/or modify + % it under the terms of the GNU Lesser General Public License as + % published by the Free Software Foundation, either version 3 of the + % License, or (at your option) any later version. + % + % This code is distributed in the hope that it will be useful, + % but WITHOUT ANY WARRANTY without even the implied warranty of + % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + % GNU General Public License for more details. + % + % You should have received a copy of the GNU Lesser General Public License + % along with this code. If not, see . + + properties (Access = 'private') + position + end + + methods (Abstract) + getNewPosition(slam, start_pos) + end + + methods (Access = 'private') + + function c = costheta(slam) + c = cosd(slam.position.theta_degrees); + end + + function s = sintheta(slam) + s = sind(slam.position.theta_degrees); + end + + end + + methods (Access = 'public') + + function slam = SinglePositionSLAM(laser, map_size_pixels, map_size_meters) + + slam = slam@CoreSLAM(laser, map_size_pixels, map_size_meters); + + % Initialize the position (x, y, theta) + init_coord_mm = 500 * map_size_meters; % center of map + slam.position.x_mm = init_coord_mm; + slam.position.y_mm = init_coord_mm; + slam.position.theta_degrees = 0; + + end + + function [x_mm, y_mm, theta_degrees] = getpos(slam) + % Returns the current position. + % [x_mm, y_mm, theta_degrees] = getpos(slam) + + x_mm = slam.position.x_mm; + y_mm = slam.position.y_mm; + theta_degrees = slam.position.theta_degrees; + end + + end + + methods (Access = 'protected') + + function slam = updateMapAndPointcloud(slam, velocities) + + % Start at current position + start_pos = slam.position; + + % Add effect of velocities + start_pos.x_mm = start_pos.x_mm + velocities(1) * slam.costheta(); + start_pos.y_mm = start_pos.y_mm + velocities(1) * slam.sintheta(); + start_pos.theta_degrees = start_pos.theta_degrees + velocities(2); + + % Add offset from laser + start_pos.x_mm = start_pos.x_mm + slam.laser.offset_mm * slam.costheta(); + start_pos.y_mm = start_pos.y_mm + slam.laser.offset_mm * slam.sintheta(); + + % Get new position from implementing class + new_position = slam.getNewPosition(start_pos); + + % Update the map with this new position + slam.map.update(slam.scan_for_mapbuild, new_position, slam.map_quality, slam.hole_width_mm) + + % Update the current position with this new position, adjusted by laser offset + slam.position = new_position; + slam.position.x_mm = slam.position.x_mm - slam.laser.offset_mm * slam.costheta(); + slam.position.y_mm = slam.position.y_mm - slam.laser.offset_mm * slam.sintheta(); + + end + + end + +end + diff --git a/matlab/WheeledRobot.m b/matlab/WheeledRobot.m new file mode 100644 index 0000000..6dd5604 --- /dev/null +++ b/matlab/WheeledRobot.m @@ -0,0 +1,112 @@ +classdef WheeledRobot + %WheeledRobot An abstract class supporting ododmetry for wheeled robots. + % Your implementing class should provide the method: + % + % extractOdometry(obj, timestamp, leftWheel, rightWheel) --> + % (timestampSeconds, leftWheelDegrees, rightWheelDegrees) + % + % Copyright (C) 2014 Simon D. Levy + % + % This code is free software: you can redistribute it and/or modify + % it under the terms of the GNU Lesser General Public License as + % published by the Free Software Foundation, either version 3 of the + % License, or (at your option) any later version. + % + % This code is distributed in the hope that it will be useful, + % but WITHOUT ANY WARRANTY without even the implied warranty of + % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + % GNU General Public License for more details. + % + % You should have received a copy of the GNU Lesser General Public License + % along with this code. If not, see . + + properties (Access = 'private') + + wheelRadiusMillimeters + halfAxleLengthMillimeters + + timestampSecondsPrev + leftWheelDegreesPrev + rightWheelDegreesPrev + + end + + methods (Access = 'protected') + + function s = str(obj) + % Returns a string representation of this WheeledRobot + s = sprintf('', ... + obj.wheelRadiusMillimeters, obj.halfAxleLengthMillimeters); + end + + function robot = WheeledRobot(wheelRadiusMillimeters, halfAxleLengthMillimeters) + % Constructs a WheeledRobot object + % robot = WheeledRobot(wheelRadiusMillimeters, halfAxleLengthMillimeters) + robot.wheelRadiusMillimeters = wheelRadiusMillimeters; + robot.halfAxleLengthMillimeters = halfAxleLengthMillimeters; + + end + + function r = deg2rad(~, d) + % Converts degrees to radians + r = d * pi / 180; + end + + end + + methods (Abstract) + + extractOdometry(obj, timestamp, leftWheel, rightWheel) + + end + + methods (Access = 'public') + + function [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 + diff --git a/matlab/make.m b/matlab/make.m new file mode 100644 index 0000000..67798f4 --- /dev/null +++ b/matlab/make.m @@ -0,0 +1,19 @@ +% Script for building BreezySLAM C extensions + +% Copyright (C) 2014 Simon D. Levy +% +% This code is free software: you can redistribute it and/or modify +% it under the terms of the GNU Lesser General Public License as +% published by the Free Software Foundation, either version 3 of the +% License, or (at your option) any later version. +% +% This code is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU Lesser General Public License +% along with this code. If not, see . + + +mex mex_coreslam.c ../c/coreslam.c ../c/coreslam_sisd.c ../c/random.c ../c/ziggurat.c diff --git a/matlab/mex_coreslam.c b/matlab/mex_coreslam.c new file mode 100644 index 0000000..b4b9b3e --- /dev/null +++ b/matlab/mex_coreslam.c @@ -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 . + */ + +#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); + } +} + diff --git a/python/pybreezyslam.c b/python/pybreezyslam.c index a039c97..a8367a9 100644 --- a/python/pybreezyslam.c +++ b/python/pybreezyslam.c @@ -673,7 +673,7 @@ Randomizer_init(Randomizer *self, PyObject *args, PyObject *kwds) return error_on_raise_argument_exception("Randomizer"); } - self->randomizer = random_init(seed); + self->randomizer = random_new(seed); return 0; }