bool VisVoxelMap::visualize(const bool force_repaint)
{
if (force_repaint)
{
openOrCreateSegment();
uint32_t shared_mem_id;
if (m_shm_memHandle == NULL)
{
// there should only be one segment of number_of_voxelmaps
std::pair<uint32_t*, std::size_t> r = m_segment.find<uint32_t>(
shm_variable_name_number_of_voxelmaps.c_str());
if (r.second == 0)
{ // if it doesn't exists ..
m_segment.construct<uint32_t>(shm_variable_name_number_of_voxelmaps.c_str())(1);
shared_mem_id = 0;
}
else
{ // if it exists increase it by one
shared_mem_id = *r.first;
(*r.first)++;
}
// get shared memory pointer
std::stringstream id;
id << shared_mem_id;
m_shm_memHandle = m_segment.find_or_construct<cudaIpcMemHandle_t>(
std::string(shm_variable_name_voxelmap_handler_dev_pointer + id.str()).c_str())(
cudaIpcMemHandle_t());
m_shm_mapDim = m_segment.find_or_construct<Vector3ui>(
std::string(shm_variable_name_voxelmap_dimension + id.str()).c_str())(Vector3ui(0));
m_shm_VoxelSize = m_segment.find_or_construct<float>(
std::string(shm_variable_name_voxel_side_length + id.str()).c_str())(0.0f);
m_shm_mapName = m_segment.find_or_construct_it<char>(
std::string(shm_variable_name_voxelmap_name + id.str()).c_str())[m_map_name.size()](
m_map_name.data());
m_shm_voxelmap_type = m_segment.find_or_construct<MapType>(
std::string(shm_variable_name_voxelmap_type + id.str()).c_str())(m_voxelmap->getMapType());
m_shm_voxelmap_changed = m_segment.find_or_construct<bool>(
std::string(shm_variable_name_voxelmap_data_changed + id.str()).c_str())(true);
}
// first open or create and the set the values
HANDLE_CUDA_ERROR(cudaIpcGetMemHandle(m_shm_memHandle, m_voxelmap->getVoidDeviceDataPtr()));
*m_shm_mapDim = m_voxelmap->getDimensions();
*m_shm_VoxelSize = m_voxelmap->getVoxelSideLength();
*m_shm_voxelmap_changed = true;
// // wait till data was read by visualizer. Otherwise a
// while(*m_shm_voxelmap_changed)
// usleep(10000); // sleep 10 ms
return true;
}
return false;
}
void VoxelMapProvider::init(Provider_Parameter& parameter)
{
m_mutex.lock();
const string prefix = "VoxelMapProvider::" + string(__FUNCTION__);
const string temp_timer = prefix + "_temp";
PERF_MON_START(prefix);
PERF_MON_START(temp_timer);
Provider::init(parameter);
// get shared memory pointer
m_shm_memHandle = m_segment.find_or_construct<cudaIpcMemHandle_t>(
std::string(shm_variable_name_voxelmap_handler_dev_pointer + m_shared_mem_id).c_str())(
cudaIpcMemHandle_t());
m_shm_mapDim = m_segment.find_or_construct<Vector3ui>(
std::string(shm_variable_name_voxelmap_dimension + m_shared_mem_id).c_str())(Vector3ui(0));
m_shm_VoxelSize = m_segment.find_or_construct<float>(
std::string(shm_variable_name_voxel_side_length + m_shared_mem_id).c_str())(0.0f);
// there should only be one segment of number_of_voxelmaps
std::pair<uint32_t*, std::size_t> r = m_segment.find<uint32_t>(
shm_variable_name_number_of_voxelmaps.c_str());
if (r.second == 0)
{
// if it doesn't exist ..
m_segment.construct<uint32_t>(shm_variable_name_number_of_voxelmaps.c_str())(1);
}
else
{
// if it exit increase it by one
(*r.first)++;
}
Vector3ui map_dim;
std::vector<Vector3f> insert_points;
float voxel_map_res = 1.0f;
if (parameter.points.size() != 0)
{
Vector3f offset;
Vector3ui point_data_bounds = getMapDimensions(parameter.points, offset);
map_dim = point_data_bounds;
printf("point cloud dimension %u %u %u\n", map_dim.x, map_dim.y, map_dim.z);
if (parameter.plan_size.x != 0.0f && parameter.plan_size.y != 0.0f && parameter.plan_size.z != 0.0f)
{
Vector3f tmp = parameter.plan_size * 1000.0f;
map_dim = Vector3ui(uint32_t(tmp.x), uint32_t(tmp.y), uint32_t(tmp.z));
printf("dim in cm %u %u %u\n", map_dim.x, map_dim.y, map_dim.z);
}
uint64_t map_voxel = uint64_t(map_dim.x) * uint64_t(map_dim.y) * uint64_t(map_dim.z);
float scaling = 1.0;
if (parameter.max_memory == 0)
{
// compute scaling factor based on voxel size
scaling = 1.0f / parameter.resolution_tree;
}
else
{
// compute max scaling factor based on memory restriction
uint64_t max_voxel = uint64_t(parameter.max_memory) / sizeof(ProbabilisticVoxel);
printf("max_voxel %lu map_voxel %lu\n", max_voxel, map_voxel);
if (max_voxel <= map_voxel)
scaling = float(pow(max_voxel / double(map_voxel), 1.0 / 3));
}
printf("scaling %f\n", scaling);
std::vector<Vector3ui> points;
Vector3ui map_dim_tmp;
transformPointCloud(parameter.points, points, map_dim_tmp, scaling * 1000.0f);
map_dim = Vector3ui(uint32_t(ceil(map_dim.x * scaling)), uint32_t(ceil(map_dim.y * scaling)),
uint32_t(ceil(map_dim.z * scaling)));
printf("voxel map dimension %u %u %u\n", map_dim.x, map_dim.y, map_dim.z);
// center data at the middle of the map, just like for NTree
point_data_bounds = Vector3ui(uint32_t(ceil(point_data_bounds.x * scaling)),
uint32_t(ceil(point_data_bounds.y * scaling)),
uint32_t(ceil(point_data_bounds.z * scaling)));
Vector3ui tmp_offset = (map_dim - point_data_bounds) / Vector3ui(2);
insert_points.resize(points.size());
printf("scaling %f\n", scaling);
voxel_map_res = (1.0f / scaling) / 1000.0f;;
printf("mapres %f\n", voxel_map_res);
for (int i = 0; i < int(points.size()); ++i)
{
points[i] = points[i] + tmp_offset;
insert_points[i].x = points[i].x * voxel_map_res + voxel_map_res / 2;
insert_points[i].y = points[i].y * voxel_map_res + voxel_map_res / 2;
insert_points[i].z = points[i].z * voxel_map_res + voxel_map_res / 2;
}
PERF_MON_START(temp_timer);
}
else
{
// VoxelMap with same size as octree
uint32_t dim = (uint32_t) pow(pow(BRANCHING_FACTOR, 1.0 / 3), parameter.resolution_tree);
map_dim = Vector3ui(dim);
voxel_map_res = parameter.resolution_tree * 0.001f; // voxel size in meter
}
switch(m_parameter->model_type)
{
case Provider_Parameter::eMT_Probabilistic:
{
m_voxelMap = new gpu_voxels::voxelmap::ProbVoxelMap(map_dim.x, map_dim.y, map_dim.z, voxel_map_res, MT_PROBAB_VOXELMAP);
break;
}
case Provider_Parameter::eMT_BitVector:
{
m_voxelMap = new gpu_voxels::voxelmap::BitVectorVoxelMap(map_dim.x, map_dim.y, map_dim.z, voxel_map_res, MT_BITVECTOR_VOXELMAP);
break;
}
default:
{
printf("ERROR: Unknown 'model_type'\n");
}
}
m_segment.find_or_construct<MapType>(std::string(shm_variable_name_voxelmap_type + m_shared_mem_id).c_str())(m_voxelMap->getMapType());
if (insert_points.size() != 0)
{
m_voxelMap->insertPointCloud(insert_points, gpu_voxels::eBVM_OCCUPIED);
// if (m_parameter->model_type == Provider_Parameter::eMT_BitVector)
// {
// Vector3f offset(1, 1, 1);
// for (uint32_t k = 1; k < 4; ++k)
// {
// std::vector<Vector3f> tmp = insert_points;
// for (int i = 0; i < int(tmp.size()); ++i)
// tmp[i] = tmp[i] + offset * k;
//
// m_voxelMap->insertPointCloud(tmp, gpu_voxels::eBVM_UNDEFINED + k);
// }
// }
PERF_MON_PRINT_INFO_P(temp_timer, "Build", prefix);
}
PERF_MON_ADD_DATA_NONTIME_P("UsedMemory", m_voxelMap->getMemoryUsage(), prefix);
m_sensor_orientation = gpu_voxels::Vector3f(0, 0, 0);
m_sensor_position = gpu_voxels::Vector3f(
(m_voxelMap->getDimensions().x * m_voxelMap->getVoxelSideLength()) / 2,
(m_voxelMap->getDimensions().y * m_voxelMap->getVoxelSideLength()) / 2,
(m_voxelMap->getDimensions().z * m_voxelMap->getVoxelSideLength()) / 2) * 0.001f; // in meter
printf("VoxelMap created!\n");
m_mutex.unlock();
}
bool VisNTree<InnerNode, LeafNode>::visualize(const bool force_repaint)
{
openOrCreateSegment();
uint32_t shared_mem_id;
if (m_shm_memHandle == NULL) // do this only once
{
// there should only be one segment of number_of_octrees
std::pair<uint32_t*, std::size_t> r = m_segment.find<uint32_t>(
shm_variable_name_number_of_octrees.c_str());
if (r.second == 0)
{ // if it doesn't exist ..
m_segment.construct<uint32_t>(shm_variable_name_number_of_octrees.c_str())(1);
shared_mem_id = 0;
}
else
{ // if it exit increase it by one
shared_mem_id = *r.first;
(*r.first)++;
}
// get shared memory pointer
std::stringstream id;
id << shared_mem_id;
m_shm_superVoxelSize = m_segment.find_or_construct<uint32_t>(shm_variable_name_super_voxel_size.c_str())(
1);
m_shm_memHandle = m_segment.find_or_construct<cudaIpcMemHandle_t>(
std::string(shm_variable_name_octree_handler_dev_pointer + id.str()).c_str())(cudaIpcMemHandle_t());
m_shm_numCubes = m_segment.find_or_construct<uint32_t>(
std::string(shm_variable_name_number_cubes + id.str()).c_str())(0);
m_shm_bufferSwapped = m_segment.find_or_construct<bool>(
std::string(shm_variable_name_buffer_swapped + id.str()).c_str())(false);
m_shm_mapName = m_segment.find_or_construct_it<char>(
std::string(shm_variable_name_octree_name + id.str()).c_str())[m_map_name.size()](m_map_name.data());
}
uint32_t tmp = *m_shm_superVoxelSize - 1;
// m_shm_bufferSwapped tells, if visualizer already rendered the frame
// m_internal_buffer tells, which buffer should be used
if (*m_shm_bufferSwapped == false && (tmp != m_min_level || force_repaint))
{
m_min_level = tmp;
uint32_t cube_buffer_size;
Cube *d_cubes_buffer;
if(m_internal_buffer_1)
{
// extractCubes() allocates memory for the d_cubes_1, if the pointer is NULL
cube_buffer_size = m_ntree->extractCubes(m_d_cubes_1, NULL, m_min_level);
d_cubes_buffer = thrust::raw_pointer_cast(m_d_cubes_1->data());
m_internal_buffer_1 = false;
}else{
// extractCubes() allocates memory for the d_cubes_2, if the pointer is NULL
cube_buffer_size = m_ntree->extractCubes(m_d_cubes_2, NULL, m_min_level);
d_cubes_buffer = thrust::raw_pointer_cast(m_d_cubes_2->data());
m_internal_buffer_1 = true;
}
HANDLE_CUDA_ERROR(cudaIpcGetMemHandle(m_shm_memHandle, d_cubes_buffer));
*m_shm_numCubes = cube_buffer_size;
*m_shm_bufferSwapped = true;
return true;
}
return false;
}
bool VisTemplateVoxelList<Voxel, VoxelIDType>::visualize(const bool force_repaint)
{
openOrCreateSegment();
uint32_t shared_mem_id;
if (m_shm_memHandle == NULL)
{
// there should only be one segment of number_of_voxelmaps
std::pair<uint32_t*, std::size_t> r = m_segment.find<uint32_t>(
shm_variable_name_number_of_voxellists.c_str());
if (r.second == 0)
{ // if it doesn't exists ..
m_segment.construct<uint32_t>(shm_variable_name_number_of_voxellists.c_str())(1);
shared_mem_id = 0;
}
else
{ // if it exists increase it by one
shared_mem_id = *r.first;
(*r.first)++;
}
// get shared memory pointer
std::stringstream id;
id << shared_mem_id;
m_shm_memHandle = m_segment.find_or_construct<cudaIpcMemHandle_t>(
std::string(shm_variable_name_voxellist_handler_dev_pointer + id.str()).c_str())(
cudaIpcMemHandle_t());
m_shm_num_cubes = m_segment.find_or_construct<uint32_t>(
std::string(shm_variable_name_voxellist_num_voxels + id.str()).c_str())(uint32_t(0));
m_shm_bufferSwapped = m_segment.find_or_construct<bool>(
std::string(shm_variable_name_voxellist_buffer_swapped + id.str()).c_str())(false);
std::cout << "Name of shared buffer swapped: " << std::string(shm_variable_name_voxellist_buffer_swapped + id.str()).c_str() << "." << std::endl;
m_shm_mapName = m_segment.find_or_construct_it<char>(
std::string(shm_variable_name_voxellist_name + id.str()).c_str())[m_map_name.size()](
m_map_name.data());
m_shm_voxellist_type = m_segment.find_or_construct<MapType>(
std::string(shm_variable_name_voxellist_type + id.str()).c_str())(m_voxellist->getMapType());
}
if (*m_shm_bufferSwapped == false && force_repaint)
{
uint32_t cube_buffer_size;
Cube *d_cubes_buffer;
if(m_internal_buffer_1)
{
// extractCubes() allocates memory for the m_dev_buffer_1, if the pointer is NULL
m_voxellist->extractCubes(&m_dev_buffer_1);
cube_buffer_size = m_dev_buffer_1->size();
d_cubes_buffer = thrust::raw_pointer_cast(m_dev_buffer_1->data());
m_internal_buffer_1 = false;
}
else
{
// extractCubes() allocates memory for the m_dev_buffer_2, if the pointer is NULL
m_voxellist->extractCubes(&m_dev_buffer_2);
cube_buffer_size = m_dev_buffer_2->size();
d_cubes_buffer = thrust::raw_pointer_cast(m_dev_buffer_2->data());
m_internal_buffer_1 = true;
}
if(cube_buffer_size > 0)
{
// first open or create and the set the values
HANDLE_CUDA_ERROR(cudaIpcGetMemHandle(m_shm_memHandle, d_cubes_buffer));
*m_shm_num_cubes = cube_buffer_size;
*m_shm_bufferSwapped = true;
return true;
}else{
return false;
}
}
return false;
}
