Added matlab support.

Modified some c and cpp code to help.

c/coreslam.c

@@ -1,32 +1,32 @@
 /*
-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
  */
 
 
@@ -133,19 +133,16 @@ map_laser_ray(
         int alpha)
 {
     
+    int x2c = x2;
+    int y2c = y2;
+    
     if (out_of_bounds(x1, map_size) || out_of_bounds(y1, map_size))
     {
         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);
@@ -174,6 +171,8 @@ map_laser_ray(
             exit(1);
         }
         
+        else
+        {
             int error = 2 * dyc - dxc;
             int horiz = 2 * dyc;
             int diago = 2 * (dyc - dxc);
@@ -226,6 +225,8 @@ map_laser_ray(
                 }
             }
         }
+    }
+}
 
 
 static void
@@ -274,9 +275,10 @@ map_init(
 {
     int npix = size_pixels * size_pixels;
 
+    int k = 0;
+    
     map->pixels = (pixel_t *)safe_malloc(npix * sizeof(pixel_t));
     
-    int k;
     for (k=0; k<npix; ++k)
     {
         map->pixels[k] = (OBSTACLE + NO_OBSTACLE) / 2;
@@ -329,13 +331,13 @@ map_update(
         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);
         
-        
+        {  
             int x2 = roundup(position.x_mm * map->scale_pixels_per_mm + x2p);
             int y2 = roundup(position.y_mm * map->scale_pixels_per_mm + y2p);
             
@@ -351,6 +353,7 @@ map_update(
             map_laser_ray(map->pixels, map->size_pixels, x1, y1, x2, y2, xp, yp, value, q);
         }
     }
+}
 
 void
         map_get(
@@ -362,7 +365,6 @@ map_get(
     {
         bytes[k] = map->pixels[k] >> 8;
     }
-
 }
 
 
@@ -427,16 +429,17 @@ scan_update(
         double velocities_dxy_mm,
         double velocities_dtheta_degrees)
 {    
-    scan->npoints = 0;
-    scan->obst_npoints = 0;
-    
     /* 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 rotation = 1 + velocities_dtheta_degrees / degrees_per_second;
     
     /* 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)
     {
         int lidar_value_mm = lidar_mm[i];
@@ -444,7 +447,7 @@ scan_update(
         /* No obstacle */
         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 */
@@ -452,16 +455,17 @@ scan_update(
         {
             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; j<scan->npoints; ++j)
             {
                 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++;
                 }
             }

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 <http:#www.gnu.org/licenses/>.
 */
 
+
 #ifdef _MSC_VER
 typedef __int64 int64_t;       /* Define it from MSVC's internal type */
+#define _USE_MATH_DEFINES
+#include <math.h>
 #else
 #include <stdint.h>            /* Use the C99 official header */
 #endif

c/random.c

@@ -19,6 +19,7 @@ along with this code.  If not, see <http:#www.gnu.org/licenses/>.
 */
 
 #include "ziggurat.h"
+#include "random.h"
 
 #include <stdlib.h>
 #include <string.h>
@@ -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;
+    random_init(r, seed);
-        
-    r4_nor_setup (r->kn, r->fn, r->wn );
     
     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;

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

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)

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 <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
+

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 <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
+

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 <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

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 <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
+

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
+

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 <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 

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 <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
+

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 <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
+

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

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

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