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