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 15:59:56 -04:00
parent 7298ad9b8a
commit a7ecc94a9b
2 changed files with 34 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
c/coreslam.c
View File

@@ -41,6 +41,22 @@
#include "random.h" #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--------------------------------------------------- */ /* Local helpers--------------------------------------------------- */
static void * safe_malloc(size_t size) static void * safe_malloc(size_t size)
@@ -56,7 +72,6 @@ static void * safe_malloc(size_t size)
return v; return v;
} }
static double * double_alloc(int size) static double * double_alloc(int size)
{ {
return (double *)safe_malloc(size * sizeof(double)); return (double *)safe_malloc(size * sizeof(double));
@@ -403,6 +418,11 @@ void scan_init(
scan->npoints = 0; scan->npoints = 0;
scan->obst_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 */ /* 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);
@@ -420,6 +440,10 @@ 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;
free(interpolation->angle_index_pairs);
free(interpolation);
} }
void scan_string( void scan_string(
@@ -439,7 +463,12 @@ scan_update(
double velocities_dxy_mm, double velocities_dxy_mm,
double velocities_dtheta_degrees) 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 */ /* Take velocity into account */
int degrees_per_second = (int)(scan->rate_hz * 360); int degrees_per_second = (int)(scan->rate_hz * 360);

3
c/coreslam.h
View File

@@ -67,6 +67,9 @@ typedef struct scan_t
double distance_no_detection_mm; /* default value when the laser returns 0 */ double distance_no_detection_mm; /* default value when the laser returns 0 */
int detection_margin; /* first scan element to consider */ int detection_margin; /* first scan element to consider */
double offset_mm; /* position of the laser wrt center of rotation */ double offset_mm; /* position of the laser wrt center of rotation */
/* for angle/distance interpolation */
void * interpolation;
/* for SSE */ /* for SSE */
float * obst_x_mm; float * obst_x_mm;