Access Soa using AoS syntax

dogm/include/dogm/dogm_types.h

@@ -41,10 +41,10 @@ struct MeasurementCell
 
 struct Particle
 {
-    int grid_cell_idx;
-    float weight;
-    bool associated;
-    glm::vec4 state;
+    glm::vec4& state;
+    int& grid_cell_idx;
+    float& weight;
+    bool& associated;
 };
 
 struct ParticlesSoA
@@ -117,7 +117,12 @@ struct ParticlesSoA
         return *this;
     }
 
-    __device__ void copy(const ParticlesSoA& other, int index, int other_index)
+    __host__ __device__ Particle operator[](std::size_t index)
+    {
+        return Particle{state[index], grid_cell_idx[index], weight[index], associated[index]};
+    }
+
+    __host__ __device__ void copy(const ParticlesSoA& other, int index, int other_index)
     {
         grid_cell_idx[index] = other.grid_cell_idx[other_index];
         weight[index] = other.weight[other_index];

dogm/include/dogm/kernel/init_new_particles.h

@@ -26,7 +26,7 @@ __global__ void initNewParticlesKernel2(ParticlesSoA birth_particle_array, const
                                         curandState* __restrict__ global_state, float velocity, int grid_size,
                                         int particle_count);
 
-__global__ void copyBirthWeightKernel(const ParticlesSoA birth_particle_array, float* __restrict__ birth_weight_array,
+__global__ void copyBirthWeightKernel(ParticlesSoA birth_particle_array, float* __restrict__ birth_weight_array,
                                       int particle_count);
 
 } /* namespace dogm */

dogm/include/dogm/kernel/statistical_moments.h

@@ -12,7 +12,7 @@ namespace dogm
 struct GridCell;
 struct Particle;
 
-__global__ void statisticalMomentsKernel1(const ParticlesSoA particle_array, const float* __restrict__ weight_array,
+__global__ void statisticalMomentsKernel1(ParticlesSoA particle_array, const float* __restrict__ weight_array,
                                           float* __restrict__ vel_x_array, float* __restrict__ vel_y_array,
                                           float* __restrict__ vel_x_squared_array,
                                           float* __restrict__ vel_y_squared_array, float* __restrict__ vel_xy_array,

dogm/include/dogm/kernel/update_persistent_particles.h

@@ -13,14 +13,14 @@ struct GridCell;
 struct MeasurementCell;
 struct Particle;
 
-__global__ void updatePersistentParticlesKernel1(const ParticlesSoA particle_array,
+__global__ void updatePersistentParticlesKernel1(ParticlesSoA particle_array,
                                                  const MeasurementCell* __restrict__ meas_cell_array,
                                                  float* __restrict__ weight_array, int particle_count);
 
 __global__ void updatePersistentParticlesKernel2(GridCell* __restrict__ grid_cell_array,
                                                  const float* __restrict__ weight_array_accum, int cell_count);
 
-__global__ void updatePersistentParticlesKernel3(const ParticlesSoA particle_array,
+__global__ void updatePersistentParticlesKernel3(ParticlesSoA particle_array,
                                                  const MeasurementCell* __restrict__ meas_cell_array,
                                                  const GridCell* __restrict__ grid_cell_array,
                                                  float* __restrict__ weight_array, int particle_count);

dogm/src/dogm.cu

@@ -262,8 +262,8 @@ void DOGM::particleAssignment()
     thrust::device_ptr<bool> associated_ptr(particle_array.associated);
     thrust::device_ptr<glm::vec4> state_ptr(particle_array.state);
 
-    auto it = thrust::make_zip_iterator(thrust::make_tuple(weight_ptr, associated_ptr, state_ptr));
-    thrust::sort_by_key(grid_index_ptr, grid_index_ptr + particle_count, it);
+    auto iter = thrust::make_zip_iterator(thrust::make_tuple(state_ptr, weight_ptr, associated_ptr));
+    thrust::sort_by_key(grid_index_ptr, grid_index_ptr + particle_count, iter);
 
     particleToGridKernel<<<particles_grid, block_dim>>>(particle_array, grid_cell_array, weight_array, particle_count);
 

dogm/src/kernel/init.cu

@@ -36,8 +36,8 @@ __global__ void initParticlesKernel(ParticlesSoA particle_array, curandState* __
         float vel_x = curand_uniform(&local_state, -velocity, velocity);
         float vel_y = curand_uniform(&local_state, -velocity, velocity);
 
-        particle_array.weight[i] = 1.0f / particle_count;
-        particle_array.state[i] = glm::vec4(x, y, vel_x, vel_y);
+        particle_array[i].weight = 1.0f / particle_count;
+        particle_array[i].state = glm::vec4(x, y, vel_x, vel_y);
 
         // printf("w: %f, x: %f, y: %f, vx: %f, vy: %f\n", particle_array[i].weight, particle_array[i].state[0],
         // particle_array[i].state[1], 	particle_array[i].state[2], particle_array[i].state[3]);

dogm/src/kernel/init_new_particles.cu

@@ -15,16 +15,16 @@
 namespace dogm
 {
 
-__device__ void set_cell_idx_A(const ParticlesSoA& birth_particle_array, int i, int grid_cell_idx)
+__device__ void set_cell_idx_A(ParticlesSoA& birth_particle_array, int i, int grid_cell_idx)
 {
-    birth_particle_array.grid_cell_idx[i] = grid_cell_idx;
-    birth_particle_array.associated[i] = true;
+    birth_particle_array[i].grid_cell_idx = grid_cell_idx;
+    birth_particle_array[i].associated = true;
 }
 
-__device__ void set_cell_idx_UA(const ParticlesSoA& birth_particle_array, int i, int grid_cell_idx)
+__device__ void set_cell_idx_UA(ParticlesSoA& birth_particle_array, int i, int grid_cell_idx)
 {
-    birth_particle_array.grid_cell_idx[i] = grid_cell_idx;
-    birth_particle_array.associated[i] = false;
+    birth_particle_array[i].grid_cell_idx = grid_cell_idx;
+    birth_particle_array[i].associated = false;
 }
 
 __device__ int calc_start_idx(const float* __restrict__ particle_orders_array_accum, int index)
@@ -127,29 +127,29 @@ __global__ void initNewParticlesKernel2(ParticlesSoA birth_particle_array, const
         float vel_x = curand_normal(&local_state, 0.0f, velocity);
         float vel_y = curand_normal(&local_state, 0.0f, velocity);
 
-        bool associated = birth_particle_array.associated[i];
+        bool associated = birth_particle_array[i].associated;
         // TODO: Use correct distribution
         if (associated)
         {
-            birth_particle_array.weight[i] = grid_cell.w_A;
-            birth_particle_array.state[i] = glm::vec4(x, y, vel_x, vel_y);
+            birth_particle_array[i].weight = grid_cell.w_A;
+            birth_particle_array[i].state = glm::vec4(x, y, vel_x, vel_y);
         }
         else
         {
-            birth_particle_array.weight[i] = grid_cell.w_UA;
-            birth_particle_array.state[i] = glm::vec4(x, y, vel_x, vel_y);
+            birth_particle_array[i].weight = grid_cell.w_UA;
+            birth_particle_array[i].state = glm::vec4(x, y, vel_x, vel_y);
         }
     }
 
     global_state[thread_id] = local_state;
 }
 
-__global__ void copyBirthWeightKernel(const ParticlesSoA birth_particle_array, float* __restrict__ birth_weight_array,
+__global__ void copyBirthWeightKernel(ParticlesSoA birth_particle_array, float* __restrict__ birth_weight_array,
                                       int particle_count)
 {
     for (int i = blockIdx.x * blockDim.x + threadIdx.x; i < particle_count; i += blockDim.x * gridDim.x)
     {
-        birth_weight_array[i] = birth_particle_array.weight[i];
+        birth_weight_array[i] = birth_particle_array[i].weight;
     }
 }
 

dogm/src/kernel/mass_update.cu

@@ -47,13 +47,13 @@ __device__ void store_values(float rho_b, float rho_p, float m_free_up, float m_
     grid_cell_array[i].occ_mass = m_occ_up;
 }
 
-__device__ void normalize_weights(const ParticlesSoA& particle_array, float* __restrict__ weight_array, int start_idx,
+__device__ void normalize_weights(ParticlesSoA& particle_array, float* __restrict__ weight_array, int start_idx,
                                   int end_idx, float occ_pred)
 {
     for (int i = start_idx; i < end_idx + 1; i++)
     {
         weight_array[i] = weight_array[i] / occ_pred;
-        particle_array.weight[i] = weight_array[i];
+        particle_array[i].weight = weight_array[i];
     }
 }
 

dogm/src/kernel/particle_to_grid.cu

@@ -14,22 +14,22 @@
 namespace dogm
 {
 
-__device__ bool is_first_particle(const ParticlesSoA& particle_array, int i)
+__device__ bool is_first_particle(ParticlesSoA& particle_array, int i)
 {
-    return i == 0 || particle_array.grid_cell_idx[i] != particle_array.grid_cell_idx[i - 1];
+    return i == 0 || particle_array[i].grid_cell_idx != particle_array[i - 1].grid_cell_idx;
 }
 
-__device__ bool is_last_particle(const ParticlesSoA& particle_array, int particle_count, int i)
+__device__ bool is_last_particle(ParticlesSoA& particle_array, int particle_count, int i)
 {
-    return i == particle_count - 1 || particle_array.grid_cell_idx[i] != particle_array.grid_cell_idx[i + 1];
+    return i == particle_count - 1 || particle_array[i].grid_cell_idx != particle_array[i + 1].grid_cell_idx;
 }
 
-__global__ void particleToGridKernel(const ParticlesSoA particle_array, GridCell* __restrict__ grid_cell_array,
+__global__ void particleToGridKernel(ParticlesSoA particle_array, GridCell* __restrict__ grid_cell_array,
                                      float* __restrict__ weight_array, int particle_count)
 {
     for (int i = blockIdx.x * blockDim.x + threadIdx.x; i < particle_count; i += blockDim.x * gridDim.x)
     {
-        int j = particle_array.grid_cell_idx[i];
+        int j = particle_array[i].grid_cell_idx;
 
         if (is_first_particle(particle_array, i))
         {
@@ -42,7 +42,7 @@ __global__ void particleToGridKernel(const ParticlesSoA particle_array, GridCell
 
         // printf("Cell: %d, Start idx: %d, End idx: %d\n", j, grid_cell_array[j].start_idx,
         // grid_cell_array[j].end_idx);
-        weight_array[i] = particle_array.weight[i];
+        weight_array[i] = particle_array[i].weight;
     }
 }
 

dogm/src/kernel/predict.cu

@@ -30,21 +30,21 @@ __global__ void predictKernel(ParticlesSoA particle_array, curandState* __restri
         float noise_vel_y = curand_normal(&local_state, 0.0f, process_noise_velocity);
         glm::vec4 process_noise(noise_pos_x, noise_pos_y, noise_vel_x, noise_vel_y);
 
-        particle_array.state[i] = transition_matrix * particle_array.state[i] + process_noise;
-        particle_array.weight[i] = p_S * particle_array.weight[i];
+        particle_array[i].state = transition_matrix * particle_array[i].state + process_noise;
+        particle_array[i].weight = p_S * particle_array[i].weight;
 
-        float x = particle_array.state[i][0];
-        float y = particle_array.state[i][1];
+        float x = particle_array[i].state[0];
+        float y = particle_array[i].state[1];
 
         // Particle out of grid so decrease its chance of being resampled
         if ((x > grid_size - 1 || x < 0) || (y > grid_size - 1 || y < 0))
         {
-            particle_array.weight[i] = 0.0f;
+            particle_array[i].weight = 0.0f;
         }
 
         int pos_x = clamp(static_cast<int>(x), 0, grid_size - 1);
         int pos_y = clamp(static_cast<int>(y), 0, grid_size - 1);
-        particle_array.grid_cell_idx[i] = pos_x + grid_size * pos_y;
+        particle_array[i].grid_cell_idx = pos_x + grid_size * pos_y;
 
         // printf("X: %d, Y: %d, Cell index: %d\n", pos_x, pos_y, (pos_x + grid_size * pos_y));
     }

dogm/src/kernel/resampling.cu

@@ -63,7 +63,7 @@ __global__ void resamplingKernel(const ParticlesSoA particle_array, ParticlesSoA
             particle_array_next.copy(birth_particle_array, i, idx - particle_count);
         }
 
-        particle_array_next.weight[i] = new_weight;
+        particle_array_next[i].weight = new_weight;
     }
 }
 

dogm/src/kernel/statistical_moments.cu

@@ -55,7 +55,7 @@ __device__ void store(GridCell* __restrict__ grid_cell_array, int j, float mean_
     grid_cell_array[j].covar_xy_vel = covar_xy_vel;
 }
 
-__global__ void statisticalMomentsKernel1(const ParticlesSoA particle_array, const float* __restrict__ weight_array,
+__global__ void statisticalMomentsKernel1(ParticlesSoA particle_array, const float* __restrict__ weight_array,
                                           float* __restrict__ vel_x_array, float* __restrict__ vel_y_array,
                                           float* __restrict__ vel_x_squared_array,
                                           float* __restrict__ vel_y_squared_array, float* __restrict__ vel_xy_array,
@@ -64,8 +64,8 @@ __global__ void statisticalMomentsKernel1(const ParticlesSoA particle_array, con
     for (int i = blockIdx.x * blockDim.x + threadIdx.x; i < particle_count; i += blockDim.x * gridDim.x)
     {
         float weight = weight_array[i];
-        float vel_x = particle_array.state[i][2];
-        float vel_y = particle_array.state[i][3];
+        float vel_x = particle_array[i].state[2];
+        float vel_y = particle_array[i].state[3];
         vel_x_array[i] = weight * vel_x;
         vel_y_array[i] = weight * vel_y;
         vel_x_squared_array[i] = weight * vel_x * vel_x;

dogm/src/kernel/update_persistent_particles.cu

@@ -15,12 +15,12 @@ namespace dogm
 
 __device__ float calc_norm_assoc(float occ_accum, float rho_p)
 {
-    return occ_accum > 0.0 ? rho_p / occ_accum : 0.0;
+    return occ_accum > 0.0f ? rho_p / occ_accum : 0.0f;
 }
 
 __device__ float calc_norm_unassoc(const GridCell& grid_cell)
 {
-    return grid_cell.occ_mass > 0.0 ? grid_cell.pers_occ_mass / grid_cell.occ_mass : 0.0;
+    return grid_cell.occ_mass > 0.0f ? grid_cell.pers_occ_mass / grid_cell.occ_mass : 0.0f;
 }
 
 __device__ void set_normalization_components(GridCell* __restrict__ grid_cell_array, int i, float mu_A, float mu_UA)
@@ -29,22 +29,22 @@ __device__ void set_normalization_components(GridCell* __restrict__ grid_cell_ar
     grid_cell_array[i].mu_UA = mu_UA;
 }
 
-__device__ float update_unnorm(const ParticlesSoA& particle_array, int i,
-                               const MeasurementCell* __restrict__ meas_cell_array)
+__device__ float update_unnorm(ParticlesSoA& particle_array, int i, const MeasurementCell* __restrict__ meas_cell_array)
 {
-    return meas_cell_array[particle_array.grid_cell_idx[i]].likelihood * particle_array.weight[i];
+    return meas_cell_array[particle_array[i].grid_cell_idx].likelihood * particle_array[i].weight;
 }
 
-__device__ float normalize(const ParticlesSoA& particle, int i, const GridCell* __restrict__ grid_cell_array,
+__device__ float normalize(ParticlesSoA& particle, int i, const GridCell* __restrict__ grid_cell_array,
                            const MeasurementCell* __restrict__ meas_cell_array, float weight)
 {
-    const GridCell& cell = grid_cell_array[particle.grid_cell_idx[i]];
-    const MeasurementCell& meas_cell = meas_cell_array[particle.grid_cell_idx[i]];
+    const int cell_idx = particle[i].grid_cell_idx;
+    const GridCell& cell = grid_cell_array[cell_idx];
+    const MeasurementCell& meas_cell = meas_cell_array[cell_idx];
 
-    return meas_cell.p_A * cell.mu_A * weight + (1.0 - meas_cell.p_A) * cell.mu_UA * particle.weight[i];
+    return meas_cell.p_A * cell.mu_A * weight + (1.0f - meas_cell.p_A) * cell.mu_UA * particle.weight[i];
 }
 
-__global__ void updatePersistentParticlesKernel1(const ParticlesSoA particle_array,
+__global__ void updatePersistentParticlesKernel1(ParticlesSoA particle_array,
                                                  const MeasurementCell* __restrict__ meas_cell_array,
                                                  float* __restrict__ weight_array, int particle_count)
 {
@@ -74,7 +74,7 @@ __global__ void updatePersistentParticlesKernel2(GridCell* __restrict__ grid_cel
     }
 }
 
-__global__ void updatePersistentParticlesKernel3(const ParticlesSoA particle_array,
+__global__ void updatePersistentParticlesKernel3(ParticlesSoA particle_array,
                                                  const MeasurementCell* __restrict__ meas_cell_array,
                                                  const GridCell* __restrict__ grid_cell_array,
                                                  float* __restrict__ weight_array, int particle_count)

dogm/test/dogm_spec.cpp

@@ -33,15 +33,15 @@ TEST(DOGM, Predict)
 
     dogm::ParticlesSoA particles = dogm.getParticles();
 
-    glm::vec4 old_state = particles.state[0];
+    glm::vec4 old_state = particles[0].state;
     glm::vec4 pred_state = old_state + delta_time * glm::vec4(old_state[2], old_state[3], 0, 0);
-    float old_weight = particles.weight[0];
+    float old_weight = particles[0].weight;
 
     dogm.particlePrediction(delta_time);
     cudaDeviceSynchronize();
 
     dogm::ParticlesSoA new_particles = dogm.getParticles();
 
-    EXPECT_EQ(pred_state, new_particles.state[0]);
-    EXPECT_EQ(old_weight * grid_params.persistence_prob, new_particles.weight[0]);
+    EXPECT_EQ(pred_state, new_particles[0].state);
+    EXPECT_EQ(old_weight * grid_params.persistence_prob, new_particles[0].weight);
 }