__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
}
}
}
}
}
__host__ __device__
bool DefaultCollider::collide(const BitVoxel<length>& v1, const ProbabilisticVoxel& v2) const
{
return v2.getOccupancy() >= m_threshold2 && !v1.bitVector().isZero();
}
bool DefaultCollider::collide(const ProbabilisticVoxel& v1) const
{
return v1.getOccupancy() >= m_threshold1;
}
bool SVCollider::collide(const ProbabilisticVoxel& v1, const ProbabilisticVoxel& v2) const
{
return v1.getOccupancy() >= m_threshold1 && v2.getOccupancy() > m_threshold2;
}
