Merge pull request #30 from simondlevy/test

Interpolate scan distances in C, not scipy

c/coreslam.c

@@ -41,6 +41,87 @@
 
 #include "random.h"
 
+/* For angle/distance interpolation ------------------------------- */
+
+typedef struct angle_distance_pair {
+
+    float angle;
+    int  distance;
+
+}  angle_distance_pair_t;
+
+typedef struct interpolation {
+
+    /* for sorting */
+    angle_distance_pair_t * angle_distance_pairs;
+
+    /* for interpolating after sorting */
+    float * angles;
+    float * distances;
+
+
+} interpolation_t;
+
+static int angle_compar(const void * v1, const void * v2)
+{
+    angle_distance_pair_t * pair1 = (angle_distance_pair_t *)v1;
+    angle_distance_pair_t * pair2 = (angle_distance_pair_t *)v2;
+
+    return pair1->angle < pair2->angle ? -1 : 1;
+}
+
+// http://www.cplusplus.com/forum/general/216928/
+float interpolate(float xData[], float yData[], int size, float x)
+{
+    int i = 0;                                                                  // find left end of interval for interpolation
+    if ( x >= xData[size - 2] ) {                                               // special case: beyond right end 
+        i = size - 2;
+    }
+    else {
+        while ( x > xData[i+1] ) i++;
+    }
+    float xL = xData[i], yL = yData[i], xR = xData[i+1], yR = yData[i+1];      // points on either side (unless beyond ends)
+
+    float dydx = ( yR - yL ) / ( xR - xL );                                    // gradient
+
+    return yL + dydx * ( x - xL );                                              // linear interpolation
+}
+
+static void interpolate_scan(scan_t * scan, float * lidar_angles_deg, int * lidar_distances_mm, int scan_size)
+{
+    // Sort angles, preserving distance for each angle
+
+    interpolation_t * interp = (interpolation_t *)scan->interpolation;
+    angle_distance_pair_t * pairs = interp->angle_distance_pairs;
+
+    int k = 0;
+
+    for (k=0; k<scan_size; ++k) 
+    {
+        angle_distance_pair_t * pair = &pairs[k];
+        pair->angle    = lidar_angles_deg[k];
+        pair->distance = lidar_distances_mm[k];
+    }
+
+    qsort(pairs, scan_size, sizeof(angle_distance_pair_t), angle_compar);
+
+    /* Copy sorted angle/distance pairs to arrays for interpolation */
+
+    for (k=0; k<scan_size; ++k) 
+    {
+        angle_distance_pair_t pair = pairs[k];
+        interp->angles[k] = pair.angle;
+        interp->distances[k] = pair.distance;
+    }
+
+    /* Interpolate */
+
+    for (k=0; k<scan->size; ++k) 
+    {
+        lidar_distances_mm[k] = (int)interpolate(interp->angles, interp->distances, scan_size, (float)k);
+    }
+}
+
 /* Local helpers--------------------------------------------------- */
 
 static void * safe_malloc(size_t size)
@@ -56,18 +137,11 @@ static void * safe_malloc(size_t size)
     return v;
 }
 
-
 static double * double_alloc(int size)
 {
     return (double *)safe_malloc(size * sizeof(double));
 }
 
-static float * float_alloc(int size)
-{
-    return (float *)safe_malloc(size * sizeof(float));
-}
-
-
 static void
         swap(int * a, int * b)
 {
@@ -264,6 +338,13 @@ int *
     return (int *)safe_malloc(size * sizeof(int));
 }
 
+float *
+        float_alloc(
+        int size)
+{
+    return (float *)safe_malloc(size * sizeof(float));
+}
+
 void
         map_init(
         map_t * map,
@@ -403,6 +484,13 @@ void scan_init(
     scan->npoints = 0;
     scan->obst_npoints = 0;
 
+    /* for angle/distance interpolation */
+    interpolation_t * interp = (interpolation_t *)safe_malloc(sizeof(interpolation_t));
+    interp->angles = float_alloc(scan->size);
+    interp->distances = float_alloc(scan->size);
+    interp->angle_distance_pairs = (angle_distance_pair_t *)safe_malloc(size*sizeof(angle_distance_pair_t));
+    scan->interpolation = interp;
+    
     /* assure size multiple of 4 for SSE */
     scan->obst_x_mm = float_alloc(size*span+4);
     scan->obst_y_mm = float_alloc(size*span+4);
@@ -419,6 +507,12 @@ void
     
     free(scan->obst_x_mm);
     free(scan->obst_y_mm);
+
+    interpolation_t * interp = (interpolation_t *)scan->interpolation;
+    free(interp->angles);
+    free(interp->distances);
+    free(interp->angle_distance_pairs);
+    free(interp);
 }
 
 void scan_string(
@@ -431,11 +525,19 @@ void scan_string(
 void
 scan_update(
         scan_t * scan,
-        int * lidar_mm,
+        float * lidar_angles_deg,
+        int *   lidar_distances_mm,
+        int     scan_size,
         double  hole_width_mm,
         double  velocities_dxy_mm,
         double  velocities_dtheta_degrees)
 {    
+    /* interpolate scan distances by angles if indicated */
+    if (lidar_angles_deg) 
+    {
+        interpolate_scan(scan, lidar_angles_deg, lidar_distances_mm, scan_size);
+    }
+
     /* Take velocity into account */
     int degrees_per_second = (int)(scan->rate_hz * 360);
     double horz_mm = velocities_dxy_mm / degrees_per_second;
@@ -449,7 +551,7 @@ scan_update(
     
     for (i=scan->detection_margin+1; i<scan->size-scan->detection_margin; ++i)
     {
-        int lidar_value_mm = lidar_mm[i];
+        int lidar_value_mm = lidar_distances_mm[i];
         
         /* No obstacle */
         if (lidar_value_mm == 0)

c/coreslam.h

@@ -68,6 +68,9 @@ typedef struct scan_t
     int detection_margin;               /* first scan element to consider */
     double offset_mm;                   /* position of the laser wrt center of rotation */
 
+    /* for angle/distance interpolation */
+    void * interpolation;
+     
     /* for SSE */
     float * obst_x_mm;
     float * obst_y_mm;
@@ -86,6 +89,10 @@ int *
 int_alloc(
     int size);
 
+float * 
+float_alloc(
+    int size);
+
 void 
 map_init(
     map_t * map, 
@@ -129,7 +136,9 @@ void scan_string(
 void 
 scan_update(
     scan_t * scan, 
-    int * lidar_mm, 
+    float * lidar_angles_deg,
+    int   * lidar_distances_mm, 
+    int     scan_size, 
     double hole_width_mm,
     double velocities_dxy_mm,
     double velocities_dtheta_degrees);

cpp/Scan.cpp

@@ -80,7 +80,9 @@ Scan::update(
 {
     scan_update(
         this->scan,
+        NULL, // no support for angles/interpolation yet
         scanvals_mm,
+        this->scan->size,// no support for angles/interpolation yet
         hole_width_millimeters,
         poseChange.dxy_mm,
         poseChange.dtheta_degrees);

examples/rpslam.py

@@ -30,9 +30,6 @@ from rplidar import RPLidar as Lidar
 
 from pltslamshow import SlamShow
 
-from scipy.interpolate import interp1d
-import numpy as np
-
 if __name__ == '__main__':
 
     # Connect to Lidar unit
@@ -55,19 +52,15 @@ if __name__ == '__main__':
 
     while True:
 
-        # Extrat (quality, angle, distance) triples from current scan
+        # Extract (quality, angle, distance) triples from current scan
         items = [item for item in next(iterator)]
 
         # Extract distances and angles from triples
         distances = [item[2] for item in items]
         angles    = [item[1] for item in items]
 
-        # Interpolate to get 360 angles from 0 through 359, and corresponding distances
+        # Update SLAM with current Lidar scan and scan angles
-        f = interp1d(angles, distances, fill_value='extrapolate')
+        slam.update(distances, scan_angles_degrees=angles)
-        distances = list(f(np.arange(360))) # slam.update wants a list
-
-        # Update SLAM with current Lidar scan, using third element of (quality, angle, distance) triples
-        slam.update(distances)
 
         # Get current robot position
         x, y, theta = slam.getpos()
@@ -75,8 +68,10 @@ if __name__ == '__main__':
         # Get current map bytes as grayscale
         slam.getmap(mapbytes)
 
+        # Display the map
         display.displayMap(mapbytes)
 
+        # Display the robot's pose in the map
         display.setPose(x, y, theta)
 
         # Break on window close

java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c

@@ -126,7 +126,8 @@ JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Scan_update (J
 
     jint * lidar_mm_c = (*env)->GetIntArrayElements(env, lidar_mm, 0);
 
-    scan_update(scan, lidar_mm_c, hole_width_mm, velocities_dxy_mm, velocities_dtheta_degrees);
+    // no support for angles/interpolation yet
+    scan_update(scan, NULL, lidar_mm_c, scan->size, hole_width_mm, velocities_dxy_mm, velocities_dtheta_degrees);
 
     (*env)->ReleaseIntArrayElements(env, lidar_mm, lidar_mm_c, 0);
 }

matlab/mex_breezyslam.c

@@ -191,7 +191,8 @@ static void _scan_update(const mxArray * prhs[])
     
     double * velocities = mxGetPr(prhs[4]);
     
-    scan_update(scan, lidar_mm, hole_width_mm, velocities[0], velocities[1]);
+    /* no support for angles/interpolation yet */
+    scan_update(scan, NULL, lidar_mm, scan->size, hole_width_mm, velocities[0], velocities[1]);
 }
 
 static void _randomizer_init(mxArray *plhs[], const mxArray * prhs[])

python/breezyslam/algorithms.py

@@ -137,9 +137,9 @@ class CoreSLAM(object):
          return self.__str__()
 
         
-    def _scan_update(self, scan, lidar, velocities, scan_angles_degrees):
+    def _scan_update(self, scan, scans_distances_mm, velocities, scan_angles_degrees):
 
-        scan.update(scans_mm=lidar, hole_width_mm=self.hole_width_mm, 
+        scan.update(scans_mm=scans_distances_mm, hole_width_mm=self.hole_width_mm, 
                 velocities=velocities, scan_angles_degrees=scan_angles_degrees)
         
         

python/pybreezyslam.c

@@ -176,7 +176,8 @@ typedef struct
     PyObject_HEAD
     
     scan_t scan;
-    int * lidar_mm;
+    int   * lidar_distances_mm;
+    float * lidar_angles_deg;
     
 } Scan;
 
@@ -186,7 +187,8 @@ Scan_dealloc(Scan* self)
 {        
     scan_free(&self->scan);
     
-    free(self->lidar_mm);
+    free(self->lidar_distances_mm);
+    free(self->lidar_angles_deg);
     
     Py_TYPE(self)->tp_free((PyObject*)self);
 }
@@ -243,7 +245,8 @@ Scan_init(Scan *self, PyObject *args, PyObject *kwds)
             detection_margin,               
             offset_mm);
  
-    self->lidar_mm = int_alloc(self->scan.size);
+    self->lidar_distances_mm = int_alloc(self->scan.size);
+    self->lidar_angles_deg   = float_alloc(self->scan.size);
     
     return 0;
 }
@@ -288,21 +291,28 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds)
             "lidar must be a list");
     }
 
-    // Scan angles provided; run bozo-filter to match against lidar-list size
+    // Scan angles provided
     if (py_scan_angles_degrees != Py_None) 
     {
-        // Bozo filter on SCAN_ANGLES_DEGREES argument
+        // Bozo filter #1: SCAN_ANGLES_DEGREES  must be a list
         if (!PyList_Check(py_scan_angles_degrees))
         {
             return null_on_raise_argument_exception_with_details("Scan", "update", 
                     "scan angles must be a list");
         }
 
+        // Bozo filter #2: must have same number of scan angles as scan distances
         if (PyList_Size(py_lidar) != PyList_Size(py_scan_angles_degrees))
         {
             return null_on_raise_argument_exception_with_details("Scan", "update", 
                     "number of scan angles must equal number of scan distances");
         }
+
+        // Extract scan angle values from argument
+        for (int k=0; k<PyList_Size(py_scan_angles_degrees); ++k)
+        {
+            self->lidar_angles_deg[k] = (float)PyFloat_AsDouble(PyList_GetItem(py_scan_angles_degrees, k));
+        }
     }
 
     // No scan angles provided; lidar-list size must match scan size
@@ -312,13 +322,12 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds)
                 "lidar size mismatch");
     }
 
-
     // Default to no velocities
     double dxy_mm = 0;
     double dtheta_degrees = 0;
 
     // Bozo filter on velocities tuple
-    if (py_velocities)
+    if (py_velocities != Py_None)
     {
         if (!PyTuple_Check(py_velocities))
         {
@@ -336,22 +345,24 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds)
     }
 
     // Extract LIDAR values from argument
-    int k = 0;
+    for (int k=0; k<PyList_Size(py_lidar); ++k)
-    for (k=0; k<self->scan.size; ++k)
     {
-        self->lidar_mm[k] = PyFloat_AsDouble(PyList_GetItem(py_lidar, k));
+        self->lidar_distances_mm[k] = (int)PyFloat_AsDouble(PyList_GetItem(py_lidar, k));
     }
 
     // Update the scan
     scan_update(
             &self->scan, 
-            self->lidar_mm, 
+            (py_scan_angles_degrees != Py_None) ? self->lidar_angles_deg :NULL,
+            self->lidar_distances_mm, 
+            PyList_Size(py_lidar),
             hole_width_mm,
             dxy_mm,
             dtheta_degrees);
 
     Py_RETURN_NONE;
-}
+
+} // Scan_update
 
 
 static PyMethodDef Scan_methods[] =