Convert GridCells to SoA

dogm/demo/main.cpp

@@ -80,7 +80,7 @@ int main(int argc, const char** argv)
 
     for (int step = 0; step < num_simulation_steps; ++step)
     {
-        dogm::MeasurementCell* meas_grid = grid_generator.generateGrid(sim_data[step].measurements);
+        dogm::MeasurementCellsSoA meas_grid = grid_generator.generateGrid(sim_data[step].measurements);
 
         const auto update_grid_caller = [&grid_map](auto&&... args) {
             grid_map.updateGrid(std::forward<decltype(args)>(args)...);

dogm/demo/simulator/include/mapping/kernel/measurement_grid.h

@@ -6,19 +6,14 @@
 
 #include <device_launch_parameters.h>
 
-namespace dogm
-{
-struct MeasurementCell;
-}
-
 __global__ void createPolarGridTextureKernel(cudaSurfaceObject_t polar, const float* __restrict__ measurements,
                                              int width, int height, float resolution);
 
 __global__ void fusePolarGridTextureKernel(cudaSurfaceObject_t polar, const float* __restrict__ measurements, int width,
                                            int height, float resolution);
 
-__global__ void cartesianGridToMeasurementGridKernel(dogm::MeasurementCell* __restrict__ meas_grid,
-                                                     cudaSurfaceObject_t cart, int grid_size);
+__global__ void cartesianGridToMeasurementGridKernel(dogm::MeasurementCellsSoA meas_grid, cudaSurfaceObject_t cart,
+                                                     int grid_size);
 
-__global__ void gridArrayToMeasurementGridKernel(dogm::MeasurementCell* __restrict__ meas_grid,
-                                                 const float2* __restrict__ grid, int grid_size);
+__global__ void gridArrayToMeasurementGridKernel(dogm::MeasurementCellsSoA meas_grid, const float2* __restrict__ grid,
+                                                 int grid_size);

dogm/demo/simulator/include/mapping/laser_to_meas_grid.h

@@ -23,10 +23,10 @@ class LaserMeasurementGrid
     LaserMeasurementGrid(const Params& params, float grid_length, float resolution);
     ~LaserMeasurementGrid();
 
-    dogm::MeasurementCell* generateGrid(const std::vector<float>& measurements);
+    dogm::MeasurementCellsSoA generateGrid(const std::vector<float>& measurements);
 
 private:
-    dogm::MeasurementCell* meas_grid;
+    dogm::MeasurementCellsSoA meas_grid;
     int grid_size;
 
     Params params;

dogm/demo/simulator/mapping/kernel/measurement_grid.cu

@@ -112,8 +112,8 @@ __global__ void fusePolarGridTextureKernel(cudaSurfaceObject_t polar, const floa
     }
 }
 
-__global__ void cartesianGridToMeasurementGridKernel(dogm::MeasurementCell* __restrict__ meas_grid,
-                                                     cudaSurfaceObject_t cart, int grid_size)
+__global__ void cartesianGridToMeasurementGridKernel(dogm::MeasurementCellsSoA meas_grid, cudaSurfaceObject_t cart,
+                                                     int grid_size)
 {
     const int x = blockIdx.x * blockDim.x + threadIdx.x;
     const int y = blockIdx.y * blockDim.y + threadIdx.y;
@@ -123,16 +123,16 @@ __global__ void cartesianGridToMeasurementGridKernel(dogm::MeasurementCell* __re
     {
         float4 color = surf2Dread<float4>(cart, x * sizeof(float4), y);
 
-        meas_grid[index].occ_mass = color.x;
-        meas_grid[index].free_mass = color.y;
+        meas_grid.occ_mass[index] = color.x;
+        meas_grid.free_mass[index] = color.y;
 
-        meas_grid[index].likelihood = 1.0f;
-        meas_grid[index].p_A = 1.0f;
+        meas_grid.likelihood[index] = 1.0f;
+        meas_grid.p_A[index] = 1.0f;
     }
 }
 
-__global__ void gridArrayToMeasurementGridKernel(dogm::MeasurementCell* __restrict__ meas_grid,
-                                                 const float2* __restrict__ grid, int grid_size)
+__global__ void gridArrayToMeasurementGridKernel(dogm::MeasurementCellsSoA meas_grid, const float2* __restrict__ grid,
+                                                 int grid_size)
 {
     const int x = blockIdx.x * blockDim.x + threadIdx.x;
     const int y = blockIdx.y * blockDim.y + threadIdx.y;
@@ -142,10 +142,10 @@ __global__ void gridArrayToMeasurementGridKernel(dogm::MeasurementCell* __restri
     {
         float2 masses = grid[index];
 
-        meas_grid[index].occ_mass = masses.x;
-        meas_grid[index].free_mass = masses.y;
+        meas_grid.occ_mass[index] = masses.x;
+        meas_grid.free_mass[index] = masses.y;
 
-        meas_grid[index].likelihood = 1.0f;
-        meas_grid[index].p_A = 1.0f;
+        meas_grid.likelihood[index] = 1.0f;
+        meas_grid.p_A[index] = 1.0f;
     }
 }

dogm/demo/simulator/mapping/laser_to_meas_grid.cu

@@ -12,23 +12,23 @@ LaserMeasurementGrid::LaserMeasurementGrid(const Params& params, float grid_leng
 {
     int grid_cell_count = grid_size * grid_size;
 
-    CHECK_ERROR(cudaMalloc(&meas_grid, grid_cell_count * sizeof(dogm::MeasurementCell)));
+    meas_grid.init(grid_cell_count, true);
 
     renderer = std::make_unique<Renderer>(grid_size, params.fov, grid_length, params.max_range);
 }
 
 LaserMeasurementGrid::~LaserMeasurementGrid()
 {
-    CHECK_ERROR(cudaFree(meas_grid));
+    meas_grid.free();
 }
 
-dogm::MeasurementCell* LaserMeasurementGrid::generateGrid(const std::vector<float>& measurements)
+dogm::MeasurementCellsSoA LaserMeasurementGrid::generateGrid(const std::vector<float>& measurements)
 {
     const int num_measurements = measurements.size();
 
     float* d_measurements;
-    CHECK_ERROR(cudaMalloc(&d_measurements, num_measurements * sizeof(float)));
-    CHECK_ERROR(
+    CUDA_CALL(cudaMalloc(&d_measurements, num_measurements * sizeof(float)));
+    CUDA_CALL(
         cudaMemcpy(d_measurements, measurements.data(), num_measurements * sizeof(float), cudaMemcpyHostToDevice));
 
     const int polar_width = num_measurements;
@@ -47,7 +47,7 @@ dogm::MeasurementCell* LaserMeasurementGrid::generateGrid(const std::vector<floa
     createPolarGridTextureKernel<<<grid_dim, dim_block>>>(polar_surface, d_measurements, polar_width, polar_height,
                                                           params.resolution);
 
-    CHECK_ERROR(cudaGetLastError());
+    CUDA_CALL(cudaGetLastError());
     polar_texture.endCudaAccess(polar_surface);
 
     // render cartesian image to texture using polar texture
@@ -60,11 +60,11 @@ dogm::MeasurementCell* LaserMeasurementGrid::generateGrid(const std::vector<floa
     // transform RGBA texture to measurement grid
     cartesianGridToMeasurementGridKernel<<<cart_grid_dim, dim_block>>>(meas_grid, cartesian_surface, grid_size);
 
-    CHECK_ERROR(cudaGetLastError());
+    CUDA_CALL(cudaGetLastError());
     framebuffer->endCudaAccess(cartesian_surface);
 
-    CHECK_ERROR(cudaFree(d_measurements));
-    CHECK_ERROR(cudaDeviceSynchronize());
+    CUDA_CALL(cudaFree(d_measurements));
+    CUDA_CALL(cudaDeviceSynchronize());
 
     return meas_grid;
 }

dogm/demo/simulator/mapping/opengl/framebuffer.cpp

@@ -23,7 +23,7 @@ Framebuffer::Framebuffer(int width, int height)
 
     glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, texture, 0);
 
-    CHECK_ERROR(cudaGraphicsGLRegisterImage(&resource, texture, GL_TEXTURE_2D, cudaGraphicsRegisterFlagsReadOnly));
+    CUDA_CALL(cudaGraphicsGLRegisterImage(&resource, texture, GL_TEXTURE_2D, cudaGraphicsRegisterFlagsReadOnly));
 
     glBindFramebuffer(GL_FRAMEBUFFER, 0);
 }
@@ -36,23 +36,23 @@ Framebuffer::~Framebuffer()
 
 void Framebuffer::beginCudaAccess(cudaSurfaceObject_t* surfaceObject)
 {
-    CHECK_ERROR(cudaGraphicsMapResources(1, &resource, nullptr));
+    CUDA_CALL(cudaGraphicsMapResources(1, &resource, nullptr));
 
     cudaArray_t cudaArray;
-    CHECK_ERROR(cudaGraphicsSubResourceGetMappedArray(&cudaArray, resource, 0, 0));
+    CUDA_CALL(cudaGraphicsSubResourceGetMappedArray(&cudaArray, resource, 0, 0));
 
     cudaResourceDesc resourceDesc;
     memset(&resourceDesc, 0, sizeof(cudaResourceDesc));
     resourceDesc.resType = cudaResourceTypeArray;
     resourceDesc.res.array.array = cudaArray;
 
-    CHECK_ERROR(cudaCreateSurfaceObject(surfaceObject, &resourceDesc));
+    CUDA_CALL(cudaCreateSurfaceObject(surfaceObject, &resourceDesc));
 }
 
 void Framebuffer::endCudaAccess(cudaSurfaceObject_t surfaceObject)
 {
-    CHECK_ERROR(cudaGraphicsUnmapResources(1, &resource, nullptr));
-    CHECK_ERROR(cudaDestroySurfaceObject(surfaceObject));
+    CUDA_CALL(cudaGraphicsUnmapResources(1, &resource, nullptr));
+    CUDA_CALL(cudaDestroySurfaceObject(surfaceObject));
 }
 
 void Framebuffer::bind()

dogm/demo/simulator/mapping/opengl/texture.cpp

@@ -31,7 +31,7 @@ Texture::Texture(int width, int height, float anisotropy_level)
     float color[] = {0.0f, 0.0f, 1.0f, 1.0f};
     glTexParameterfv(GL_TEXTURE_2D, GL_TEXTURE_BORDER_COLOR, color);
 
-    CHECK_ERROR(
+    CUDA_CALL(
         cudaGraphicsGLRegisterImage(&resource, texture, GL_TEXTURE_2D, cudaGraphicsRegisterFlagsSurfaceLoadStore));
 
     glBindTexture(GL_TEXTURE_2D, 0);
@@ -44,24 +44,24 @@ Texture::~Texture()
 
 void Texture::beginCudaAccess(cudaSurfaceObject_t* surfaceObject)
 {
-    CHECK_ERROR(cudaGraphicsMapResources(1, &resource, nullptr));
+    CUDA_CALL(cudaGraphicsMapResources(1, &resource, nullptr));
 
     cudaArray_t cudaArray;
-    CHECK_ERROR(cudaGraphicsSubResourceGetMappedArray(&cudaArray, resource, 0, 0));
+    CUDA_CALL(cudaGraphicsSubResourceGetMappedArray(&cudaArray, resource, 0, 0));
 
     cudaResourceDesc resourceDesc;
     memset(&resourceDesc, 0, sizeof(cudaResourceDesc));
     resourceDesc.resType = cudaResourceTypeArray;
     resourceDesc.res.array.array = cudaArray;
 
-    CHECK_ERROR(cudaCreateSurfaceObject(surfaceObject, &resourceDesc));
+    CUDA_CALL(cudaCreateSurfaceObject(surfaceObject, &resourceDesc));
 }
 
 void Texture::endCudaAccess(cudaSurfaceObject_t surfaceObject)
 {
-    CHECK_ERROR(cudaGraphicsUnmapResources(1, &resource, nullptr));
-    CHECK_ERROR(cudaGraphicsUnregisterResource(resource));
-    CHECK_ERROR(cudaDestroySurfaceObject(surfaceObject));
+    CUDA_CALL(cudaGraphicsUnmapResources(1, &resource, nullptr));
+    CUDA_CALL(cudaGraphicsUnregisterResource(resource));
+    CUDA_CALL(cudaDestroySurfaceObject(surfaceObject));
 }
 
 void Texture::generateMipMap()

dogm/demo/utils/image_creation.cpp

@@ -32,29 +32,34 @@ std::vector<Point<dogm::GridCell>> computeCellsWithVelocity(const dogm::DOGM& gr
         {
             int index = y * grid_map.getGridSize() + x;
 
-            const dogm::GridCell& cell = grid_cells[index];
-            float occ = pignistic_transformation(cell.free_mass, cell.occ_mass);
+            float occ = pignistic_transformation(grid_cells.free_mass[index], grid_cells.occ_mass[index]);
             cv::Mat velocity_mean(2, 1, CV_32FC1);
-            velocity_mean.at<float>(0) = cell.mean_x_vel;
-            velocity_mean.at<float>(1) = cell.mean_y_vel;
+            velocity_mean.at<float>(0) = grid_cells.mean_x_vel[index];
+            velocity_mean.at<float>(1) = grid_cells.mean_y_vel[index];
 
             cv::Mat velocity_covar(2, 2, CV_32FC1);
-            velocity_covar.at<float>(0, 0) = cell.var_x_vel;
-            velocity_covar.at<float>(1, 0) = cell.covar_xy_vel;
-            velocity_covar.at<float>(0, 1) = cell.covar_xy_vel;
-            velocity_covar.at<float>(1, 1) = cell.var_y_vel;
+            velocity_covar.at<float>(0, 0) = grid_cells.var_x_vel[index];
+            velocity_covar.at<float>(1, 0) = grid_cells.covar_xy_vel[index];
+            velocity_covar.at<float>(0, 1) = grid_cells.covar_xy_vel[index];
+            velocity_covar.at<float>(1, 1) = grid_cells.var_y_vel[index];
 
             cv::Mat velocity_normalized_by_variance = velocity_mean.t() * velocity_covar.inv() * velocity_mean;
 
             if (occ >= min_occupancy_threshold &&
                 velocity_normalized_by_variance.at<float>(0, 0) >= min_velocity_threshold)
             {
-                Point<dogm::GridCell> point;
+                Point<dogm::GridCell> point{};
 
                 // Storing the point as grid index to be consistent with cell.mean_x_vel and cell.mean_y_vel
                 point.x = static_cast<float>(x);
                 point.y = static_cast<float>(y);
-                point.data = cell;
+                point.data.free_mass = grid_cells.free_mass[index];
+                point.data.occ_mass = grid_cells.occ_mass[index];
+                point.data.mean_x_vel = grid_cells.mean_x_vel[index];
+                point.data.mean_y_vel = grid_cells.mean_y_vel[index];
+                point.data.var_x_vel = grid_cells.var_x_vel[index];
+                point.data.var_y_vel = grid_cells.var_y_vel[index];
+                point.data.covar_xy_vel = grid_cells.covar_xy_vel[index];
                 point.cluster_id = UNCLASSIFIED;
 
                 cells_with_velocity.push_back(point);
@@ -76,8 +81,7 @@ cv::Mat compute_measurement_grid_image(const dogm::DOGM& grid_map)
         {
             int index = y * grid_map.getGridSize() + x;
 
-            const dogm::MeasurementCell& cell = meas_cells[index];
-            float occ = pignistic_transformation(cell.free_mass, cell.occ_mass);
+            float occ = pignistic_transformation(meas_cells.free_mass[index], meas_cells.occ_mass[index]);
             auto temp = static_cast<uchar>(occ * 255.0f);
 
             row_ptr[x] = cv::Vec3b(255 - temp, 255 - temp, 255 - temp);
@@ -97,9 +101,8 @@ cv::Mat compute_raw_measurement_grid_image(const dogm::DOGM& grid_map)
         for (int x = 0; x < grid_map.getGridSize(); x++)
         {
             int index = y * grid_map.getGridSize() + x;
-            const dogm::MeasurementCell& cell = meas_cells[index];
-            auto red = static_cast<int>(cell.occ_mass * 255.0f);
-            auto green = static_cast<int>(cell.free_mass * 255.0f);
+            auto red = static_cast<int>(meas_cells.occ_mass[index] * 255.0f);
+            auto green = static_cast<int>(meas_cells.free_mass[index] * 255.0f);
             int blue = 255 - red - green;
 
             row_ptr[x] = cv::Vec3b(blue, green, red);
@@ -120,8 +123,7 @@ cv::Mat compute_dogm_image(const dogm::DOGM& grid_map, const std::vector<Point<d
         {
             int index = y * grid_map.getGridSize() + x;
 
-            const dogm::GridCell& cell = grid_cells[index];
-            float occ = pignistic_transformation(cell.free_mass, cell.occ_mass);
+            float occ = pignistic_transformation(grid_cells.free_mass[index], grid_cells.occ_mass[index]);
             uchar grayscale_value = 255 - static_cast<uchar>(floor(occ * 255));
 
             row_ptr[x] = cv::Vec3b(grayscale_value, grayscale_value, grayscale_value);

dogm/include/dogm/cuda_utils.h

@@ -10,18 +10,15 @@
 
 #define GPU_LAMBDA [=] __host__ __device__
 
-#define CHECK_ERROR(ans)                                                                                               \
+#ifndef CUDA_CALL
+#define CUDA_CALL(call)                                                                                                \
     {                                                                                                                  \
-        checkError((ans), __FILE__, __LINE__);                                                                         \
+        auto status = static_cast<cudaError_t>(call);                                                                  \
+        if (status != cudaSuccess)                                                                                     \
+            fprintf(stderr, "ERROR: CUDA RT call \"%s\" in line %d of file %s failed with %s (%d).\n", #call,          \
+                    __LINE__, __FILE__, cudaGetErrorString(status), status);                                           \
     }
-
-inline void checkError(cudaError_t code, const char* file, int line)
-{
-    if (code != cudaSuccess)
-    {
-        printf("GPU Kernel Error: %s %s %d\n", cudaGetErrorString(code), file, line);
-    }
-}
+#endif
 
 inline int divUp(int total, int grain)
 {

dogm/include/dogm/dogm.h

@@ -76,22 +76,22 @@ class DOGM
      * @param dt delta time since the last update.
      * @param device whether the measurement grid resides in GPU memory (default: true).
      */
-    void updateGrid(MeasurementCell* measurement_grid, float new_x, float new_y, float new_yaw, float dt,
+    void updateGrid(MeasurementCellsSoA measurement_grid, float new_x, float new_y, float new_yaw, float dt,
                     bool device = true);
 
     /**
      * Returns the grid map in the host memory.
      *
      * @return grid map.
      */
-    std::vector<GridCell> getGridCells() const;
+    GridCellsSoA getGridCells() const;
 
     /**
      * Returns the measurement grid map in the host memory.
      *
      * @return measurement grid map.
      */
-    std::vector<MeasurementCell> getMeasurementCells() const;
+    MeasurementCellsSoA getMeasurementCells() const;
 
     /**
      * Returns the persistent particles of the particle filter.
@@ -139,7 +139,7 @@ class DOGM
     void initialize();
 
     void updatePose(float new_x, float new_y, float new_yaw);
-    void updateMeasurementGrid(MeasurementCell* measurement_grid, bool device);
+    void updateMeasurementGrid(MeasurementCellsSoA measurement_grid, bool device);
 
 public:
     void initializeParticles();
@@ -155,11 +155,11 @@ class DOGM
 public:
     Params params;
 
-    GridCell* grid_cell_array;
+    GridCellsSoA grid_cell_array;
     ParticlesSoA particle_array;
     ParticlesSoA particle_array_next;
     ParticlesSoA birth_particle_array;
-    MeasurementCell* meas_cell_array;
+    MeasurementCellsSoA meas_cell_array;
 
     float* weight_array;
     float* birth_weight_array;