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; }