diff --git a/common/model-views.cpp b/common/model-views.cpp index e7c02afec4..42647d2038 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -2713,7 +2713,7 @@ namespace rs2 float(dev->algo_roi.max_x - dev->algo_roi.min_x), float(dev->algo_roi.max_y - dev->algo_roi.min_y) }; - r = r.normalize(_normalized_zoom.unnormalize(get_stream_bounds())).unnormalize(stream_rect).cut_by(stream_rect); + r = r.normalize(_normalized_zoom.unnormalize(get_original_stream_bounds())).unnormalize(stream_rect).cut_by(stream_rect); glColor3f(yellow.x, yellow.y, yellow.z); draw_rect(r, 2); @@ -3016,10 +3016,10 @@ namespace rs2 { for (auto&& s : viewer.streams) { - if(!s.second.dev) continue; + if (!s.second.dev) continue; auto dev = s.second.dev; - if(s.second.original_profile.unique_id() == f.get_profile().unique_id()) + if (s.second.original_profile.unique_id() == f.get_profile().unique_id()) { if (dev->post_processing_enabled) { @@ -3051,20 +3051,33 @@ namespace rs2 f = hole_filling->invoke(f); } - return f; + break; } } } } - // Override the the first pixel in Depth->RGB map to be used as a mark when occlusion filter is active - if ((f.get_profile().stream_type() == RS2_STREAM_COLOR)) + // Override the zero pixel in texture frame with black color for occlusion invalidation + // TODO - this is a temporal solution to be refactored from the app level into the core library + switch (stream_type) + { + case RS2_STREAM_COLOR: { - auto rgb_stream = const_cast(static_cast(f.get_data())); - memset(rgb_stream, 0, 3); // Override the zero pixel with black color for occlusion marking - // Alternatively, enable the next two lines to render invalidation with magenta color for inspection - //rgb_stream[0] = rgb_stream[2] = 0xff; // Use magenta to highlight the occlusion areas - //rgb_stream[1] = 0; + auto rgb_stream = const_cast(static_cast(f.get_data())); + memset(rgb_stream, 0, 3); + // Alternatively, enable the next two lines to render invalidation with magenta color for inspection + //rgb_stream[0] = rgb_stream[2] = 0xff; // Use magenta to highlight the occlusion areas + //rgb_stream[1] = 0; + } + break; + case RS2_STREAM_INFRARED: + { + auto ir_stream = const_cast(static_cast(f.get_data())); + memset(ir_stream, 0, 2); // Override the first two bytes to cover Y8/Y16 formats + } + break; + default: + break; } return f; diff --git a/common/model-views.h b/common/model-views.h index ad169783bf..9eeede2608 100644 --- a/common/model-views.h +++ b/common/model-views.h @@ -929,7 +929,7 @@ namespace rs2 float3 target = { 0.0f, 0.0f, 0.0f }; float3 up; bool fixed_up = true; - bool render_quads = false; + bool render_quads = true; float view[16]; bool texture_wrapping_on = true; diff --git a/common/rendering.h b/common/rendering.h index 08f4f9d28a..1e1875dfe3 100644 --- a/common/rendering.h +++ b/common/rendering.h @@ -1060,6 +1060,8 @@ namespace rs2 if (auto colorized_frame = colorize->colorize(frame).as()) { data = colorized_frame.get_data(); + // Override the first pixel in the colorized image for occlusion invalidation. + memset((void*)data,0, colorized_frame.get_bytes_per_pixel()); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, colorized_frame.get_width(), colorized_frame.get_height(), diff --git a/src/proc/colorizer.cpp b/src/proc/colorizer.cpp index 1ec884fdb4..8dfc653a6c 100644 --- a/src/proc/colorizer.cpp +++ b/src/proc/colorizer.cpp @@ -196,7 +196,7 @@ namespace librealsense { { std::lock_guard lock(_mutex); - if (!_stream) + if (!_stream || (f.get_profile().get() != _stream->get())) { _stream = std::make_shared(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); diff --git a/src/proc/decimation-filter.cpp b/src/proc/decimation-filter.cpp index 739da2a58a..1019b1e3ab 100644 --- a/src/proc/decimation-filter.cpp +++ b/src/proc/decimation-filter.cpp @@ -1,880 +1,907 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2017 Intel Corporation. All Rights Reserved. - -#include "../include/librealsense2/hpp/rs_sensor.hpp" -#include "../include/librealsense2/hpp/rs_processing.hpp" - -#include -#include -#include "option.h" -#include "context.h" -#include "proc/synthetic-stream.h" -#include "proc/decimation-filter.h" -#include "environment.h" - -#define PIX_SORT(a,b) { if ((a)>(b)) PIX_SWAP((a),(b)); } -#define PIX_SWAP(a,b) { pixelvalue temp=(a);(a)=(b);(b)=temp; } -#define PIX_MIN(a,b) ((a)>(b)) ? (b) : (a) -#define PIX_MAX(a,b) ((a)>(b)) ? (a) : (b) - -namespace librealsense -{ - /*---------------------------------------------------------------------------- - Function : opt_med3() - In : pointer to array of 3 pixel values - Out : a pixelvalue - Job : optimized search of the median of 3 pixel values - Notice : found on sci.image.processing - cannot go faster unless assumptions are made - on the nature of the input signal. - ---------------------------------------------------------------------------*/ - template - inline pixelvalue opt_med3(pixelvalue * p) - { - PIX_SORT(p[0], p[1]); - PIX_SORT(p[1], p[2]); - PIX_SORT(p[0], p[1]); - return p[1]; - } - - /* - ** opt_med4() - hacked version - **/ - template - inline pixelvalue opt_med4(pixelvalue * p) - { - PIX_SORT(p[0], p[1]); - PIX_SORT(p[2], p[3]); - PIX_SORT(p[0], p[2]); - PIX_SORT(p[1], p[3]); - return PIX_MIN(p[1], p[2]); - } - - /*---------------------------------------------------------------------------- - Function : opt_med5() - In : pointer to array of 5 pixel values - Out : a pixelvalue - Job : optimized search of the median of 5 pixel values - Notice : found on sci.image.processing - cannot go faster unless assumptions are made - on the nature of the input signal. - ---------------------------------------------------------------------------*/ - template - inline pixelvalue opt_med5(pixelvalue * p) - { - PIX_SORT(p[0], p[1]); - PIX_SORT(p[3], p[4]); - p[3] = PIX_MAX(p[0], p[3]); - p[1] = PIX_MIN(p[1], p[4]); - PIX_SORT(p[1], p[2]); - p[2] = PIX_MIN(p[2], p[3]); - return PIX_MAX(p[1], p[2]); - } - - /*---------------------------------------------------------------------------- - Function : opt_med6() - In : pointer to array of 6 pixel values - Out : a pixelvalue - Job : optimized search of the median of 6 pixel values - Notice : from Christoph_John@gmx.de - based on a selection network which was proposed in - "FAST, EFFICIENT MEDIAN FILTERS WITH EVEN LENGTH WINDOWS" - J.P. HAVLICEK, K.A. SAKADY, G.R.KATZ - If you need larger even length kernels check the paper - ---------------------------------------------------------------------------*/ - template - inline pixelvalue opt_med6(pixelvalue * p) - { - PIX_SORT(p[1], p[2]); - PIX_SORT(p[3], p[4]); - PIX_SORT(p[0], p[1]); - PIX_SORT(p[2], p[3]); - PIX_SORT(p[4], p[5]); - PIX_SORT(p[1], p[2]); - PIX_SORT(p[3], p[4]); - PIX_SORT(p[0], p[1]); - PIX_SORT(p[2], p[3]); - p[4] = PIX_MIN(p[4], p[5]); - p[2] = PIX_MAX(p[1], p[2]); - p[3] = PIX_MIN(p[3], p[4]); - return PIX_MIN(p[2], p[3]); - } - - /*---------------------------------------------------------------------------- - Function : opt_med7() - In : pointer to array of 7 pixel values - Out : a pixelvalue - Job : optimized search of the median of 7 pixel values - Notice : found on sci.image.processing - cannot go faster unless assumptions are made - on the nature of the input signal. - ---------------------------------------------------------------------------*/ - template - inline pixelvalue opt_med7(pixelvalue * p) - { - PIX_SORT(p[0], p[5]); - PIX_SORT(p[0], p[3]); - PIX_SORT(p[1], p[6]); - PIX_SORT(p[2], p[4]); - PIX_SORT(p[0], p[1]); - PIX_SORT(p[3], p[5]); - PIX_SORT(p[2], p[6]); - p[3] = PIX_MAX(p[2], p[3]); - p[3] = PIX_MIN(p[3], p[6]); - p[4] = PIX_MIN(p[4], p[5]); - PIX_SORT(p[1], p[4]); - p[3] = PIX_MAX(p[1], p[3]); - return PIX_MIN(p[3], p[4]); - } - - /*---------------------------------------------------------------------------- - Function : opt_med9() - Hacked version of opt_med9() - */ - template - inline pixelvalue opt_med8(pixelvalue * p) - { - PIX_SORT(p[0], p[1]); - PIX_SORT(p[3], p[4]); - PIX_SORT(p[6], p[7]); - PIX_SORT(p[2], p[3]); - PIX_SORT(p[5], p[6]); - PIX_SORT(p[3], p[4]); - PIX_SORT(p[6], p[7]); - p[4] = PIX_MIN(p[4], p[7]); - PIX_SORT(p[3], p[6]); - p[5] = PIX_MAX(p[2], p[5]); - p[3] = PIX_MAX(p[0], p[3]); - p[1] = PIX_MIN(p[1], p[4]); - p[3] = PIX_MIN(p[3], p[6]); - PIX_SORT(p[3], p[1]); - p[3] = PIX_MAX(p[5], p[3]); - return PIX_MIN(p[3], p[1]); - } - - /*---------------------------------------------------------------------------- - Function : opt_med9() - In : pointer to an array of 9 pixelvalues - Out : a pixelvalue - Job : optimized search of the median of 9 pixelvalues - Notice : in theory, cannot go faster without assumptions on the - signal. - Formula from: - XILINX XCELL magazine, vol. 23 by John L. Smith - - The input array is modified in the process - The result array is guaranteed to contain the median - value - in middle position, but other elements are NOT sorted. - ---------------------------------------------------------------------------*/ - template - inline pixelvalue opt_med9(pixelvalue * p) - { - PIX_SORT(p[1], p[2]); - PIX_SORT(p[4], p[5]); - PIX_SORT(p[7], p[8]); - PIX_SORT(p[0], p[1]); - PIX_SORT(p[3], p[4]); - PIX_SORT(p[6], p[7]); - PIX_SORT(p[1], p[2]); - PIX_SORT(p[4], p[5]); - PIX_SORT(p[7], p[8]); - p[3] = PIX_MAX(p[0], p[3]); - p[5] = PIX_MIN(p[5], p[8]); - PIX_SORT(p[4], p[7]); - p[6] = PIX_MAX(p[3], p[6]); - p[4] = PIX_MAX(p[1], p[4]); - p[2] = PIX_MIN(p[2], p[5]); - p[4] = PIX_MIN(p[4], p[7]); - PIX_SORT(p[4], p[2]); - p[4] = PIX_MAX(p[6], p[4]); - return PIX_MIN(p[4], p[2]); - } - - const uint8_t decimation_min_val = 1; - const uint8_t decimation_max_val = 8; // Decimation levels according to the reference design - const uint8_t decimation_default_val = 2; - const uint8_t decimation_step = 1; // Linear decimation - - decimation_filter::decimation_filter() : - _decimation_factor(decimation_default_val), - _patch_size(decimation_default_val), - _kernel_size(_patch_size*_patch_size), - _real_width(), - _real_height(0), - _padded_width(0), - _padded_height(0), - _recalc_profile(false) - { - auto decimation_control = std::make_shared>( - decimation_min_val, - decimation_max_val, - decimation_step, - decimation_default_val, - &_decimation_factor, "Decimation scale"); - decimation_control->on_set([this, decimation_control](float val) - { - std::lock_guard lock(_mutex); - - if (!decimation_control->is_valid(val)) - throw invalid_value_exception(to_string() - << "Unsupported decimation scale " << val << " is out of range."); - - // Linear decimation factor - _patch_size = _decimation_factor = static_cast(val); - _kernel_size = _patch_size*_patch_size; - _recalc_profile = true; - }); - - register_option(RS2_OPTION_FILTER_MAGNITUDE, decimation_control); - - auto on_frame = [this](rs2::frame f, const rs2::frame_source& source) - { - std::lock_guard lock(_mutex); - - rs2::frame out = f, tgt; - bool composite = f.is(); - - if (composite) - { - rs2::frame depth = f.as().first_or_default(RS2_STREAM_DEPTH); - if (depth) - { - update_output_profile(depth); - rs2_extension tgt_type = depth.is() ? RS2_EXTENSION_DISPARITY_FRAME : RS2_EXTENSION_DEPTH_FRAME; - if (tgt = prepare_target_frame(depth, source, tgt_type)) - { - auto src = depth.as(); - decimate_depth(static_cast(src.get_data()), - static_cast(const_cast(tgt.get_data())), - src.get_width(), src.get_height(), this->_patch_size); - } - } - - rs2::frame color = f.as().first_or_default(RS2_STREAM_COLOR); - if (color) - { - update_output_profile(color); - if (tgt = prepare_target_frame(color, source, RS2_EXTENSION_VIDEO_FRAME)) - { - auto src = color.as(); - decimate_others(color.get_profile().format(), src.get_data(), - const_cast(tgt.get_data()), - src.get_width(), src.get_height(), this->_patch_size); - } - } - - rs2::frame infared_0 = f.as().get_infrared_frame(0); - if (infared_0) - { - update_output_profile(infared_0); - if (tgt = prepare_target_frame(infared_0, source, RS2_EXTENSION_VIDEO_FRAME)) - { - auto src = infared_0.as(); - decimate_others(infared_0.get_profile().format(), src.get_data(), - const_cast(tgt.get_data()), - src.get_width(), src.get_height(), this->_patch_size); - } - } - - rs2::frame infared_1 = f.as().get_infrared_frame(1); - if (infared_1) - { - update_output_profile(infared_1); - if (tgt = prepare_target_frame(infared_1, source, RS2_EXTENSION_VIDEO_FRAME)) - { - auto src = infared_1.as(); - decimate_others(infared_1.get_profile().format(), src.get_data(), - const_cast(tgt.get_data()), - src.get_width(), src.get_height(), this->_patch_size); - } - } - - out = source.allocate_composite_frame({ tgt }); - } - else - { - if (f) - { - update_output_profile(f); - - auto src = f.as(); - rs2::stream_profile profile = f.get_profile(); - rs2_format format = profile.format(); - rs2_stream type = profile.stream_type(); - - rs2_extension tgt_type; - if (type == RS2_STREAM_COLOR || type == RS2_STREAM_INFRARED) - tgt_type = RS2_EXTENSION_VIDEO_FRAME; - else - tgt_type = f.is() ? RS2_EXTENSION_DISPARITY_FRAME : RS2_EXTENSION_DEPTH_FRAME; - - if (tgt = prepare_target_frame(f, source, tgt_type)) - { - if (format == RS2_FORMAT_Z16) - { - decimate_depth(static_cast(src.get_data()), - static_cast(const_cast(tgt.get_data())), - src.get_width(), src.get_height(), this->_patch_size); - } - else - { - decimate_others(format, src.get_data(), - const_cast(tgt.get_data()), - src.get_width(), src.get_height(), this->_patch_size); - } - } - } - - out = tgt; - } - - source.frame_ready(out); - }; - - auto callback = new rs2::frame_processor_callback(on_frame); - processing_block::set_processing_callback(std::shared_ptr(callback)); - } - - void decimation_filter::update_output_profile(const rs2::frame& f) - { - if (f.get_profile().get() != _source_stream_profile.get()) - { - _source_stream_profile = f.get_profile(); - _recalc_profile = true; - } - - // Buld a new target profile for every system/filter change - if (_recalc_profile) - { - auto vp = _source_stream_profile.as(); - - _target_stream_profile = _source_stream_profile.clone(f.get_profile().stream_type(), f.get_profile().stream_index(), _source_stream_profile.format()); - environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*(stream_interface*)(_source_stream_profile.get()->profile), *(stream_interface*)(_target_stream_profile.get()->profile)); - auto src_vspi = dynamic_cast(_source_stream_profile.get()->profile); - auto tgt_vspi = dynamic_cast(_target_stream_profile.get()->profile); - rs2_intrinsics src_intrin = src_vspi->get_intrinsics(); - rs2_intrinsics tgt_intrin = tgt_vspi->get_intrinsics(); - - // recalculate real/padded output frame size based on new input porperties - _real_width = src_vspi->get_width() / _patch_size; - _real_height = src_vspi->get_height() / _patch_size; - - // The resulted frame dimension will be dividible by 4; - _padded_width = _real_width + 3; - _padded_width /= 4; - _padded_width *= 4; - - _padded_height = _real_height + 3; - _padded_height /= 4; - _padded_height *= 4; - - tgt_intrin.width = _padded_width; - tgt_intrin.height = _padded_height; - tgt_intrin.fx = src_intrin.fx/_patch_size; - tgt_intrin.fy = src_intrin.fy/_patch_size; - tgt_intrin.ppx = src_intrin.ppx/_patch_size; - tgt_intrin.ppy = src_intrin.ppy/_patch_size; - - tgt_vspi->set_intrinsics([tgt_intrin]() { return tgt_intrin; }); - tgt_vspi->set_dims(tgt_intrin.width, tgt_intrin.height); - - _recalc_profile = false; - } - } - - rs2::frame decimation_filter::prepare_target_frame(const rs2::frame& f, const rs2::frame_source& source, rs2_extension tgt_type) - { - auto vf = f.as(); - return source.allocate_video_frame(_target_stream_profile, f, - vf.get_bytes_per_pixel(), - _padded_width, - _padded_height, - _padded_width*vf.get_bytes_per_pixel(), - tgt_type); - } - - void decimation_filter::decimate_depth(const uint16_t * frame_data_in, uint16_t * frame_data_out, - size_t width_in, size_t height_in, size_t scale) - { - // Use median filtering - std::vector working_kernel(_kernel_size); - auto wk_begin = working_kernel.data(); - auto wk_itr = wk_begin; - std::vector pixel_raws(scale); - uint16_t* block_start = const_cast(frame_data_in); - - if (scale == 2 || scale == 3) - { - for (int j = 0; j < _real_height; j++) - { - uint16_t *p{}; - // Mark the beginning of each of the N lines that the filter will run upon - for (size_t i = 0; i < pixel_raws.size(); i++) - pixel_raws[i] = block_start + (width_in*i); - - for (size_t i = 0, chunk_offset = 0; i < _real_width; i++) - { - wk_itr = wk_begin; - // extract data the kernel to process - for (size_t n = 0; n < scale; ++n) - { - p = pixel_raws[n] + chunk_offset; - for (size_t m = 0; m < scale; ++m) - { - if (*(p + m)) - *wk_itr++ = *(p + m); - } - } - - // For even-size kernels pick the member one below the middle - auto ks = (int)(wk_itr - wk_begin); - if (ks == 0) - *frame_data_out++ = 0; - else - { - switch (ks) - { - case 1: - *frame_data_out++ = working_kernel[0]; - break; - case 2: - *frame_data_out++ = PIX_MIN(working_kernel[0], working_kernel[1]); - break; - case 3: - *frame_data_out++ = opt_med3(working_kernel.data()); - break; - case 4: - *frame_data_out++ = opt_med4(working_kernel.data()); - break; - case 5: - *frame_data_out++ = opt_med5(working_kernel.data()); - break; - case 6: - *frame_data_out++ = opt_med6(working_kernel.data()); - break; - case 7: - *frame_data_out++ = opt_med7(working_kernel.data()); - break; - case 8: - *frame_data_out++ = opt_med8(working_kernel.data()); - break; - case 9: - *frame_data_out++ = opt_med9(working_kernel.data()); - break; - } - } - - chunk_offset += scale; - } - - // Fill-in the padded colums with zeros - for (int j = _real_width; j < _padded_width; j++) - *frame_data_out++ = 0; - - // Skip N lines to the beginnig of the next processing segment - block_start += width_in*scale; - } - } - else - { - for (int j = 0; j < _real_height; j++) - { - uint16_t *p{}; - // Mark the beginning of each of the N lines that the filter will run upon - for (size_t i = 0; i < pixel_raws.size(); i++) - pixel_raws[i] = block_start + (width_in*i); - - for (size_t i = 0, chunk_offset = 0; i < _real_width; i++) - { - int sum = 0; - int counter = 0; - - // extract data the kernel to process - for (size_t n = 0; n < scale; ++n) - { - p = pixel_raws[n] + chunk_offset; - for (size_t m = 0; m < scale; ++m) - { - if (*(p + m)) - { - sum += p[m]; - ++counter; - } - } - } - - *frame_data_out++ = (counter == 0 ? 0 : sum / counter); - chunk_offset += scale; - } - - // Fill-in the padded colums with zeros - for (int j = _real_width; j < _padded_width; j++) - *frame_data_out++ = 0; - - // Skip N lines to the beginnig of the next processing segment - block_start += width_in*scale; - } - } - - // Fill-in the padded rows with zeros - for (auto v = _real_height; v < _padded_height; ++v) - { - for (auto u = 0; u < _padded_width; ++u) - *frame_data_out++ = 0; - } - } - - void decimation_filter::decimate_others(rs2_format format, const void * frame_data_in, void * frame_data_out, - size_t width_in, size_t height_in, size_t scale) - { - int sum = 0; - auto patch_size = scale * scale; - - switch (format) - { - case RS2_FORMAT_YUYV: - { - uint8_t* from = (uint8_t*)frame_data_in; - uint8_t* p = nullptr; - uint8_t* q = (uint8_t*)frame_data_out; - - auto w_2 = width_in >> 1; - auto rw_2 = _real_width >> 1; - auto pw_2 = _padded_width >> 1; - auto s2 = scale >> 1; - bool odd = (scale & 1); - for (int j = 0; j < _real_height; ++j) - { - for (int i = 0; i < rw_2; ++i) - { - p = from + scale * (j * w_2 + i) * 4; - sum = 0; - for (int n = 0; n < scale; ++n) - { - for (int m = 0; m < scale; ++m) - sum += p[m * 2]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - - p = from + scale * (j * w_2 + i) * 4 + 1; - sum = 0; - for (int n = 0; n < scale; ++n) - { - for (int m = 0; m < s2; ++m) - sum += 2 * p[m * 4]; - - if (odd) - sum += p[s2 * 4]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - - p = from + scale * (j * w_2 + i) * 4 + s2 * 4 + (odd ? 2 : 0); - sum = 0; - for (int n = 0; n < scale; ++n) - { - for (int m = 0; m < scale; ++m) - sum += p[m * 2]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - - p = from + scale * (j * w_2 + i) * 4 + 3; - sum = 0; - for (int n = 0; n < scale; ++n) - { - for (int m = 0; m < s2; ++m) - sum += 2 * p[m * 4]; - - if (odd) - sum += p[s2 * 4]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - } - - for (int i = rw_2; i < pw_2; ++i) - { - *q++ = 0; - *q++ = 0; - *q++ = 0; - *q++ = 0; - } - } - - for (int j = _real_height; j < _padded_height; ++j) - { - for (int i = 0; i < _padded_width; ++i) - { - *q++ = 0; - *q++ = 0; - } - } - } - break; - - case RS2_FORMAT_UYVY: - { - uint8_t* from = (uint8_t*)frame_data_in; - uint8_t* p = nullptr; - uint8_t* q = (uint8_t*)frame_data_out; - - auto w_2 = width_in >> 1; - auto rw_2 = _real_width >> 1; - auto pw_2 = _padded_width >> 1; - auto s2 = scale >> 1; - bool odd = (scale & 1); - for (int j = 0; j < _real_height; ++j) - { - for (int i = 0; i < rw_2; ++i) - { - p = from + scale * (j * w_2 + i) * 4; - sum = 0; - for (int n = 0; n < scale; ++n) - { - for (int m = 0; m < s2; ++m) - sum += 2 * p[m * 4]; - - if (odd) - sum += p[s2 * 4]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - - p = from + scale * (j * w_2 + i) * 4 + 1; - sum = 0; - for (int n = 0; n < scale; ++n) - { - for (int m = 0; m < scale; ++m) - sum += p[m * 2]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - - p = from + scale * (j * w_2 + i) * 4 + 2; - sum = 0; - for (int n = 0; n < scale; ++n) - { - for (int m = 0; m < s2; ++m) - sum += 2 * p[m * 4]; - - if (odd) - sum += p[s2 * 4]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - - p = from + scale * (j * w_2 + i) * 4 + s2 * 4 + (odd ? 3 : 1); - sum = 0; - for (int n = 0; n < scale; ++n) - { - for (int m = 0; m < scale; ++m) - sum += p[m * 2]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - } - - for (int i = rw_2; i < pw_2; ++i) - { - *q++ = 0; - *q++ = 0; - *q++ = 0; - *q++ = 0; - } - } - - for (int j = _real_height; j < _padded_height; ++j) - { - for (int i = 0; i < _padded_width; ++i) - { - *q++ = 0; - *q++ = 0; - } - } - } - break; - - case RS2_FORMAT_RGB8: - case RS2_FORMAT_BGR8: - { - uint8_t* from = (uint8_t*)frame_data_in; - uint8_t* p = nullptr; - uint8_t* q = (uint8_t*)frame_data_out;; - - for (int j = 0; j < _real_height; ++j) - { - for (int i = 0; i < _real_width; ++i) - { - for (int k = 0; k < 3; ++k) - { - p = from + scale * (j * width_in + i) * 3 + k; - sum = 0; - for (int n = 0; n < scale; ++n) - { - for (int m = 0; m < scale; ++m) - sum += p[m * 3]; - - p += width_in * 3; - } - - *q++ = (uint8_t)(sum / patch_size); - } - } - - for (int i = _real_width; i < _padded_width; ++i) - { - *q++ = 0; - *q++ = 0; - *q++ = 0; - } - } - - for (int j = _real_height; j < _padded_height; ++j) - { - for (int i = 0; i < _padded_width; ++i) - { - *q++ = 0; - *q++ = 0; - *q++ = 0; - } - } - } - break; - - case RS2_FORMAT_RGBA8: - case RS2_FORMAT_BGRA8: - { - uint8_t* from = (uint8_t*)frame_data_in; - uint8_t* p = nullptr; - uint8_t* q = (uint8_t*)frame_data_out; - - for (int j = 0; j < _real_height; ++j) - { - for (int i = 0; i < _real_width; ++i) - { - for (int k = 0; k < 4; ++k) - { - p = from + scale * (j * width_in + i) * 4 + k; - sum = 0; - for (int n = 0; n < scale; ++n) - { - for (int m = 0; m < scale; ++m) - sum += p[m * 4]; - - p += width_in * 4; - } - - *q++ = (uint8_t)(sum / patch_size); - } - } - - for (int i = _real_width; i < _padded_width; ++i) - { - *q++ = 0; - *q++ = 0; - *q++ = 0; - *q++ = 0; - } - } - - for (int j = _real_height; j < _padded_height; ++j) - { - for (int i = 0; i < _padded_width; ++i) - { - *q++ = 0; - *q++ = 0; - *q++ = 0; - *q++ = 0; - } - } - } - break; - - case RS2_FORMAT_Y8: - { - uint8_t* from = (uint8_t*)frame_data_in; - uint8_t* p = nullptr; - uint8_t* q = (uint8_t*)frame_data_out; - - for (int j = 0; j < _real_height; ++j) - { - for (int i = 0; i < _real_width; ++i) - { - p = from + scale * (j * width_in + i); - sum = 0; - for (int n = 0; n < scale; ++n) - { - for (int m = 0; m < scale; ++m) - sum += p[m]; - - p += width_in; - } - - *q++ = (uint8_t)(sum / patch_size); - } - - for (int i = _real_width; i < _padded_width; ++i) - *q++ = 0; - } - - for (int j = _real_height; j < _padded_height; ++j) - { - for (int i = 0; i < _padded_width; ++i) - *q++ = 0; - } - } - break; - - case RS2_FORMAT_Y16: - { - uint16_t* from = (uint16_t*)frame_data_in; - uint16_t* p = nullptr; - uint16_t* q = (uint16_t*)frame_data_out; - - for (int j = 0; j < _real_height; ++j) - { - for (int i = 0; i < _real_width; ++i) - { - p = from + scale * (j * width_in + i); - sum = 0; - for (int n = 0; n < scale; ++n) - { - for (int m = 0; m < scale; ++m) - sum += p[m]; - - p += width_in; - } - - *q++ = (uint16_t)(sum / patch_size); - } - - for (int i = _real_width; i < _padded_width; ++i) - *q++ = 0; - } - - for (int j = _real_height; j < _padded_height; ++j) - { - for (int i = 0; i < _padded_width; ++i) - *q++ = 0; - } - } - break; - - default: - break; - } - } -} - +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#include "../include/librealsense2/hpp/rs_sensor.hpp" +#include "../include/librealsense2/hpp/rs_processing.hpp" + +#include +#include +#include "option.h" +#include "context.h" +#include "proc/synthetic-stream.h" +#include "proc/decimation-filter.h" +#include "environment.h" + +#define PIX_SORT(a,b) { if ((a)>(b)) PIX_SWAP((a),(b)); } +#define PIX_SWAP(a,b) { pixelvalue temp=(a);(a)=(b);(b)=temp; } +#define PIX_MIN(a,b) ((a)>(b)) ? (b) : (a) +#define PIX_MAX(a,b) ((a)>(b)) ? (a) : (b) + +namespace librealsense +{ + /*---------------------------------------------------------------------------- + Function : opt_med3() + In : pointer to array of 3 pixel values + Out : a pixelvalue + Job : optimized search of the median of 3 pixel values + Notice : found on sci.image.processing + cannot go faster unless assumptions are made + on the nature of the input signal. + ---------------------------------------------------------------------------*/ + template + inline pixelvalue opt_med3(pixelvalue * p) + { + PIX_SORT(p[0], p[1]); + PIX_SORT(p[1], p[2]); + PIX_SORT(p[0], p[1]); + return p[1]; + } + + /* + ** opt_med4() + hacked version + **/ + template + inline pixelvalue opt_med4(pixelvalue * p) + { + PIX_SORT(p[0], p[1]); + PIX_SORT(p[2], p[3]); + PIX_SORT(p[0], p[2]); + PIX_SORT(p[1], p[3]); + return PIX_MIN(p[1], p[2]); + } + + /*---------------------------------------------------------------------------- + Function : opt_med5() + In : pointer to array of 5 pixel values + Out : a pixelvalue + Job : optimized search of the median of 5 pixel values + Notice : found on sci.image.processing + cannot go faster unless assumptions are made + on the nature of the input signal. + ---------------------------------------------------------------------------*/ + template + inline pixelvalue opt_med5(pixelvalue * p) + { + PIX_SORT(p[0], p[1]); + PIX_SORT(p[3], p[4]); + p[3] = PIX_MAX(p[0], p[3]); + p[1] = PIX_MIN(p[1], p[4]); + PIX_SORT(p[1], p[2]); + p[2] = PIX_MIN(p[2], p[3]); + return PIX_MAX(p[1], p[2]); + } + + /*---------------------------------------------------------------------------- + Function : opt_med6() + In : pointer to array of 6 pixel values + Out : a pixelvalue + Job : optimized search of the median of 6 pixel values + Notice : from Christoph_John@gmx.de + based on a selection network which was proposed in + "FAST, EFFICIENT MEDIAN FILTERS WITH EVEN LENGTH WINDOWS" + J.P. HAVLICEK, K.A. SAKADY, G.R.KATZ + If you need larger even length kernels check the paper + ---------------------------------------------------------------------------*/ + template + inline pixelvalue opt_med6(pixelvalue * p) + { + PIX_SORT(p[1], p[2]); + PIX_SORT(p[3], p[4]); + PIX_SORT(p[0], p[1]); + PIX_SORT(p[2], p[3]); + PIX_SORT(p[4], p[5]); + PIX_SORT(p[1], p[2]); + PIX_SORT(p[3], p[4]); + PIX_SORT(p[0], p[1]); + PIX_SORT(p[2], p[3]); + p[4] = PIX_MIN(p[4], p[5]); + p[2] = PIX_MAX(p[1], p[2]); + p[3] = PIX_MIN(p[3], p[4]); + return PIX_MIN(p[2], p[3]); + } + + /*---------------------------------------------------------------------------- + Function : opt_med7() + In : pointer to array of 7 pixel values + Out : a pixelvalue + Job : optimized search of the median of 7 pixel values + Notice : found on sci.image.processing + cannot go faster unless assumptions are made + on the nature of the input signal. + ---------------------------------------------------------------------------*/ + template + inline pixelvalue opt_med7(pixelvalue * p) + { + PIX_SORT(p[0], p[5]); + PIX_SORT(p[0], p[3]); + PIX_SORT(p[1], p[6]); + PIX_SORT(p[2], p[4]); + PIX_SORT(p[0], p[1]); + PIX_SORT(p[3], p[5]); + PIX_SORT(p[2], p[6]); + p[3] = PIX_MAX(p[2], p[3]); + p[3] = PIX_MIN(p[3], p[6]); + p[4] = PIX_MIN(p[4], p[5]); + PIX_SORT(p[1], p[4]); + p[3] = PIX_MAX(p[1], p[3]); + return PIX_MIN(p[3], p[4]); + } + + /*---------------------------------------------------------------------------- + Function : opt_med9() + Hacked version of opt_med9() + */ + template + inline pixelvalue opt_med8(pixelvalue * p) + { + PIX_SORT(p[0], p[1]); + PIX_SORT(p[3], p[4]); + PIX_SORT(p[6], p[7]); + PIX_SORT(p[2], p[3]); + PIX_SORT(p[5], p[6]); + PIX_SORT(p[3], p[4]); + PIX_SORT(p[6], p[7]); + p[4] = PIX_MIN(p[4], p[7]); + PIX_SORT(p[3], p[6]); + p[5] = PIX_MAX(p[2], p[5]); + p[3] = PIX_MAX(p[0], p[3]); + p[1] = PIX_MIN(p[1], p[4]); + p[3] = PIX_MIN(p[3], p[6]); + PIX_SORT(p[3], p[1]); + p[3] = PIX_MAX(p[5], p[3]); + return PIX_MIN(p[3], p[1]); + } + + /*---------------------------------------------------------------------------- + Function : opt_med9() + In : pointer to an array of 9 pixelvalues + Out : a pixelvalue + Job : optimized search of the median of 9 pixelvalues + Notice : in theory, cannot go faster without assumptions on the + signal. + Formula from: + XILINX XCELL magazine, vol. 23 by John L. Smith + + The input array is modified in the process + The result array is guaranteed to contain the median + value + in middle position, but other elements are NOT sorted. + ---------------------------------------------------------------------------*/ + template + inline pixelvalue opt_med9(pixelvalue * p) + { + PIX_SORT(p[1], p[2]); + PIX_SORT(p[4], p[5]); + PIX_SORT(p[7], p[8]); + PIX_SORT(p[0], p[1]); + PIX_SORT(p[3], p[4]); + PIX_SORT(p[6], p[7]); + PIX_SORT(p[1], p[2]); + PIX_SORT(p[4], p[5]); + PIX_SORT(p[7], p[8]); + p[3] = PIX_MAX(p[0], p[3]); + p[5] = PIX_MIN(p[5], p[8]); + PIX_SORT(p[4], p[7]); + p[6] = PIX_MAX(p[3], p[6]); + p[4] = PIX_MAX(p[1], p[4]); + p[2] = PIX_MIN(p[2], p[5]); + p[4] = PIX_MIN(p[4], p[7]); + PIX_SORT(p[4], p[2]); + p[4] = PIX_MAX(p[6], p[4]); + return PIX_MIN(p[4], p[2]); + } + + const uint8_t decimation_min_val = 1; + const uint8_t decimation_max_val = 8; // Decimation levels according to the reference design + const uint8_t decimation_default_val = 2; + const uint8_t decimation_step = 1; // Linear decimation + + decimation_filter::decimation_filter() : + _decimation_factor(decimation_default_val), + _control_val(decimation_default_val), + _patch_size(decimation_default_val), + _kernel_size(_patch_size*_patch_size), + _real_width(), + _real_height(0), + _padded_width(0), + _padded_height(0), + _recalc_profile(false), + _options_changed(false) + { + auto decimation_control = std::make_shared>( + decimation_min_val, + decimation_max_val, + decimation_step, + decimation_default_val, + &_control_val, "Decimation scale"); + decimation_control->on_set([this, decimation_control](float val) + { + std::lock_guard lock(_mutex); + + if (!decimation_control->is_valid(val)) + throw invalid_value_exception(to_string() + << "Unsupported decimation scale " << val << " is out of range."); + + // Linear decimation factor + if (_control_val != _decimation_factor) + { + _patch_size = _decimation_factor = _control_val; + _kernel_size = _patch_size*_patch_size; + _options_changed = true; + } + }); + + register_option(RS2_OPTION_FILTER_MAGNITUDE, decimation_control); + + auto on_frame = [this](rs2::frame f, const rs2::frame_source& source) + { + std::lock_guard lock(_mutex); + + rs2::frame out = f, tgt; + bool composite = f.is(); + + if (composite) + { + rs2::frame depth = f.as().first_or_default(RS2_STREAM_DEPTH); + if (depth) + { + update_output_profile(depth); + rs2_extension tgt_type = depth.is() ? RS2_EXTENSION_DISPARITY_FRAME : RS2_EXTENSION_DEPTH_FRAME; + if (tgt = prepare_target_frame(depth, source, tgt_type)) + { + auto src = depth.as(); + decimate_depth(static_cast(src.get_data()), + static_cast(const_cast(tgt.get_data())), + src.get_width(), src.get_height(), this->_patch_size); + } + } + + rs2::frame color = f.as().first_or_default(RS2_STREAM_COLOR); + if (color) + { + update_output_profile(color); + if (tgt = prepare_target_frame(color, source, RS2_EXTENSION_VIDEO_FRAME)) + { + auto src = color.as(); + decimate_others(color.get_profile().format(), src.get_data(), + const_cast(tgt.get_data()), + src.get_width(), src.get_height(), this->_patch_size); + } + } + + rs2::frame infared_0 = f.as().get_infrared_frame(0); + if (infared_0) + { + update_output_profile(infared_0); + if (tgt = prepare_target_frame(infared_0, source, RS2_EXTENSION_VIDEO_FRAME)) + { + auto src = infared_0.as(); + decimate_others(infared_0.get_profile().format(), src.get_data(), + const_cast(tgt.get_data()), + src.get_width(), src.get_height(), this->_patch_size); + } + } + + rs2::frame infared_1 = f.as().get_infrared_frame(1); + if (infared_1) + { + update_output_profile(infared_1); + if (tgt = prepare_target_frame(infared_1, source, RS2_EXTENSION_VIDEO_FRAME)) + { + auto src = infared_1.as(); + decimate_others(infared_1.get_profile().format(), src.get_data(), + const_cast(tgt.get_data()), + src.get_width(), src.get_height(), this->_patch_size); + } + } + + out = source.allocate_composite_frame({ tgt }); + } + else + { + if (f) + { + update_output_profile(f); + + auto src = f.as(); + rs2::stream_profile profile = f.get_profile(); + rs2_format format = profile.format(); + rs2_stream type = profile.stream_type(); + + rs2_extension tgt_type; + if (type == RS2_STREAM_COLOR || type == RS2_STREAM_INFRARED) + tgt_type = RS2_EXTENSION_VIDEO_FRAME; + else + tgt_type = f.is() ? RS2_EXTENSION_DISPARITY_FRAME : RS2_EXTENSION_DEPTH_FRAME; + + if (tgt = prepare_target_frame(f, source, tgt_type)) + { + if (format == RS2_FORMAT_Z16) + { + decimate_depth(static_cast(src.get_data()), + static_cast(const_cast(tgt.get_data())), + src.get_width(), src.get_height(), this->_patch_size); + } + else + { + decimate_others(format, src.get_data(), + const_cast(tgt.get_data()), + src.get_width(), src.get_height(), this->_patch_size); + } + } + } + + out = tgt; + } + + source.frame_ready(out); + }; + + auto callback = new rs2::frame_processor_callback(on_frame); + processing_block::set_processing_callback(std::shared_ptr(callback)); + } + + void decimation_filter::update_output_profile(const rs2::frame& f) + { + if (_options_changed || (f.get_profile().get() != _source_stream_profile.get())) + { + _options_changed = false; + _source_stream_profile = f.get_profile(); + const auto pf = _registered_profiles.find(std::make_tuple(_source_stream_profile.get(), _decimation_factor)); + if (_registered_profiles.end() != pf) + { + _target_stream_profile = pf->second; + auto tgt_vspi = dynamic_cast(_target_stream_profile.get()->profile); + auto f_pf = dynamic_cast(_source_stream_profile.get()->profile); + rs2_intrinsics tgt_intrin = tgt_vspi->get_intrinsics(); + + // Update real/padded output frame size based on retrieved input properties + _real_width = f_pf->get_width() / _patch_size; + _real_height = f_pf->get_height() / _patch_size; + _padded_width = tgt_intrin.width; + _padded_height = tgt_intrin.height; + } + else + { + _recalc_profile = true; + } + } + + // Buld a new target profile for every system/filter change + if (_recalc_profile) + { + auto vp = _source_stream_profile.as(); + + auto tmp_profile = _source_stream_profile.clone(_source_stream_profile.stream_type(), _source_stream_profile.stream_index(), _source_stream_profile.format()); + environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*(stream_interface*)(_source_stream_profile.get()->profile), *(stream_interface*)(tmp_profile.get()->profile)); + auto src_vspi = dynamic_cast(_source_stream_profile.get()->profile); + auto tgt_vspi = dynamic_cast(tmp_profile.get()->profile); + rs2_intrinsics src_intrin = src_vspi->get_intrinsics(); + rs2_intrinsics tgt_intrin = tgt_vspi->get_intrinsics(); + + // recalculate real/padded output frame size based on new input porperties + _real_width = src_vspi->get_width() / _patch_size; + _real_height = src_vspi->get_height() / _patch_size; + + // The resulted frame dimension will be dividible by 4; + _padded_width = _real_width + 3; + _padded_width /= 4; + _padded_width *= 4; + + _padded_height = _real_height + 3; + _padded_height /= 4; + _padded_height *= 4; + + tgt_intrin.width = _padded_width; + tgt_intrin.height = _padded_height; + tgt_intrin.fx = src_intrin.fx/_patch_size; + tgt_intrin.fy = src_intrin.fy/_patch_size; + tgt_intrin.ppx = src_intrin.ppx/_patch_size; + tgt_intrin.ppy = src_intrin.ppy/_patch_size; + + tgt_vspi->set_intrinsics([tgt_intrin]() { return tgt_intrin; }); + tgt_vspi->set_dims(tgt_intrin.width, tgt_intrin.height); + + _registered_profiles[std::make_tuple(_source_stream_profile.get(),_decimation_factor)]= _target_stream_profile = tmp_profile; + + _recalc_profile = false; + } + } + + rs2::frame decimation_filter::prepare_target_frame(const rs2::frame& f, const rs2::frame_source& source, rs2_extension tgt_type) + { + auto vf = f.as(); + auto ret = source.allocate_video_frame(_target_stream_profile, f, + vf.get_bytes_per_pixel(), + _padded_width, + _padded_height, + _padded_width*vf.get_bytes_per_pixel(), + tgt_type); + + return ret; + } + + void decimation_filter::decimate_depth(const uint16_t * frame_data_in, uint16_t * frame_data_out, + size_t width_in, size_t height_in, size_t scale) + { + // Use median filtering + std::vector working_kernel(_kernel_size); + auto wk_begin = working_kernel.data(); + auto wk_itr = wk_begin; + std::vector pixel_raws(scale); + uint16_t* block_start = const_cast(frame_data_in); + + if (scale == 2 || scale == 3) + { + for (int j = 0; j < _real_height; j++) + { + uint16_t *p{}; + // Mark the beginning of each of the N lines that the filter will run upon + for (size_t i = 0; i < pixel_raws.size(); i++) + pixel_raws[i] = block_start + (width_in*i); + + for (size_t i = 0, chunk_offset = 0; i < _real_width; i++) + { + wk_itr = wk_begin; + // extract data the kernel to process + for (size_t n = 0; n < scale; ++n) + { + p = pixel_raws[n] + chunk_offset; + for (size_t m = 0; m < scale; ++m) + { + if (*(p + m)) + *wk_itr++ = *(p + m); + } + } + + // For even-size kernels pick the member one below the middle + auto ks = (int)(wk_itr - wk_begin); + if (ks == 0) + *frame_data_out++ = 0; + else + { + switch (ks) + { + case 1: + *frame_data_out++ = working_kernel[0]; + break; + case 2: + *frame_data_out++ = PIX_MIN(working_kernel[0], working_kernel[1]); + break; + case 3: + *frame_data_out++ = opt_med3(working_kernel.data()); + break; + case 4: + *frame_data_out++ = opt_med4(working_kernel.data()); + break; + case 5: + *frame_data_out++ = opt_med5(working_kernel.data()); + break; + case 6: + *frame_data_out++ = opt_med6(working_kernel.data()); + break; + case 7: + *frame_data_out++ = opt_med7(working_kernel.data()); + break; + case 8: + *frame_data_out++ = opt_med8(working_kernel.data()); + break; + case 9: + *frame_data_out++ = opt_med9(working_kernel.data()); + break; + } + } + + chunk_offset += scale; + } + + // Fill-in the padded colums with zeros + for (int j = _real_width; j < _padded_width; j++) + *frame_data_out++ = 0; + + // Skip N lines to the beginnig of the next processing segment + block_start += width_in*scale; + } + } + else + { + for (int j = 0; j < _real_height; j++) + { + uint16_t *p{}; + // Mark the beginning of each of the N lines that the filter will run upon + for (size_t i = 0; i < pixel_raws.size(); i++) + pixel_raws[i] = block_start + (width_in*i); + + for (size_t i = 0, chunk_offset = 0; i < _real_width; i++) + { + int sum = 0; + int counter = 0; + + // extract data the kernel to process + for (size_t n = 0; n < scale; ++n) + { + p = pixel_raws[n] + chunk_offset; + for (size_t m = 0; m < scale; ++m) + { + if (*(p + m)) + { + sum += p[m]; + ++counter; + } + } + } + + *frame_data_out++ = (counter == 0 ? 0 : sum / counter); + chunk_offset += scale; + } + + // Fill-in the padded colums with zeros + for (int j = _real_width; j < _padded_width; j++) + *frame_data_out++ = 0; + + // Skip N lines to the beginnig of the next processing segment + block_start += width_in*scale; + } + } + + // Fill-in the padded rows with zeros + for (auto v = _real_height; v < _padded_height; ++v) + { + for (auto u = 0; u < _padded_width; ++u) + *frame_data_out++ = 0; + } + } + + void decimation_filter::decimate_others(rs2_format format, const void * frame_data_in, void * frame_data_out, + size_t width_in, size_t height_in, size_t scale) + { + int sum = 0; + auto patch_size = scale * scale; + + switch (format) + { + case RS2_FORMAT_YUYV: + { + uint8_t* from = (uint8_t*)frame_data_in; + uint8_t* p = nullptr; + uint8_t* q = (uint8_t*)frame_data_out; + + auto w_2 = width_in >> 1; + auto rw_2 = _real_width >> 1; + auto pw_2 = _padded_width >> 1; + auto s2 = scale >> 1; + bool odd = (scale & 1); + for (int j = 0; j < _real_height; ++j) + { + for (int i = 0; i < rw_2; ++i) + { + p = from + scale * (j * w_2 + i) * 4; + sum = 0; + for (int n = 0; n < scale; ++n) + { + for (int m = 0; m < scale; ++m) + sum += p[m * 2]; + + p += w_2 * 4; + } + *q++ = (uint8_t)(sum / patch_size); + + p = from + scale * (j * w_2 + i) * 4 + 1; + sum = 0; + for (int n = 0; n < scale; ++n) + { + for (int m = 0; m < s2; ++m) + sum += 2 * p[m * 4]; + + if (odd) + sum += p[s2 * 4]; + + p += w_2 * 4; + } + *q++ = (uint8_t)(sum / patch_size); + + p = from + scale * (j * w_2 + i) * 4 + s2 * 4 + (odd ? 2 : 0); + sum = 0; + for (int n = 0; n < scale; ++n) + { + for (int m = 0; m < scale; ++m) + sum += p[m * 2]; + + p += w_2 * 4; + } + *q++ = (uint8_t)(sum / patch_size); + + p = from + scale * (j * w_2 + i) * 4 + 3; + sum = 0; + for (int n = 0; n < scale; ++n) + { + for (int m = 0; m < s2; ++m) + sum += 2 * p[m * 4]; + + if (odd) + sum += p[s2 * 4]; + + p += w_2 * 4; + } + *q++ = (uint8_t)(sum / patch_size); + } + + for (int i = rw_2; i < pw_2; ++i) + { + *q++ = 0; + *q++ = 0; + *q++ = 0; + *q++ = 0; + } + } + + for (int j = _real_height; j < _padded_height; ++j) + { + for (int i = 0; i < _padded_width; ++i) + { + *q++ = 0; + *q++ = 0; + } + } + } + break; + + case RS2_FORMAT_UYVY: + { + uint8_t* from = (uint8_t*)frame_data_in; + uint8_t* p = nullptr; + uint8_t* q = (uint8_t*)frame_data_out; + + auto w_2 = width_in >> 1; + auto rw_2 = _real_width >> 1; + auto pw_2 = _padded_width >> 1; + auto s2 = scale >> 1; + bool odd = (scale & 1); + for (int j = 0; j < _real_height; ++j) + { + for (int i = 0; i < rw_2; ++i) + { + p = from + scale * (j * w_2 + i) * 4; + sum = 0; + for (int n = 0; n < scale; ++n) + { + for (int m = 0; m < s2; ++m) + sum += 2 * p[m * 4]; + + if (odd) + sum += p[s2 * 4]; + + p += w_2 * 4; + } + *q++ = (uint8_t)(sum / patch_size); + + p = from + scale * (j * w_2 + i) * 4 + 1; + sum = 0; + for (int n = 0; n < scale; ++n) + { + for (int m = 0; m < scale; ++m) + sum += p[m * 2]; + + p += w_2 * 4; + } + *q++ = (uint8_t)(sum / patch_size); + + p = from + scale * (j * w_2 + i) * 4 + 2; + sum = 0; + for (int n = 0; n < scale; ++n) + { + for (int m = 0; m < s2; ++m) + sum += 2 * p[m * 4]; + + if (odd) + sum += p[s2 * 4]; + + p += w_2 * 4; + } + *q++ = (uint8_t)(sum / patch_size); + + p = from + scale * (j * w_2 + i) * 4 + s2 * 4 + (odd ? 3 : 1); + sum = 0; + for (int n = 0; n < scale; ++n) + { + for (int m = 0; m < scale; ++m) + sum += p[m * 2]; + + p += w_2 * 4; + } + *q++ = (uint8_t)(sum / patch_size); + } + + for (int i = rw_2; i < pw_2; ++i) + { + *q++ = 0; + *q++ = 0; + *q++ = 0; + *q++ = 0; + } + } + + for (int j = _real_height; j < _padded_height; ++j) + { + for (int i = 0; i < _padded_width; ++i) + { + *q++ = 0; + *q++ = 0; + } + } + } + break; + + case RS2_FORMAT_RGB8: + case RS2_FORMAT_BGR8: + { + uint8_t* from = (uint8_t*)frame_data_in; + uint8_t* p = nullptr; + uint8_t* q = (uint8_t*)frame_data_out;; + + for (int j = 0; j < _real_height; ++j) + { + for (int i = 0; i < _real_width; ++i) + { + for (int k = 0; k < 3; ++k) + { + p = from + scale * (j * width_in + i) * 3 + k; + sum = 0; + for (int n = 0; n < scale; ++n) + { + for (int m = 0; m < scale; ++m) + sum += p[m * 3]; + + p += width_in * 3; + } + + *q++ = (uint8_t)(sum / patch_size); + } + } + + for (int i = _real_width; i < _padded_width; ++i) + { + *q++ = 0; + *q++ = 0; + *q++ = 0; + } + } + + for (int j = _real_height; j < _padded_height; ++j) + { + for (int i = 0; i < _padded_width; ++i) + { + *q++ = 0; + *q++ = 0; + *q++ = 0; + } + } + } + break; + + case RS2_FORMAT_RGBA8: + case RS2_FORMAT_BGRA8: + { + uint8_t* from = (uint8_t*)frame_data_in; + uint8_t* p = nullptr; + uint8_t* q = (uint8_t*)frame_data_out; + + for (int j = 0; j < _real_height; ++j) + { + for (int i = 0; i < _real_width; ++i) + { + for (int k = 0; k < 4; ++k) + { + p = from + scale * (j * width_in + i) * 4 + k; + sum = 0; + for (int n = 0; n < scale; ++n) + { + for (int m = 0; m < scale; ++m) + sum += p[m * 4]; + + p += width_in * 4; + } + + *q++ = (uint8_t)(sum / patch_size); + } + } + + for (int i = _real_width; i < _padded_width; ++i) + { + *q++ = 0; + *q++ = 0; + *q++ = 0; + *q++ = 0; + } + } + + for (int j = _real_height; j < _padded_height; ++j) + { + for (int i = 0; i < _padded_width; ++i) + { + *q++ = 0; + *q++ = 0; + *q++ = 0; + *q++ = 0; + } + } + } + break; + + case RS2_FORMAT_Y8: + { + uint8_t* from = (uint8_t*)frame_data_in; + uint8_t* p = nullptr; + uint8_t* q = (uint8_t*)frame_data_out; + + for (int j = 0; j < _real_height; ++j) + { + for (int i = 0; i < _real_width; ++i) + { + p = from + scale * (j * width_in + i); + sum = 0; + for (int n = 0; n < scale; ++n) + { + for (int m = 0; m < scale; ++m) + sum += p[m]; + + p += width_in; + } + + *q++ = (uint8_t)(sum / patch_size); + } + + for (int i = _real_width; i < _padded_width; ++i) + *q++ = 0; + } + + for (int j = _real_height; j < _padded_height; ++j) + { + for (int i = 0; i < _padded_width; ++i) + *q++ = 0; + } + } + break; + + case RS2_FORMAT_Y16: + { + uint16_t* from = (uint16_t*)frame_data_in; + uint16_t* p = nullptr; + uint16_t* q = (uint16_t*)frame_data_out; + + for (int j = 0; j < _real_height; ++j) + { + for (int i = 0; i < _real_width; ++i) + { + p = from + scale * (j * width_in + i); + sum = 0; + for (int n = 0; n < scale; ++n) + { + for (int m = 0; m < scale; ++m) + sum += p[m]; + + p += width_in; + } + + *q++ = (uint16_t)(sum / patch_size); + } + + for (int i = _real_width; i < _padded_width; ++i) + *q++ = 0; + } + + for (int j = _real_height; j < _padded_height; ++j) + { + for (int i = 0; i < _padded_width; ++i) + *q++ = 0; + } + } + break; + + default: + break; + } + } +} + diff --git a/src/proc/decimation-filter.h b/src/proc/decimation-filter.h index fff6bcc000..49fd0c17f0 100644 --- a/src/proc/decimation-filter.h +++ b/src/proc/decimation-filter.h @@ -27,14 +27,17 @@ namespace librealsense void update_output_profile(const rs2::frame& f); uint8_t _decimation_factor; + uint8_t _control_val; uint8_t _patch_size; uint8_t _kernel_size; rs2::stream_profile _source_stream_profile; rs2::stream_profile _target_stream_profile; + std::map, rs2::stream_profile> _registered_profiles; uint16_t _real_width; // Number of rows/columns with real datain the decimated image uint16_t _real_height; // Correspond to w,h in the reference code uint16_t _padded_width; // Corresponds to w4/h4 in the reference code uint16_t _padded_height; bool _recalc_profile; + bool _options_changed; // Tracking changes imposed by user }; } diff --git a/src/proc/pointcloud.cpp b/src/proc/pointcloud.cpp index 10fe5d0363..76333b6747 100644 --- a/src/proc/pointcloud.cpp +++ b/src/proc/pointcloud.cpp @@ -406,7 +406,6 @@ namespace librealsense void pointcloud::process_depth_frame(const rs2::depth_frame& depth) { - frame_holder res = get_source().allocate_points(_output_stream, (frame_interface*)depth.get()); auto pframe = (points*)(res.frame);