From b64defee231cf0290828b8f4eefd9cc6bae07daa Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 14:50:51 -0400 Subject: [PATCH 01/15] Begin C interpolation code for distances,angles --- examples/rpslam.py | 50 +++++++++++++++++++++++-------------------- python/pybreezyslam.c | 11 ++++++---- 2 files changed, 34 insertions(+), 27 deletions(-) diff --git a/examples/rpslam.py b/examples/rpslam.py index 4875dab..3db09ed 100755 --- a/examples/rpslam.py +++ b/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,34 +52,41 @@ if __name__ == '__main__': while True: - # Extrat (quality, angle, distance) triples from current scan - items = [item for item in next(iterator)] + try: - # Extract distances and angles from triples - distances = [item[2] for item in items] - angles = [item[1] for item in items] + # Extract (quality, angle, distance) triples from current scan + items = [item for item in next(iterator)] - # Interpolate to get 360 angles from 0 through 359, and corresponding distances - f = interp1d(angles, distances, fill_value='extrapolate') - distances = list(f(np.arange(360))) # slam.update wants a list + # Extract distances and angles from triples + distances = [item[2] for item in items] + angles = [item[1] for item in items] - # Update SLAM with current Lidar scan, using third element of (quality, angle, distance) triples - slam.update(distances) + print(len(distances), len(angles)) - # Get current robot position - x, y, theta = slam.getpos() + # Update SLAM with current Lidar scan and scan angles + slam.update(distances, scan_angles_degrees=angles) - # Get current map bytes as grayscale - slam.getmap(mapbytes) + except KeyboardInterrupt: - display.displayMap(mapbytes) - - display.setPose(x, y, theta) - - # Break on window close - if not display.refresh(): break + # Get current robot position + #x, y, theta = slam.getpos() + + # 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 + #if not display.refresh(): + # break + # Shut down the lidar connection + from time import sleep lidar.stop() lidar.disconnect() diff --git a/python/pybreezyslam.c b/python/pybreezyslam.c index 1f34a81..6860fe4 100644 --- a/python/pybreezyslam.c +++ b/python/pybreezyslam.c @@ -288,21 +288,25 @@ 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"); } + + printf("Interpolate!\n"); + Py_RETURN_NONE; } // No scan angles provided; lidar-list size must match scan size @@ -312,13 +316,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)) { From e62c308cf6acbe2bc41220700fa1d5cff1105e3a Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 15:01:33 -0400 Subject: [PATCH 02/15] Cast lidar distances as int during extraction from Python --- python/pybreezyslam.c | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/python/pybreezyslam.c b/python/pybreezyslam.c index 6860fe4..95b0424 100644 --- a/python/pybreezyslam.c +++ b/python/pybreezyslam.c @@ -304,9 +304,6 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) return null_on_raise_argument_exception_with_details("Scan", "update", "number of scan angles must equal number of scan distances"); } - - printf("Interpolate!\n"); - Py_RETURN_NONE; } // No scan angles provided; lidar-list size must match scan size @@ -339,10 +336,9 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) } // Extract LIDAR values from argument - int k = 0; - for (k=0; kscan.size; ++k) + for (int k=0; kscan.size; ++k) { - self->lidar_mm[k] = PyFloat_AsDouble(PyList_GetItem(py_lidar, k)); + self->lidar_mm[k] = (int)PyFloat_AsDouble(PyList_GetItem(py_lidar, k)); } // Update the scan @@ -354,7 +350,8 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) dtheta_degrees); Py_RETURN_NONE; -} + +} // Scan_update static PyMethodDef Scan_methods[] = From 22ee7dc8cdfb62bf67c4c29b687dec17720e39d7 Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 15:05:08 -0400 Subject: [PATCH 03/15] Extra correct number of lidar distances (constant or variable) --- python/pybreezyslam.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/pybreezyslam.c b/python/pybreezyslam.c index 95b0424..0632f80 100644 --- a/python/pybreezyslam.c +++ b/python/pybreezyslam.c @@ -336,7 +336,7 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) } // Extract LIDAR values from argument - for (int k=0; kscan.size; ++k) + for (int k=0; klidar_mm[k] = (int)PyFloat_AsDouble(PyList_GetItem(py_lidar, k)); } From c5b86a6940370fde85397413d118533be9eb0ac1 Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 15:10:07 -0400 Subject: [PATCH 04/15] More interpolation support --- c/coreslam.c | 13 +++++++------ c/coreslam.h | 4 ++++ python/pybreezyslam.c | 13 ++++++++----- 3 files changed, 19 insertions(+), 11 deletions(-) diff --git a/c/coreslam.c b/c/coreslam.c index bcb2d7d..dd858f6 100644 --- a/c/coreslam.c +++ b/c/coreslam.c @@ -62,12 +62,6 @@ 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 +258,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, diff --git a/c/coreslam.h b/c/coreslam.h index e3f1b6e..879a8ac 100644 --- a/c/coreslam.h +++ b/c/coreslam.h @@ -86,6 +86,10 @@ int * int_alloc( int size); +float * +float_alloc( + int size); + void map_init( map_t * map, diff --git a/python/pybreezyslam.c b/python/pybreezyslam.c index 0632f80..9ac1878 100644 --- a/python/pybreezyslam.c +++ b/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; } @@ -338,13 +341,13 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) // Extract LIDAR values from argument for (int k=0; klidar_mm[k] = (int)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, + self->lidar_distances_mm, hole_width_mm, dxy_mm, dtheta_degrees); From b4567062ad1642dc0111791536fce13ecd0922ac Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 15:16:50 -0400 Subject: [PATCH 05/15] Begin support for interpolation in c/coreslam:scan_update() --- c/coreslam.c | 5 +++-- c/coreslam.h | 3 ++- cpp/Scan.cpp | 1 + .../cs/levy/breezyslam/components/jnibreezyslam_components.c | 3 ++- matlab/mex_breezyslam.c | 3 ++- python/pybreezyslam.c | 1 + 6 files changed, 11 insertions(+), 5 deletions(-) diff --git a/c/coreslam.c b/c/coreslam.c index dd858f6..09ee999 100644 --- a/c/coreslam.c +++ b/c/coreslam.c @@ -432,7 +432,8 @@ void scan_string( void scan_update( scan_t * scan, - int * lidar_mm, + float * lidar_angles_deg, + int * lidar_distances_mm, double hole_width_mm, double velocities_dxy_mm, double velocities_dtheta_degrees) @@ -450,7 +451,7 @@ scan_update( for (i=scan->detection_margin+1; isize-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) diff --git a/c/coreslam.h b/c/coreslam.h index 879a8ac..c6c6d93 100644 --- a/c/coreslam.h +++ b/c/coreslam.h @@ -133,7 +133,8 @@ void scan_string( void scan_update( scan_t * scan, - int * lidar_mm, + float * lidar_angles_deg, + int * lidar_distances_mm, double hole_width_mm, double velocities_dxy_mm, double velocities_dtheta_degrees); diff --git a/cpp/Scan.cpp b/cpp/Scan.cpp index e59e137..6068d8f 100644 --- a/cpp/Scan.cpp +++ b/cpp/Scan.cpp @@ -80,6 +80,7 @@ Scan::update( { scan_update( this->scan, + NULL, // no support for angles/interpolation yet scanvals_mm, hole_width_millimeters, poseChange.dxy_mm, diff --git a/java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c b/java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c index 6e4d8fa..fa8edfc 100644 --- a/java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c +++ b/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, hole_width_mm, velocities_dxy_mm, velocities_dtheta_degrees); (*env)->ReleaseIntArrayElements(env, lidar_mm, lidar_mm_c, 0); } diff --git a/matlab/mex_breezyslam.c b/matlab/mex_breezyslam.c index b74cbf3..cd45295 100644 --- a/matlab/mex_breezyslam.c +++ b/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, hole_width_mm, velocities[0], velocities[1]); } static void _randomizer_init(mxArray *plhs[], const mxArray * prhs[]) diff --git a/python/pybreezyslam.c b/python/pybreezyslam.c index 9ac1878..4aa5420 100644 --- a/python/pybreezyslam.c +++ b/python/pybreezyslam.c @@ -347,6 +347,7 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) // Update the scan scan_update( &self->scan, + NULL, self->lidar_distances_mm, hole_width_mm, dxy_mm, From 0fe7b2243dc483399cc12d248a1fd2c9192b2528 Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 15:26:45 -0400 Subject: [PATCH 06/15] Support variable-size input to scan_update() --- c/coreslam.c | 9 +++++---- c/coreslam.h | 3 ++- cpp/Scan.cpp | 1 + .../breezyslam/components/jnibreezyslam_components.c | 2 +- matlab/mex_breezyslam.c | 2 +- python/pybreezyslam.c | 1 + 6 files changed, 11 insertions(+), 7 deletions(-) diff --git a/c/coreslam.c b/c/coreslam.c index 09ee999..b6dd380 100644 --- a/c/coreslam.c +++ b/c/coreslam.c @@ -433,10 +433,11 @@ void scan_update( scan_t * scan, float * lidar_angles_deg, - int * lidar_distances_mm, - double hole_width_mm, - double velocities_dxy_mm, - double velocities_dtheta_degrees) + int * lidar_distances_mm, + int scan_size, + double hole_width_mm, + double velocities_dxy_mm, + double velocities_dtheta_degrees) { /* Take velocity into account */ int degrees_per_second = (int)(scan->rate_hz * 360); diff --git a/c/coreslam.h b/c/coreslam.h index c6c6d93..75fe163 100644 --- a/c/coreslam.h +++ b/c/coreslam.h @@ -134,7 +134,8 @@ void scan_update( scan_t * scan, float * lidar_angles_deg, - int * lidar_distances_mm, + int * lidar_distances_mm, + int scan_size, double hole_width_mm, double velocities_dxy_mm, double velocities_dtheta_degrees); diff --git a/cpp/Scan.cpp b/cpp/Scan.cpp index 6068d8f..415436d 100644 --- a/cpp/Scan.cpp +++ b/cpp/Scan.cpp @@ -82,6 +82,7 @@ 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); diff --git a/java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c b/java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c index fa8edfc..7e5f6d9 100644 --- a/java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c +++ b/java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c @@ -127,7 +127,7 @@ JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Scan_update (J jint * lidar_mm_c = (*env)->GetIntArrayElements(env, lidar_mm, 0); // no support for angles/interpolation yet - scan_update(scan, NULL, lidar_mm_c, hole_width_mm, velocities_dxy_mm, velocities_dtheta_degrees); + 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); } diff --git a/matlab/mex_breezyslam.c b/matlab/mex_breezyslam.c index cd45295..8ae2147 100644 --- a/matlab/mex_breezyslam.c +++ b/matlab/mex_breezyslam.c @@ -192,7 +192,7 @@ static void _scan_update(const mxArray * prhs[]) double * velocities = mxGetPr(prhs[4]); /* no support for angles/interpolation yet */ - scan_update(scan, NULL, lidar_mm, hole_width_mm, velocities[0], velocities[1]); + scan_update(scan, NULL, lidar_mm, scan->size, hole_width_mm, velocities[0], velocities[1]); } static void _randomizer_init(mxArray *plhs[], const mxArray * prhs[]) diff --git a/python/pybreezyslam.c b/python/pybreezyslam.c index 4aa5420..772a216 100644 --- a/python/pybreezyslam.c +++ b/python/pybreezyslam.c @@ -349,6 +349,7 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) &self->scan, NULL, self->lidar_distances_mm, + PyList_Size(py_lidar), hole_width_mm, dxy_mm, dtheta_degrees); From 6cda4901dd335dd5a198f739f55915542b4e3e95 Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 15:36:27 -0400 Subject: [PATCH 07/15] Extract scan angles from Python arg --- python/pybreezyslam.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/python/pybreezyslam.c b/python/pybreezyslam.c index 772a216..9367030 100644 --- a/python/pybreezyslam.c +++ b/python/pybreezyslam.c @@ -307,6 +307,12 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) 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; klidar_angles_deg[k] = (float)PyFloat_AsDouble(PyList_GetItem(py_scan_angles_degrees, k)); + } } // No scan angles provided; lidar-list size must match scan size From 7298ad9b8ab6fb93647d81ba5ebd6f2c49d3c77f Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 15:40:59 -0400 Subject: [PATCH 08/15] Passing angles, size to scan_update() --- c/coreslam.c | 2 ++ python/pybreezyslam.c | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/c/coreslam.c b/c/coreslam.c index b6dd380..35ae1fb 100644 --- a/c/coreslam.c +++ b/c/coreslam.c @@ -439,6 +439,8 @@ scan_update( double velocities_dxy_mm, double velocities_dtheta_degrees) { + printf("%p %d\n", lidar_angles_deg, scan_size); + /* Take velocity into account */ int degrees_per_second = (int)(scan->rate_hz * 360); double horz_mm = velocities_dxy_mm / degrees_per_second; diff --git a/python/pybreezyslam.c b/python/pybreezyslam.c index 9367030..ca66f9f 100644 --- a/python/pybreezyslam.c +++ b/python/pybreezyslam.c @@ -353,7 +353,7 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) // Update the scan scan_update( &self->scan, - NULL, + (py_scan_angles_degrees != Py_None) ? self->lidar_angles_deg :NULL, self->lidar_distances_mm, PyList_Size(py_lidar), hole_width_mm, From a7ecc94a9bb695c94c503d418f01540f3d144b96 Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 15:59:56 -0400 Subject: [PATCH 09/15] More interpolation progress --- c/coreslam.c | 33 +++++++++++++++++++++++++++++++-- c/coreslam.h | 3 +++ 2 files changed, 34 insertions(+), 2 deletions(-) diff --git a/c/coreslam.c b/c/coreslam.c index 35ae1fb..a490965 100644 --- a/c/coreslam.c +++ b/c/coreslam.c @@ -41,6 +41,22 @@ #include "random.h" +/* For angle/distance interpolation ------------------------------- */ + + +typedef struct angle_index_pair { + + float angle; + int index; + +} angle_index_pair_t; + +typedef struct interpolation { + + angle_index_pair_t * angle_index_pairs; + +} interpolation_t; + /* Local helpers--------------------------------------------------- */ static void * safe_malloc(size_t size) @@ -56,7 +72,6 @@ static void * safe_malloc(size_t size) return v; } - static double * double_alloc(int size) { return (double *)safe_malloc(size * sizeof(double)); @@ -403,6 +418,11 @@ void scan_init( scan->npoints = 0; scan->obst_npoints = 0; + + /* for angle/distance interpolation */ + interpolation_t * interpolation = (interpolation_t *)safe_malloc(sizeof(interpolation_t)); + interpolation->angle_index_pairs = (angle_index_pair_t *)safe_malloc(size*sizeof(angle_index_pair_t)); + scan->interpolation = interpolation; /* assure size multiple of 4 for SSE */ scan->obst_x_mm = float_alloc(size*span+4); @@ -420,6 +440,10 @@ void free(scan->obst_x_mm); free(scan->obst_y_mm); + + interpolation_t * interpolation = (interpolation_t *)scan->interpolation; + free(interpolation->angle_index_pairs); + free(interpolation); } void scan_string( @@ -439,7 +463,12 @@ scan_update( double velocities_dxy_mm, double velocities_dtheta_degrees) { - printf("%p %d\n", lidar_angles_deg, scan_size); + /* interpolate scan distances by angles if indicated */ + if (lidar_angles_deg) + { + printf("interpolate\n"); + } + /* Take velocity into account */ int degrees_per_second = (int)(scan->rate_hz * 360); diff --git a/c/coreslam.h b/c/coreslam.h index 75fe163..6f746c1 100644 --- a/c/coreslam.h +++ b/c/coreslam.h @@ -67,6 +67,9 @@ typedef struct scan_t double distance_no_detection_mm; /* default value when the laser returns 0 */ 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; From 0258187beabc0eb8ab6af84f28955787376161e6 Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 16:44:04 -0400 Subject: [PATCH 10/15] Sorting scans by angle in preparation for interpolating --- c/coreslam.c | 49 +++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 41 insertions(+), 8 deletions(-) diff --git a/c/coreslam.c b/c/coreslam.c index a490965..14c6137 100644 --- a/c/coreslam.c +++ b/c/coreslam.c @@ -43,20 +43,53 @@ /* For angle/distance interpolation ------------------------------- */ - -typedef struct angle_index_pair { +typedef struct angle_distance_pair { float angle; - int index; + int distance; -} angle_index_pair_t; +} angle_distance_pair_t; typedef struct interpolation { - angle_index_pair_t * angle_index_pairs; + angle_distance_pair_t * angle_distance_pairs; } 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; +} + +static void interpolate(scan_t * scan, float * lidar_angles_deg, int * lidar_distances_mm, int scan_size) +{ + // Sort angles, preserving indices + + interpolation_t * interpolate = (interpolation_t *)scan->interpolation; + angle_distance_pair_t * pairs = interpolate->angle_distance_pairs; + + int k; + for (k=0; kangle = lidar_angles_deg[k]; + pair->distance = lidar_distances_mm[k]; + } + + qsort(pairs, scan_size, sizeof(angle_distance_pair_t), angle_compar); + + for (k=0; kangle_index_pairs = (angle_index_pair_t *)safe_malloc(size*sizeof(angle_index_pair_t)); + interpolation->angle_distance_pairs = (angle_distance_pair_t *)safe_malloc(size*sizeof(angle_distance_pair_t)); scan->interpolation = interpolation; /* assure size multiple of 4 for SSE */ @@ -442,7 +475,7 @@ void free(scan->obst_y_mm); interpolation_t * interpolation = (interpolation_t *)scan->interpolation; - free(interpolation->angle_index_pairs); + free(interpolation->angle_distance_pairs); free(interpolation); } @@ -466,7 +499,7 @@ scan_update( /* interpolate scan distances by angles if indicated */ if (lidar_angles_deg) { - printf("interpolate\n"); + interpolate(scan, lidar_angles_deg, lidar_distances_mm, scan_size); } From 6e10d051441b375b523e16875dd80fb3bb14987f Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 17:10:09 -0400 Subject: [PATCH 11/15] More interpolation progress --- c/coreslam.c | 54 +++++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 43 insertions(+), 11 deletions(-) diff --git a/c/coreslam.c b/c/coreslam.c index 14c6137..7ab9738 100644 --- a/c/coreslam.c +++ b/c/coreslam.c @@ -52,8 +52,14 @@ typedef struct angle_distance_pair { 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) @@ -64,12 +70,29 @@ static int angle_compar(const void * v1, const void * v2) return pair1->angle < pair2->angle ? -1 : 1; } -static void interpolate(scan_t * scan, float * lidar_angles_deg, int * lidar_distances_mm, int scan_size) +// 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 indices - interpolation_t * interpolate = (interpolation_t *)scan->interpolation; - angle_distance_pair_t * pairs = interpolate->angle_distance_pairs; + interpolation_t * interp = (interpolation_t *)scan->interpolation; + angle_distance_pair_t * pairs = interp->angle_distance_pairs; int k; for (k=0; kangles[k] = pair.angle; + interp->distances[k] = pair.distance; + + printf("%3.1f: %4.0f\n", interp->angles[k], interp->distances[k]); } printf("\n"); @@ -453,9 +481,11 @@ void scan_init( scan->obst_npoints = 0; /* for angle/distance interpolation */ - interpolation_t * interpolation = (interpolation_t *)safe_malloc(sizeof(interpolation_t)); - interpolation->angle_distance_pairs = (angle_distance_pair_t *)safe_malloc(size*sizeof(angle_distance_pair_t)); - scan->interpolation = 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); @@ -474,9 +504,11 @@ void free(scan->obst_x_mm); free(scan->obst_y_mm); - interpolation_t * interpolation = (interpolation_t *)scan->interpolation; - free(interpolation->angle_distance_pairs); - free(interpolation); + interpolation_t * interp = (interpolation_t *)scan->interpolation; + free(interp->angles); + free(interp->distances); + free(interp->angle_distance_pairs); + free(interp); } void scan_string( @@ -499,7 +531,7 @@ scan_update( /* interpolate scan distances by angles if indicated */ if (lidar_angles_deg) { - interpolate(scan, lidar_angles_deg, lidar_distances_mm, scan_size); + interpolate_scan(scan, lidar_angles_deg, lidar_distances_mm, scan_size); } From fc368c4cb358b963636672bbf6091912b60f8969 Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 17:15:23 -0400 Subject: [PATCH 12/15] Interpolating --- c/coreslam.c | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/c/coreslam.c b/c/coreslam.c index 7ab9738..5727745 100644 --- a/c/coreslam.c +++ b/c/coreslam.c @@ -89,12 +89,13 @@ float interpolate(float xData[], float yData[], int size, float x) static void interpolate_scan(scan_t * scan, float * lidar_angles_deg, int * lidar_distances_mm, int scan_size) { - // Sort angles, preserving indices + // 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; + int k = 0; + for (k=0; kangles[k] = pair.angle; interp->distances[k] = pair.distance; + } - printf("%3.1f: %4.0f\n", interp->angles[k], interp->distances[k]); + /* Interpolate */ + + for (k=0; ksize; ++k) + { + lidar_distances_mm[k] = (int)interpolate(interp->angles, interp->distances, scan_size, (float)k); + printf("%3d: %d\n", k, lidar_distances_mm[k]); } printf("\n"); From b2e40984911b558783327e6381624c8601dfc4f3 Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 17:19:11 -0400 Subject: [PATCH 13/15] RPSPLAM example working without scipy --- c/coreslam.c | 4 ---- examples/rpslam.py | 36 ++++++++++++++---------------------- 2 files changed, 14 insertions(+), 26 deletions(-) diff --git a/c/coreslam.c b/c/coreslam.c index 5727745..ebfecca 100644 --- a/c/coreslam.c +++ b/c/coreslam.c @@ -119,10 +119,7 @@ static void interpolate_scan(scan_t * scan, float * lidar_angles_deg, int * lida for (k=0; ksize; ++k) { lidar_distances_mm[k] = (int)interpolate(interp->angles, interp->distances, scan_size, (float)k); - printf("%3d: %d\n", k, lidar_distances_mm[k]); } - - printf("\n"); } /* Local helpers--------------------------------------------------- */ @@ -540,7 +537,6 @@ scan_update( { interpolate_scan(scan, lidar_angles_deg, lidar_distances_mm, scan_size); } - /* Take velocity into account */ int degrees_per_second = (int)(scan->rate_hz * 360); diff --git a/examples/rpslam.py b/examples/rpslam.py index 3db09ed..a86999a 100755 --- a/examples/rpslam.py +++ b/examples/rpslam.py @@ -52,41 +52,33 @@ if __name__ == '__main__': while True: - try: + # Extract (quality, angle, distance) triples from current scan + items = [item for item in next(iterator)] - # 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] - # Extract distances and angles from triples - distances = [item[2] for item in items] - angles = [item[1] for item in items] - - print(len(distances), len(angles)) - - # Update SLAM with current Lidar scan and scan angles - slam.update(distances, scan_angles_degrees=angles) - - except KeyboardInterrupt: - - break + # Update SLAM with current Lidar scan and scan angles + slam.update(distances, scan_angles_degrees=angles) # Get current robot position - #x, y, theta = slam.getpos() + x, y, theta = slam.getpos() # Get current map bytes as grayscale - #slam.getmap(mapbytes) + slam.getmap(mapbytes) # Display the map - #display.displayMap(mapbytes) + display.displayMap(mapbytes) # Display the robot's pose in the map - #display.setPose(x, y, theta) + display.setPose(x, y, theta) # Break on window close - #if not display.refresh(): - # break + if not display.refresh(): + break - # Shut down the lidar connection +# Shut down the lidar connection from time import sleep lidar.stop() lidar.disconnect() From de6573c6b1b8ebcd34b60be0fbae5d6bbab5845a Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 17:20:49 -0400 Subject: [PATCH 14/15] Cleanup --- examples/rpslam.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/rpslam.py b/examples/rpslam.py index a86999a..e859e99 100755 --- a/examples/rpslam.py +++ b/examples/rpslam.py @@ -78,7 +78,6 @@ if __name__ == '__main__': if not display.refresh(): break -# Shut down the lidar connection - from time import sleep + # Shut down the lidar connection lidar.stop() lidar.disconnect() From 860641b7cd368674ec7d81cab00a82f7ae14ba5c Mon Sep 17 00:00:00 2001 From: simondlevy Date: Wed, 4 Jul 2018 17:27:29 -0400 Subject: [PATCH 15/15] Cleanup --- python/breezyslam/algorithms.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/breezyslam/algorithms.py b/python/breezyslam/algorithms.py index 14d7d72..07080e9 100644 --- a/python/breezyslam/algorithms.py +++ b/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)