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

Merge pull request #30 from simondlevy/test

Browse Source 
Interpolate scan distances in C, not scipy
This commit is contained in:
Simon D. Levy
2018-07-04 17:30:53 -04:00
committed by GitHub
parent 5c3b00ec9c 860641b7cd
commit 9099bdb049
8 changed files with 160 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

120
c/coreslam.c
View File

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

11
c/coreslam.h
View File

@@ -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);

2
cpp/Scan.cpp
View File

@@ -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);

15
examples/rpslam.py
View File

@@ -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
f = interp1d(angles, distances, fill_value='extrapolate')
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)
# 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()
@@ -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

3
java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c
View File

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

3
matlab/mex_breezyslam.c
View File

@@ -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[])

4
python/breezyslam/algorithms.py
View File

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

35
python/pybreezyslam.c
View File

@@ -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 (k=0; k<self->scan.size; ++k)
for (int k=0; k<PyList_Size(py_lidar); ++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[] =