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

More interpolation progress

Browse Source
This commit is contained in:
simondlevy
2018-07-04 17:10:09 -04:00
parent 0258187bea
commit 6e10d05144
1 changed files with 43 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
c/coreslam.c
View File

@@ -52,8 +52,14 @@ typedef struct angle_distance_pair {
typedef struct interpolation { typedef struct interpolation {
/* for sorting */
angle_distance_pair_t * angle_distance_pairs; angle_distance_pair_t * angle_distance_pairs;
/* for interpolating after sorting */
float * angles;
float * distances;
} interpolation_t; } interpolation_t;
static int angle_compar(const void * v1, const void * v2) 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; 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 // Sort angles, preserving indices
interpolation_t * interpolate = (interpolation_t *)scan->interpolation; interpolation_t * interp = (interpolation_t *)scan->interpolation;
angle_distance_pair_t * pairs = interpolate->angle_distance_pairs; angle_distance_pair_t * pairs = interp->angle_distance_pairs;
int k; int k;
for (k=0; k<scan_size; ++k) for (k=0; k<scan_size; ++k)
@@ -81,10 +104,15 @@ static void interpolate(scan_t * scan, float * lidar_angles_deg, int * lidar_dis
qsort(pairs, scan_size, sizeof(angle_distance_pair_t), angle_compar); qsort(pairs, scan_size, sizeof(angle_distance_pair_t), angle_compar);
/* Interpolate */
for (k=0; k<scan_size; ++k) for (k=0; k<scan_size; ++k)
{ {
angle_distance_pair_t pair = pairs[k]; angle_distance_pair_t pair = pairs[k];
printf("%3.1f %05d\n", pair.angle, pair.distance); interp->angles[k] = pair.angle;
interp->distances[k] = pair.distance;
printf("%3.1f: %4.0f\n", interp->angles[k], interp->distances[k]);
} }
printf("\n"); printf("\n");
@@ -453,9 +481,11 @@ void scan_init(
scan->obst_npoints = 0; scan->obst_npoints = 0;
/* for angle/distance interpolation */ /* for angle/distance interpolation */
interpolation_t * interpolation = (interpolation_t *)safe_malloc(sizeof(interpolation_t)); interpolation_t * interp = (interpolation_t *)safe_malloc(sizeof(interpolation_t));
interpolation->angle_distance_pairs = (angle_distance_pair_t *)safe_malloc(size*sizeof(angle_distance_pair_t)); interp->angles = float_alloc(scan->size);
scan->interpolation = interpolation; 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 */ /* assure size multiple of 4 for SSE */
scan->obst_x_mm = float_alloc(size*span+4); scan->obst_x_mm = float_alloc(size*span+4);
@@ -474,9 +504,11 @@ void
free(scan->obst_x_mm); free(scan->obst_x_mm);
free(scan->obst_y_mm); free(scan->obst_y_mm);
interpolation_t * interpolation = (interpolation_t *)scan->interpolation; interpolation_t * interp = (interpolation_t *)scan->interpolation;
free(interpolation->angle_distance_pairs); free(interp->angles);
free(interpolation); free(interp->distances);
free(interp->angle_distance_pairs);
free(interp);
} }
void scan_string( void scan_string(
@@ -499,7 +531,7 @@ scan_update(
/* interpolate scan distances by angles if indicated */ /* interpolate scan distances by angles if indicated */
if (lidar_angles_deg) 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);
} }