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

Sorting scans by angle in preparation for interpolating

Browse Source
This commit is contained in:
simondlevy
2018-07-04 16:44:04 -04:00
parent a7ecc94a9b
commit 0258187bea
1 changed files with 41 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
c/coreslam.c
View File

@@ -43,20 +43,53 @@
/* For angle/distance interpolation ------------------------------- */ /* For angle/distance interpolation ------------------------------- */
typedef struct angle_distance_pair {
typedef struct angle_index_pair {
float angle; float angle;
int index; int distance;
} angle_index_pair_t; } angle_distance_pair_t;
typedef struct interpolation { typedef struct interpolation {
angle_index_pair_t * angle_index_pairs; angle_distance_pair_t * angle_distance_pairs;
} interpolation_t; } 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; 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);
for (k=0; k<scan_size; ++k)
{
angle_distance_pair_t pair = pairs[k];
printf("%3.1f %05d\n", pair.angle, pair.distance);
}
printf("\n");
}
/* Local helpers--------------------------------------------------- */ /* Local helpers--------------------------------------------------- */
static void * safe_malloc(size_t size) static void * safe_malloc(size_t size)
@@ -421,7 +454,7 @@ void scan_init(
/* for angle/distance interpolation */ /* for angle/distance interpolation */
interpolation_t * interpolation = (interpolation_t *)safe_malloc(sizeof(interpolation_t)); 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)); interpolation->angle_distance_pairs = (angle_distance_pair_t *)safe_malloc(size*sizeof(angle_distance_pair_t));
scan->interpolation = interpolation; scan->interpolation = interpolation;
/* assure size multiple of 4 for SSE */ /* assure size multiple of 4 for SSE */
@@ -442,7 +475,7 @@ void
free(scan->obst_y_mm); free(scan->obst_y_mm);
interpolation_t * interpolation = (interpolation_t *)scan->interpolation; interpolation_t * interpolation = (interpolation_t *)scan->interpolation;
free(interpolation->angle_index_pairs); free(interpolation->angle_distance_pairs);
free(interpolation); free(interpolation);
} }
@@ -466,7 +499,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)
{ {
printf("interpolate\n"); interpolate(scan, lidar_angles_deg, lidar_distances_mm, scan_size);
} }