diff --git a/dogm/include/dogm/dogm_types.h b/dogm/include/dogm/dogm_types.h index 589dfd2..1dbef98 100644 --- a/dogm/include/dogm/dogm_types.h +++ b/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]; diff --git a/dogm/include/dogm/kernel/init_new_particles.h b/dogm/include/dogm/kernel/init_new_particles.h index 1666abb..39727b0 100644 --- a/dogm/include/dogm/kernel/init_new_particles.h +++ b/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 */ diff --git a/dogm/include/dogm/kernel/statistical_moments.h b/dogm/include/dogm/kernel/statistical_moments.h index 8f715a1..39e195e 100644 --- a/dogm/include/dogm/kernel/statistical_moments.h +++ b/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, diff --git a/dogm/include/dogm/kernel/update_persistent_particles.h b/dogm/include/dogm/kernel/update_persistent_particles.h index 9b309fa..7c564d5 100644 --- a/dogm/include/dogm/kernel/update_persistent_particles.h +++ b/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); diff --git a/dogm/src/dogm.cu b/dogm/src/dogm.cu index 311d2ec..b60627d 100644 --- a/dogm/src/dogm.cu +++ b/dogm/src/dogm.cu @@ -262,8 +262,8 @@ void DOGM::particleAssignment() thrust::device_ptr associated_ptr(particle_array.associated); thrust::device_ptr 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<<>>(particle_array, grid_cell_array, weight_array, particle_count); diff --git a/dogm/src/kernel/init.cu b/dogm/src/kernel/init.cu index 02eb196..925ecad 100644 --- a/dogm/src/kernel/init.cu +++ b/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]); diff --git a/dogm/src/kernel/init_new_particles.cu b/dogm/src/kernel/init_new_particles.cu index 506b9bc..bf625cd 100644 --- a/dogm/src/kernel/init_new_particles.cu +++ b/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; } } diff --git a/dogm/src/kernel/mass_update.cu b/dogm/src/kernel/mass_update.cu index dd0cef9..ba8ba28 100644 --- a/dogm/src/kernel/mass_update.cu +++ b/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]; } } diff --git a/dogm/src/kernel/particle_to_grid.cu b/dogm/src/kernel/particle_to_grid.cu index ac339af..d25d5d3 100644 --- a/dogm/src/kernel/particle_to_grid.cu +++ b/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; } } diff --git a/dogm/src/kernel/predict.cu b/dogm/src/kernel/predict.cu index f623e67..06a5257 100644 --- a/dogm/src/kernel/predict.cu +++ b/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(x), 0, grid_size - 1); int pos_y = clamp(static_cast(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)); } diff --git a/dogm/src/kernel/resampling.cu b/dogm/src/kernel/resampling.cu index b1161b5..5734135 100644 --- a/dogm/src/kernel/resampling.cu +++ b/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; } } diff --git a/dogm/src/kernel/statistical_moments.cu b/dogm/src/kernel/statistical_moments.cu index 1c6a551..b6e16bb 100644 --- a/dogm/src/kernel/statistical_moments.cu +++ b/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; diff --git a/dogm/src/kernel/update_persistent_particles.cu b/dogm/src/kernel/update_persistent_particles.cu index 533ddf5..f90f0ba 100644 --- a/dogm/src/kernel/update_persistent_particles.cu +++ b/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) diff --git a/dogm/test/dogm_spec.cpp b/dogm/test/dogm_spec.cpp index a08700c..dbd6d9b 100644 --- a/dogm/test/dogm_spec.cpp +++ b/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); }