rs2::frame pointcloud::process_frame(const rs2::frame_source& source, const rs2::frame& f) { rs2::frame rv; if (auto composite = f.as<rs2::frameset>()) { auto texture = composite.first(_stream_filter.stream); inspect_other_frame(texture); auto depth = composite.first(RS2_STREAM_DEPTH, RS2_FORMAT_Z16); inspect_depth_frame(depth); rv = process_depth_frame(source, depth); } else { if (f.is<rs2::depth_frame>()) { inspect_depth_frame(f); rv = process_depth_frame(source, f); } if (f.get_profile().stream_type() == _stream_filter.stream && f.get_profile().format() == _stream_filter.format) { inspect_other_frame(f); } } return rv; }
void pointcloud::inspect_other_frame(const rs2::frame& other) { if (_stream_filter != _prev_stream_filter) { _prev_stream_filter = _stream_filter; } if (_extrinsics.has_value() && other.get_profile().get() == _other_stream.get_profile().get()) return; _other_stream = other; _other_intrinsics = optional_value<rs2_intrinsics>(); _extrinsics = optional_value<rs2_extrinsics>(); if (!_other_intrinsics) { auto stream_profile = _other_stream.get_profile(); if (auto video = stream_profile.as<rs2::video_stream_profile>()) { _other_intrinsics = video.get_intrinsics(); _occlusion_filter->set_texel_intrinsics(_other_intrinsics.value()); } } set_extrinsics(); }
void operator()(int size, const rs2::frame& rs_matrix, char* max_matrix) { const auto image = (const uint8_t *)(rs_matrix.get_data()); auto matrix_out = (char*)(max_matrix); std::copy(image, image + size, matrix_out); }
rs2::frame hole_filling_filter::prepare_target_frame(const rs2::frame& f, const rs2::frame_source& source) { // Allocate and copy the content of the input data to the target rs2::frame tgt = source.allocate_video_frame(_target_stream_profile, f, int(_bpp), int(_width), int(_height), int(_stride), _extension_type); memmove(const_cast<void*>(tgt.get_data()), f.get_data(), _current_frm_size_pixels * _bpp); return tgt; }
bool disparity_transform::should_process(const rs2::frame& frame) { if (!frame) return false; if (frame.is<rs2::frameset>()) return false; if (_transform_to_disparity && (frame.get_profile().stream_type() != RS2_STREAM_DEPTH || frame.get_profile().format() != RS2_FORMAT_Z16)) return false; if (!_transform_to_disparity && (frame.get_profile().stream_type() != RS2_STREAM_DEPTH || (frame.get_profile().format() != RS2_FORMAT_DISPARITY16 && frame.get_profile().format() != RS2_FORMAT_DISPARITY32))) return false; if (frame.is<rs2::disparity_frame>() == _transform_to_disparity) return false; return true; }
void compare_frame_md(rs2::frame origin_depth, rs2::frame result_depth) { for (auto i = 0; i < rs2_frame_metadata_value::RS2_FRAME_METADATA_COUNT; i++) { bool origin_supported = origin_depth.supports_frame_metadata((rs2_frame_metadata_value)i); bool result_supported = result_depth.supports_frame_metadata((rs2_frame_metadata_value)i); REQUIRE(origin_supported == result_supported); if (origin_supported && result_supported) { //FRAME_TIMESTAMP and SENSOR_TIMESTAMP metadatas are not included in post proccesing frames, //TIME_OF_ARRIVAL continues to increase after post proccesing if (i == RS2_FRAME_METADATA_FRAME_TIMESTAMP || i == RS2_FRAME_METADATA_SENSOR_TIMESTAMP || i == RS2_FRAME_METADATA_TIME_OF_ARRIVAL) continue; rs2_metadata_type origin_val = origin_depth.get_frame_metadata((rs2_frame_metadata_value)i); rs2_metadata_type result_val = result_depth.get_frame_metadata((rs2_frame_metadata_value)i); REQUIRE(origin_val == result_val); } } }
bool validate_ppf_results(rs2::frame origin_depth, rs2::frame result_depth, const ppf_test_config& reference_data, size_t frame_idx) { std::vector<uint16_t> diff2orig; std::vector<uint16_t> diff2ref; // Basic sanity scenario with no filters applied. // validating domain transform in/out conversion. Requiring input=output bool domain_transform_only = (reference_data.downsample_scale == 1) && (!reference_data.spatial_filter) && (!reference_data.temporal_filter); auto result_profile = result_depth.get_profile().as<rs2::video_stream_profile>(); REQUIRE(result_profile); CAPTURE(result_profile.width()); CAPTURE(result_profile.height()); REQUIRE(result_profile.width() == reference_data.output_res_x); REQUIRE(result_profile.height() == reference_data.output_res_y); auto pixels = result_profile.width()*result_profile.height(); diff2ref.resize(pixels); if (domain_transform_only) diff2orig.resize(pixels); // Pixel-by-pixel comparison of the resulted filtered depth vs data ercorded with external tool auto v1 = reinterpret_cast<const uint16_t*>(result_depth.get_data()); auto v2 = reinterpret_cast<const uint16_t*>(reference_data._output_frames[frame_idx].data()); for (auto i = 0; i < pixels; i++) { uint16_t diff = std::abs(*v1++ - *v2++); diff2ref[i] = diff; } // validating depth<->disparity domain transformation is lostless. if (domain_transform_only) REQUIRE(profile_diffs("./DomainTransform.txt",diff2orig, 0, 0, frame_idx)); // Validate the filters // The differences between the reference code and librealsense implementation are byte-compared below return profile_diffs("./Filterstransform.txt", diff2ref, 0.f, 0, frame_idx); }
void hole_filling_filter::update_configuration(const rs2::frame& f) { if (f.get_profile().get() != _source_stream_profile.get()) { _source_stream_profile = f.get_profile(); _target_stream_profile = _source_stream_profile.clone(RS2_STREAM_DEPTH, 0, _source_stream_profile.format()); environment::get_instance().get_extrinsics_graph().register_same_extrinsics( *(stream_interface*)(f.get_profile().get()->profile), *(stream_interface*)(_target_stream_profile.get()->profile)); _extension_type = f.is<rs2::disparity_frame>() ? RS2_EXTENSION_DISPARITY_FRAME : RS2_EXTENSION_DEPTH_FRAME; _bpp = (_extension_type == RS2_EXTENSION_DISPARITY_FRAME) ? sizeof(float) : sizeof(uint16_t); auto vp = _target_stream_profile.as<rs2::video_stream_profile>(); _width = vp.width(); _height = vp.height(); _stride = _width * _bpp; _current_frm_size_pixels = _width * _height; } }
bool pointcloud::should_process(const rs2::frame& frame) { if (!frame) return false; auto set = frame.as<rs2::frameset>(); if (set) { //process composite frame only if it contains both a depth frame and the requested texture frame if (_stream_filter.stream == RS2_STREAM_ANY) return false; auto tex = set.first_or_default(_stream_filter.stream, _stream_filter.format); if (!tex) return false; auto depth = set.first_or_default(RS2_STREAM_DEPTH, RS2_FORMAT_Z16); if (!depth) return false; } else { if (frame.get_profile().stream_type() == RS2_STREAM_DEPTH && frame.get_profile().format() == RS2_FORMAT_Z16) return true; auto p = frame.get_profile(); if (p.stream_type() == _stream_filter.stream && p.format() == _stream_filter.format && p.stream_index() == _stream_filter.index) return true; return false; //TODO: switch to this code when map_to api is removed //if (_stream_filter != RS2_STREAM_ANY) // return false; //process single frame only if it is a depth frame //if (frame.get_profile().stream_type() != RS2_STREAM_DEPTH || frame.get_profile().format() != RS2_FORMAT_Z16) // return false; } return true; }
void pointcloud::inspect_depth_frame(const rs2::frame& depth) { if (!_output_stream || _depth_stream.get_profile().get() != depth.get_profile().get()) { _output_stream = depth.get_profile().as<rs2::video_stream_profile>().clone( RS2_STREAM_DEPTH, depth.get_profile().stream_index(), RS2_FORMAT_XYZ32F); _depth_stream = depth; _depth_intrinsics = optional_value<rs2_intrinsics>(); _depth_units = optional_value<float>(); _extrinsics = optional_value<rs2_extrinsics>(); } bool found_depth_intrinsics = false; bool found_depth_units = false; if (!_depth_intrinsics) { auto stream_profile = depth.get_profile(); if (auto video = stream_profile.as<rs2::video_stream_profile>()) { _depth_intrinsics = video.get_intrinsics(); _pixels_map.resize(_depth_intrinsics->height*_depth_intrinsics->width); _occlusion_filter->set_depth_intrinsics(_depth_intrinsics.value()); preprocess(); found_depth_intrinsics = true; } } if (!_depth_units) { auto sensor = ((frame_interface*)depth.get())->get_sensor().get(); _depth_units = sensor->get_option(RS2_OPTION_DEPTH_UNITS).query(); found_depth_units = true; } set_extrinsics(); }
rs2::frame yuy2rgb::process_frame(const rs2::frame_source& source, const rs2::frame& f) { auto p = f.get_profile(); if (p.get() != _source_stream_profile.get()) { _source_stream_profile = p; _target_stream_profile = p.clone(p.stream_type(), p.stream_index(), RS2_FORMAT_RGB8); } rs2::frame ret; auto vf = f.as<rs2::video_frame>(); ret = source.allocate_video_frame(_target_stream_profile, f, _traget_bpp, vf.get_width(), vf.get_height(), vf.get_width() * _traget_bpp, RS2_EXTENSION_VIDEO_FRAME); byte* planes[1]; planes[0] = (byte*)ret.get_data(); unpack_yuy2_rgb8(planes, (const byte*)f.get_data(), vf.get_width(), vf.get_height()); return ret; }
rs2::frame align::process_frame(const rs2::frame_source& source, const rs2::frame& f) { rs2::frame rv; std::vector<rs2::frame> output_frames; std::vector<rs2::frame> other_frames; auto frames = f.as<rs2::frameset>(); auto depth = frames.first_or_default(RS2_STREAM_DEPTH, RS2_FORMAT_Z16).as<rs2::depth_frame>(); _depth_scale = ((librealsense::depth_frame*)depth.get())->get_units(); if (_to_stream_type == RS2_STREAM_DEPTH) frames.foreach([&other_frames](const rs2::frame& f) {if (f.get_profile().stream_type() != RS2_STREAM_DEPTH) other_frames.push_back(f); }); else frames.foreach([this, &other_frames](const rs2::frame& f) {if (f.get_profile().stream_type() == _to_stream_type) other_frames.push_back(f); }); if (_to_stream_type == RS2_STREAM_DEPTH) { for (auto from : other_frames) { auto aligned_frame = allocate_aligned_frame(source, from, depth); align_frames(aligned_frame, from, depth); output_frames.push_back(aligned_frame); } } else { auto to = other_frames.front(); auto aligned_frame = allocate_aligned_frame(source, depth, to); align_frames(aligned_frame, depth, to); output_frames.push_back(aligned_frame); } auto new_composite = source.allocate_composite_frame(std::move(output_frames)); return new_composite; }
bool align::should_process(const rs2::frame& frame) { if (!frame) return false; auto set = frame.as<rs2::frameset>(); if (!set) return false; auto profile = frame.get_profile(); rs2_stream stream = profile.stream_type(); rs2_format format = profile.format(); int index = profile.stream_index(); //process composite frame only if it contains both a depth frame and the requested texture frame bool has_tex = false, has_depth = false; set.foreach([this, &has_tex](const rs2::frame& frame) { if (frame.get_profile().stream_type() == _to_stream_type) has_tex = true; }); set.foreach([&has_depth](const rs2::frame& frame) { if (frame.get_profile().stream_type() == RS2_STREAM_DEPTH && frame.get_profile().format() == RS2_FORMAT_Z16) has_depth = true; }); if (!has_tex || !has_depth) return false; return true; }
void disparity_transform::update_transformation_profile(const rs2::frame& f) { if(f.get_profile().get() != _source_stream_profile.get()) { _source_stream_profile = f.get_profile(); // Check if the new frame originated from stereo-based depth sensor // and retrieve the stereo baseline parameter that will be used in transformations auto snr = ((frame_interface*)f.get())->get_sensor().get(); librealsense::depth_stereo_sensor* dss; // Playback sensor if (auto a = As<librealsense::extendable_interface>(snr)) { librealsense::depth_stereo_sensor* ptr; if (_stereoscopic_depth = a->extend_to(TypeToExtension<librealsense::depth_stereo_sensor>::value, (void**)&ptr)) { dss = ptr; _depth_units = dss->get_depth_scale(); _stereo_baseline_meter = dss->get_stereo_baseline_mm()*0.001f; } } else // Live sensor { _stereoscopic_depth = Is<librealsense::depth_stereo_sensor>(snr); if (_stereoscopic_depth) { dss = As<librealsense::depth_stereo_sensor>(snr); _depth_units = dss->get_depth_scale(); _stereo_baseline_meter = dss->get_stereo_baseline_mm()* 0.001f; } } if (_stereoscopic_depth) { auto vp = _source_stream_profile.as<rs2::video_stream_profile>(); _focal_lenght_mm = vp.get_intrinsics().fx; const uint8_t fractional_bits = 5; const uint8_t fractions = 1 << fractional_bits; _d2d_convert_factor = (_stereo_baseline_meter * _focal_lenght_mm * fractions) / _depth_units; _width = vp.width(); _height = vp.height(); _update_target = true; } } // Adjust the target profile if (_update_target) { auto tgt_format = _transform_to_disparity ? RS2_FORMAT_DISPARITY32 : RS2_FORMAT_Z16; _target_stream_profile = _source_stream_profile.clone(RS2_STREAM_DEPTH, 0, tgt_format); auto src_vspi = dynamic_cast<video_stream_profile_interface*>(_source_stream_profile.get()->profile); auto tgt_vspi = dynamic_cast<video_stream_profile_interface*>(_target_stream_profile.get()->profile); rs2_intrinsics src_intrin = src_vspi->get_intrinsics(); tgt_vspi->set_intrinsics([src_intrin]() { return src_intrin; }); tgt_vspi->set_dims(src_intrin.width, src_intrin.height); _update_target = false; } }
rs2::frame colorizer::process_frame(const rs2::frame_source& source, const rs2::frame& f) { if (f.get_profile().get() != _source_stream_profile.get()) { _source_stream_profile = f.get_profile(); _target_stream_profile = f.get_profile().clone(RS2_STREAM_DEPTH, 0, RS2_FORMAT_RGB8); } 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; 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(_target_stream_profile, f, 3, vf.get_width(), vf.get_height(), vf.get_width() * 3, RS2_EXTENSION_VIDEO_FRAME); if (_equalize) make_equalized_histogram(f, ret); else make_value_cropped_frame(f, ret); return ret; }