pointcloud::pointcloud() :stream_filter_processing_block("Pointcloud") { _occlusion_filter = std::make_shared<occlusion_filter>(); auto occlusion_invalidation = std::make_shared<ptr_option<uint8_t>>( occlusion_none, occlusion_max - 1, 1, occlusion_none, (uint8_t*)&_occlusion_filter->_occlusion_filter, "Occlusion removal"); occlusion_invalidation->on_set([this, occlusion_invalidation](float val) { if (!occlusion_invalidation->is_valid(val)) throw invalid_value_exception(to_string() << "Unsupported occlusion filtering requiested " << val << " is out of range."); _occlusion_filter->set_mode(static_cast<uint8_t>(val)); }); occlusion_invalidation->set_description(0.f, "Off"); occlusion_invalidation->set_description(1.f, "Heuristic"); occlusion_invalidation->set_description(2.f, "Exhaustive"); register_option(RS2_OPTION_FILTER_MAGNITUDE, occlusion_invalidation); }
disparity_transform::disparity_transform(bool transform_to_disparity): generic_processing_block(transform_to_disparity ? "Depth to Disparity" : "Disparity to Depth"), _transform_to_disparity(transform_to_disparity), _update_target(false), _stereoscopic_depth(false), _focal_lenght_mm(0.f), _stereo_baseline_meter(0.f), _depth_units(0.f), _d2d_convert_factor(0.f), _width(0), _height(0), _bpp(0) { auto transform_opt = std::make_shared<ptr_option<bool>>( false,true,true,true, &_transform_to_disparity, "Stereoscopic Transformation Mode"); transform_opt->set_description(false, "Disparity to Depth"); transform_opt->set_description(true, "Depth to Disparity"); transform_opt->on_set([this, transform_opt](float val) { std::lock_guard<std::mutex> lock(_mutex); if (!transform_opt->is_valid(val)) throw invalid_value_exception(to_string() << "Unsupported transformation mode" << (int)val << " is out of range."); on_set_mode(static_cast<bool>(!!int(val))); }); unregister_option(RS2_OPTION_FRAMES_QUEUE_SIZE); on_set_mode(_transform_to_disparity); }
hole_filling_filter::hole_filling_filter() : _width(0), _height(0), _stride(0), _bpp(0), _extension_type(RS2_EXTENSION_DEPTH_FRAME), _current_frm_size_pixels(0), _hole_filling_mode(hole_fill_def) { _stream_filter.stream = RS2_STREAM_DEPTH; _stream_filter.format = RS2_FORMAT_Z16; auto hole_filling_mode = std::make_shared<ptr_option<uint8_t>>( hole_fill_min, hole_fill_max, hole_fill_step, hole_fill_def, &_hole_filling_mode, "Hole Filling mode"); hole_filling_mode->set_description(hf_fill_from_left, "Fill from Left"); hole_filling_mode->set_description(hf_farest_from_around, "Farest from around"); hole_filling_mode->set_description(hf_nearest_from_around, "Nearest from around"); hole_filling_mode->on_set([this, hole_filling_mode](float val) { std::lock_guard<std::mutex> lock(_mutex); if (!hole_filling_mode->is_valid(val)) throw invalid_value_exception(to_string() << "Unsupported mode for hole filling selected: value " << val << " is out of range."); _hole_filling_mode = static_cast<uint8_t>(val); }); register_option(RS2_OPTION_HOLES_FILL, hole_filling_mode); }
colorizer::colorizer() : _min(0.f), _max(6.f), _equalize(true), _stream() { _maps = { &jet, &classic, &grayscale, &inv_grayscale, &biomes, &cold, &warm, &quantized, &pattern }; auto min_opt = std::make_shared<ptr_option<float>>(0.f, 16.f, 0.1f, 0.f, &_min, "Min range in meters"); register_option(RS2_OPTION_MIN_DISTANCE, min_opt); auto max_opt = std::make_shared<ptr_option<float>>(0.f, 16.f, 0.1f, 6.f, &_max, "Max range in meters"); register_option(RS2_OPTION_MAX_DISTANCE, max_opt); auto color_map = std::make_shared<ptr_option<int>>(0, (int)_maps.size() - 1, 1, 0, &_map_index, "Color map"); color_map->set_description(0.f, "Jet"); color_map->set_description(1.f, "Classic"); color_map->set_description(2.f, "White to Black"); color_map->set_description(3.f, "Black to White"); color_map->set_description(4.f, "Bio"); color_map->set_description(5.f, "Cold"); color_map->set_description(6.f, "Warm"); color_map->set_description(7.f, "Quantized"); color_map->set_description(8.f, "Pattern"); register_option(RS2_OPTION_COLOR_SCHEME, color_map); auto preset_opt = std::make_shared<ptr_option<int>>(0, 3, 1, 0, &_preset, "Preset depth colorization"); preset_opt->set_description(0.f, "Dynamic"); preset_opt->set_description(1.f, "Fixed"); preset_opt->set_description(2.f, "Near"); preset_opt->set_description(3.f, "Far"); preset_opt->on_set([this](float val) { if (fabs(val - 0.f) < 1e-6) { // Dynamic _equalize = true; _map_index = 0; } if (fabs(val - 1.f) < 1e-6) { // Fixed _equalize = false; _map_index = 0; _min = 0.f; _max = 6.f; } if (fabs(val - 2.f) < 1e-6) { // Near _equalize = false; _map_index = 1; _min = 0.3f; _max = 1.5f; } if (fabs(val - 3.f) < 1e-6) { // Far _equalize = false; _map_index = 0; _min = 1.f; _max = 16.f; } }); register_option(RS2_OPTION_VISUAL_PRESET, preset_opt); auto hist_opt = std::make_shared<ptr_option<bool>>(false, true, true, true, &_equalize, "Perform histogram equalization"); register_option(RS2_OPTION_HISTOGRAM_EQUALIZATION_ENABLED, hist_opt); auto on_frame = [this](rs2::frame f, const rs2::frame_source& source) { auto process_frame = [this, &source](const rs2::frame f) { { std::lock_guard<std::mutex> lock(_mutex); if (!_stream) { _stream = std::make_shared<rs2::stream_profile>(f.get_profile().clone(RS2_STREAM_DEPTH, 0, RS2_FORMAT_RGB8)); environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*_stream->get()->profile, *f.get_profile().get()->profile); } } auto make_equalized_histogram = [this](const rs2::video_frame& depth, rs2::video_frame rgb) { const auto max_depth = 0x10000; static uint32_t histogram[max_depth]; memset(histogram, 0, sizeof(histogram)); const auto w = depth.get_width(), h = depth.get_height(); const auto depth_data = reinterpret_cast<const uint16_t*>(depth.get_data()); auto rgb_data = reinterpret_cast<uint8_t*>(const_cast<void *>(rgb.get_data())); for (auto i = 0; i < w*h; ++i) ++histogram[depth_data[i]]; for (auto i = 2; i < max_depth; ++i) histogram[i] += histogram[i - 1]; // Build a cumulative histogram for the indices in [1,0xFFFF] auto cm = _maps[_map_index]; for (auto i = 0; i < w*h; ++i) { auto d = depth_data[i]; if (d) { auto f = histogram[d] / (float)histogram[0xFFFF]; // 0-255 based on histogram location auto c = cm->get(f); rgb_data[i * 3 + 0] = (uint8_t)c.x; rgb_data[i * 3 + 1] = (uint8_t)c.y; rgb_data[i * 3 + 2] = (uint8_t)c.z; } else { rgb_data[i * 3 + 0] = 0; rgb_data[i * 3 + 1] = 0; rgb_data[i * 3 + 2] = 0; } } }; auto make_value_cropped_frame = [this](const rs2::video_frame& depth, rs2::video_frame rgb) { const auto max_depth = 0x10000; const auto w = depth.get_width(), h = depth.get_height(); const auto depth_data = reinterpret_cast<const uint16_t*>(depth.get_data()); auto rgb_data = reinterpret_cast<uint8_t*>(const_cast<void *>(rgb.get_data())); auto fi = (frame_interface*)depth.get(); auto df = dynamic_cast<librealsense::depth_frame*>(fi); auto depth_units = df->get_units(); for (auto i = 0; i < w*h; ++i) { auto d = depth_data[i]; if (d) { auto f = (d * depth_units - _min) / (_max - _min); auto c = _maps[_map_index]->get(f); rgb_data[i * 3 + 0] = (uint8_t)c.x; rgb_data[i * 3 + 1] = (uint8_t)c.y; rgb_data[i * 3 + 2] = (uint8_t)c.z; } else { rgb_data[i * 3 + 0] = 0; rgb_data[i * 3 + 1] = 0; rgb_data[i * 3 + 2] = 0; } } }; rs2::frame ret = f; if (f.get_profile().stream_type() == RS2_STREAM_DEPTH) { auto vf = f.as<rs2::video_frame>(); rs2_extension ext = f.is<rs2::disparity_frame>() ? RS2_EXTENSION_DISPARITY_FRAME : RS2_EXTENSION_DEPTH_FRAME; ret = source.allocate_video_frame(*_stream, f, 3, vf.get_width(), vf.get_height(), vf.get_width() * 3, ext); if (_equalize) make_equalized_histogram(f, ret); else make_value_cropped_frame(f, ret); } source.frame_ready(ret); }; if (auto composite = f.as<rs2::frameset>()) composite.foreach(process_frame); else process_frame(f); }; auto callback = new rs2::frame_processor_callback<decltype(on_frame)>(on_frame); processing_block::set_processing_callback(std::shared_ptr<rs2_frame_processor_callback>(callback)); }
disparity_transform::disparity_transform(bool transform_to_disparity): _transform_to_disparity(transform_to_disparity), _update_target(false), _stereoscopic_depth(false), _focal_lenght_mm(0.f), _stereo_baseline(0.f), _depth_units(0.f), _d2d_convert_factor(0.f), _width(0), _height(0), _bpp(0) { auto transform_opt = std::make_shared<ptr_option<bool>>( false,true,true,true, &_transform_to_disparity, "Stereoscopic Transformation Mode"); transform_opt->set_description(false, "Disparity to Depth"); transform_opt->set_description(true, "Depth to Disparity"); transform_opt->on_set([this, transform_opt](float val) { std::lock_guard<std::mutex> lock(_mutex); if (!transform_opt->is_valid(val)) throw invalid_value_exception(to_string() << "Unsupported transformation mode" << (int)val << " is out of range."); on_set_mode(static_cast<bool>(!!int(val))); }); unregister_option(RS2_OPTION_FRAMES_QUEUE_SIZE); auto on_frame = [this](rs2::frame f, const rs2::frame_source& source) { std::lock_guard<std::mutex> lock(_mutex); rs2::frame out = f; rs2::frame tgt, depth_data; bool composite = f.is<rs2::frameset>(); tgt = depth_data = (composite) ? f.as<rs2::frameset>().first_or_default(RS2_STREAM_DEPTH) : f; // Verify that the input depth format is aligned with the block's configuration if (depth_data && (f.is<rs2::disparity_frame>() != _transform_to_disparity)) { update_transformation_profile(depth_data); if (_stereoscopic_depth && (tgt = prepare_target_frame(depth_data, source))) { auto src = depth_data.as<rs2::video_frame>(); if (_transform_to_disparity) convert<uint16_t, float>(src.get_data(), const_cast<void*>(tgt.get_data())); else convert<float, uint16_t>(src.get_data(), const_cast<void*>(tgt.get_data())); } } out = composite ? source.allocate_composite_frame({ tgt }) : tgt; source.frame_ready(out); }; auto callback = new rs2::frame_processor_callback<decltype(on_frame)>(on_frame); processing_block::set_processing_callback(std::shared_ptr<rs2_frame_processor_callback>(callback)); on_set_mode(_transform_to_disparity); }
colorizer::colorizer() : _min(0.f), _max(6.f), _equalize(true), _target_stream_profile() { _stream_filter.stream = RS2_STREAM_DEPTH; _stream_filter.format = RS2_FORMAT_Z16; _maps = { &jet, &classic, &grayscale, &inv_grayscale, &biomes, &cold, &warm, &quantized, &pattern }; auto min_opt = std::make_shared<ptr_option<float>>(0.f, 16.f, 0.1f, 0.f, &_min, "Min range in meters"); register_option(RS2_OPTION_MIN_DISTANCE, min_opt); auto max_opt = std::make_shared<ptr_option<float>>(0.f, 16.f, 0.1f, 6.f, &_max, "Max range in meters"); register_option(RS2_OPTION_MAX_DISTANCE, max_opt); auto color_map = std::make_shared<ptr_option<int>>(0, (int)_maps.size() - 1, 1, 0, &_map_index, "Color map"); color_map->set_description(0.f, "Jet"); color_map->set_description(1.f, "Classic"); color_map->set_description(2.f, "White to Black"); color_map->set_description(3.f, "Black to White"); color_map->set_description(4.f, "Bio"); color_map->set_description(5.f, "Cold"); color_map->set_description(6.f, "Warm"); color_map->set_description(7.f, "Quantized"); color_map->set_description(8.f, "Pattern"); register_option(RS2_OPTION_COLOR_SCHEME, color_map); auto preset_opt = std::make_shared<ptr_option<int>>(0, 3, 1, 0, &_preset, "Preset depth colorization"); preset_opt->set_description(0.f, "Dynamic"); preset_opt->set_description(1.f, "Fixed"); preset_opt->set_description(2.f, "Near"); preset_opt->set_description(3.f, "Far"); preset_opt->on_set([this](float val) { if (fabs(val - 0.f) < 1e-6) { // Dynamic _equalize = true; _map_index = 0; } if (fabs(val - 1.f) < 1e-6) { // Fixed _equalize = false; _map_index = 0; _min = 0.f; _max = 6.f; } if (fabs(val - 2.f) < 1e-6) { // Near _equalize = false; _map_index = 1; _min = 0.3f; _max = 1.5f; } if (fabs(val - 3.f) < 1e-6) { // Far _equalize = false; _map_index = 0; _min = 1.f; _max = 16.f; } }); register_option(RS2_OPTION_VISUAL_PRESET, preset_opt); auto hist_opt = std::make_shared<ptr_option<bool>>(false, true, true, true, &_equalize, "Perform histogram equalization"); register_option(RS2_OPTION_HISTOGRAM_EQUALIZATION_ENABLED, hist_opt); }