__host__ __device__ ProbabilisticVoxel ProbabilisticVoxel::reduce(const ProbabilisticVoxel voxel, const ProbabilisticVoxel other_voxel) { ProbabilisticVoxel res = voxel; res.updateOccupancy(other_voxel.getOccupancy()); return res; }
__global__ void kernelInsertSensorData(ProbabilisticVoxel* voxelmap, const uint32_t voxelmap_size, const Vector3ui* dimensions, const float voxel_side_length, Sensor* sensor, const Vector3f* sensor_data, const bool cut_real_robot, BitVoxel<length>* robotmap, const uint32_t bit_index, RayCasting rayCaster) { const Vector3ui map_dim = (*dimensions); for (uint32_t i = blockIdx.x * blockDim.x + threadIdx.x; (i < voxelmap_size) && (i < sensor->data_size); i += gridDim.x * blockDim.x) { if (!(isnan(sensor_data[i].x) || isnan(sensor_data[i].y) || isnan(sensor_data[i].z))) { const Vector3ui integer_coordinates = mapToVoxels(voxel_side_length, sensor_data[i]); const Vector3ui sensor_coordinates = mapToVoxels(voxel_side_length, sensor->position); /* both data and sensor coordinates must be within boundaries for raycasting to work */ if ((integer_coordinates.x < map_dim.x) && (integer_coordinates.y < map_dim.y) && (integer_coordinates.z < map_dim.z) && (sensor_coordinates.x < map_dim.x) && (sensor_coordinates.y < map_dim.y) && (sensor_coordinates.z < map_dim.z)) { bool update = false; if (cut_real_robot) { BitVoxel<length>* robot_voxel = getVoxelPtr(robotmap, dimensions, integer_coordinates.x, integer_coordinates.y, integer_coordinates.z); // if (!((robot_voxel->occupancy > 0) && (robot_voxel->voxeltype == eVT_OCCUPIED))) // not occupied by robot // { update = !robot_voxel->bitVector().getBit(bit_index); // not occupied by robot // else // else: sensor sees robot, no need to insert data. // { // printf("cutting robot from sensor data in kernel %u\n", i); // } } else update = true; if (update) { // sensor does not see robot, so insert data into voxelmap // raycasting rayCaster.rayCast(voxelmap, dimensions, sensor, sensor_coordinates, integer_coordinates); // insert measured data itself: ProbabilisticVoxel* voxel = getVoxelPtr(voxelmap, dimensions, integer_coordinates.x, integer_coordinates.y, integer_coordinates.z); voxel->updateOccupancy(cSENSOR_MODEL_OCCUPIED); // voxel->voxeltype = eVT_OCCUPIED; // increaseOccupancy(voxel, cSENSOR_MODEL_OCCUPIED); // todo: replace with "occupied" of sensor model } } } } }