add stdev param to laser (#89)

* add stdev param to laser * rename sigma to stddev_range
TheCodez · Jul 3, 2022 · 839b137 · 839b137
1 parent 303d3b2
commit 839b137
Show file tree

Hide file tree

Showing 5 changed files with 14 additions and 12 deletions.
diff --git a/dogm/demo/main.cpp b/dogm/demo/main.cpp
@@ -36,6 +36,7 @@ int main(int argc, const char** argv)
     laser_params.fov = 120.0f;
     laser_params.max_range = 50.0f;
     laser_params.resolution = grid_params.resolution;  // TODO make independent of grid_params.resolution
+    laser_params.stddev_range = 0.5f;
     LaserMeasurementGrid grid_generator(laser_params, grid_params.size, grid_params.resolution);
 
     const int sensor_horizontal_scan_points = 100;

diff --git a/dogm/demo/simulator/include/mapping/kernel/measurement_grid.h b/dogm/demo/simulator/include/mapping/kernel/measurement_grid.h
@@ -12,10 +12,10 @@ struct MeasurementCell;
 }
 
 __global__ void createPolarGridTextureKernel(cudaSurfaceObject_t polar, const float* __restrict__ measurements,
-                                             int width, int height, float resolution);
+                                             int width, int height, float resolution, float stddev_range);
 
 __global__ void fusePolarGridTextureKernel(cudaSurfaceObject_t polar, const float* __restrict__ measurements, int width,
-                                           int height, float resolution);
+                                           int height, float resolution, float stddev_range);
 
 __global__ void cartesianGridToMeasurementGridKernel(dogm::MeasurementCell* __restrict__ meas_grid,
                                                      cudaSurfaceObject_t cart, int grid_size);

diff --git a/dogm/demo/simulator/include/mapping/laser_to_meas_grid.h b/dogm/demo/simulator/include/mapping/laser_to_meas_grid.h
@@ -18,6 +18,7 @@ class LaserMeasurementGrid
         float max_range;
         float resolution;
         float fov;
+        float stddev_range;
     };
 
     LaserMeasurementGrid(const Params& params, float grid_length, float resolution);

diff --git a/dogm/demo/simulator/mapping/kernel/measurement_grid.cu b/dogm/demo/simulator/mapping/kernel/measurement_grid.cu
@@ -35,15 +35,15 @@ __device__ float pFree(int i, float p_min, float p_max, int max_range)
     return p_min + i * (p_max - p_min) / max_range;
 }
 
-__device__ float pOcc(int r, float zk, int index, float resolution)
+__device__ float pOcc(int r, float zk, int index, float resolution, float stddev_range)
 {
     float occ_max = 0.95f;
-    float delta = 0.6f / resolution;
+    auto diff = float(index - r) * resolution;
 
-    return occ_max * exp(-0.5f * (index - r) * (index - r) / (delta * delta));
+    return occ_max * exp(-0.5f * diff * diff / (stddev_range * stddev_range));
 }
 
-__device__ float2 inverse_sensor_model(int i, float resolution, float zk, float r_max)
+__device__ float2 inverse_sensor_model(int i, float resolution, float zk, float r_max, float stddev_range)
 {
     // Masses: mOcc, mFree
 
@@ -52,7 +52,7 @@ __device__ float2 inverse_sensor_model(int i, float resolution, float zk, float
     if (isfinite(zk))
     {
         const int r = static_cast<int>(zk / resolution);
-        const float occ = pOcc(r, zk, i, resolution);
+        const float occ = pOcc(r, zk, i, resolution, stddev_range);
 
         if (i <= r)
         {
@@ -70,7 +70,7 @@ __device__ float2 inverse_sensor_model(int i, float resolution, float zk, float
 }
 
 __global__ void createPolarGridTextureKernel(cudaSurfaceObject_t polar, const float* __restrict__ measurements,
-                                             int width, int height, float resolution)
+                                             int width, int height, float resolution, float stddev_range)
 {
     const int theta = blockIdx.x * blockDim.x + threadIdx.x;
     const int range = blockIdx.y * blockDim.y + threadIdx.y;
@@ -80,7 +80,7 @@ __global__ void createPolarGridTextureKernel(cudaSurfaceObject_t polar, const fl
         const float epsilon = 0.00001f;
         const float zk = measurements[theta];
 
-        float2 masses = inverse_sensor_model(range, resolution, zk, height);
+        float2 masses = inverse_sensor_model(range, resolution, zk, height, stddev_range);
         masses.x = max(epsilon, min(1.0f - epsilon, masses.x));
         masses.y = max(epsilon, min(1.0f - epsilon, masses.y));
 
@@ -89,7 +89,7 @@ __global__ void createPolarGridTextureKernel(cudaSurfaceObject_t polar, const fl
 }
 
 __global__ void fusePolarGridTextureKernel(cudaSurfaceObject_t polar, const float* __restrict__ measurements, int width,
-                                           int height, float resolution)
+                                           int height, float resolution, float stddev_range)
 {
     const int theta = blockIdx.x * blockDim.x + threadIdx.x;
     const int range = blockIdx.y * blockDim.y + threadIdx.y;
@@ -100,7 +100,7 @@ __global__ void fusePolarGridTextureKernel(cudaSurfaceObject_t polar, const floa
         const float zk = measurements[theta];
 
         float2 prior = surf2Dread<float2>(polar, theta * sizeof(float2), range);
-        float2 masses = inverse_sensor_model(range, resolution, zk, height);
+        float2 masses = inverse_sensor_model(range, resolution, zk, height, stddev_range);
         masses.x = max(epsilon, min(1.0f - epsilon, masses.x));
         masses.y = max(epsilon, min(1.0f - epsilon, masses.y));
 

diff --git a/dogm/demo/simulator/mapping/laser_to_meas_grid.cu b/dogm/demo/simulator/mapping/laser_to_meas_grid.cu
@@ -45,7 +45,7 @@ dogm::MeasurementCell* LaserMeasurementGrid::generateGrid(const std::vector<floa
     // create polar texture
     polar_texture.beginCudaAccess(&polar_surface);
     createPolarGridTextureKernel<<<grid_dim, dim_block>>>(polar_surface, d_measurements, polar_width, polar_height,
-                                                          params.resolution);
+                                                          params.resolution, params.stddev_range);
 
     CHECK_ERROR(cudaGetLastError());
     polar_texture.endCudaAccess(polar_surface);