temporal-filter.cpp

Go to the documentation of this file.

// 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 "source.h"
#include "option.h"
#include "environment.h"
#include "context.h"
#include "proc/synthetic-stream.h"
#include "proc/temporal-filter.h"

namespace librealsense
{
    const size_t PERSISTENCE_MAP_NUM = 9;

    // The persistence parameter/holes filling mode
    const uint8_t persistence_min = 0;
    const uint8_t persistence_max = PERSISTENCE_MAP_NUM-1;
    const uint8_t persistence_default = 3;  // Credible if two of the last four frames are valid at this pixel
    const uint8_t persistence_step = 1;

    //alpha -  the weight with default value 0.4, between 1 and 0 -- 1 means 100% weight from the current pixel
    const float temp_alpha_min = 0.f;
    const float temp_alpha_max = 1.f;
    const float temp_alpha_default = 0.4f;
    const float temp_alpha_step = 0.01f;

    //delta -  the filter threshold for edge classification and preserving with default value of 20 depth increments
    const uint8_t temp_delta_min = 1;
    const uint8_t temp_delta_max = 100;
    const uint8_t temp_delta_default = 20;
    const uint8_t temp_delta_step = 1;

    temporal_filter::temporal_filter() :
        depth_processing_block("Temporal Filter"),
        _persistence_param(persistence_default),
        _alpha_param(temp_alpha_default),
        _one_minus_alpha(1- _alpha_param),
        _delta_param(temp_delta_default),
        _width(0), _height(0), _stride(0), _bpp(0),
        _extension_type(RS2_EXTENSION_DEPTH_FRAME),
        _current_frm_size_pixels(0)
    {
        _stream_filter.stream = RS2_STREAM_DEPTH;
        _stream_filter.format = RS2_FORMAT_Z16;

        auto temporal_persistence_control = std::make_shared<ptr_option<uint8_t>>(
            persistence_min,
            persistence_max,
            persistence_step,
            persistence_default,
            &_persistence_param, "Persistency mode");

        temporal_persistence_control->set_description(0, "Disabled");
        temporal_persistence_control->set_description(1, "Valid in 8/8");
        temporal_persistence_control->set_description(2, "Valid in 2/last 3");
        temporal_persistence_control->set_description(3, "Valid in 2/last 4");
        temporal_persistence_control->set_description(4, "Valid in 2/8");
        temporal_persistence_control->set_description(5, "Valid in 1/last 2");
        temporal_persistence_control->set_description(6, "Valid in 1/last 5");
        temporal_persistence_control->set_description(7, "Valid in 1/8");
        temporal_persistence_control->set_description(8, "Always on");

        temporal_persistence_control->on_set([this,temporal_persistence_control](float val)
        {
            if (!temporal_persistence_control->is_valid(val))
                throw invalid_value_exception(to_string()
                    << "Unsupported temporal persistence param "
                    << (int)val << " is out of range.");

            on_set_persistence_control(static_cast<uint8_t>(val));
        });

        register_option(RS2_OPTION_HOLES_FILL, temporal_persistence_control);

        auto temporal_filter_alpha = std::make_shared<ptr_option<float>>(
            temp_alpha_min,
            temp_alpha_max,
            temp_alpha_step,
            temp_alpha_default,
            &_alpha_param, "Alpha factor of Exp. moving average, 1=no filter, 0=infinite filter");
        temporal_filter_alpha->on_set([this](float val)
        {
            on_set_alpha(val);
        });

        auto temporal_filter_delta = std::make_shared<ptr_option<uint8_t>>(
            temp_delta_min,
            temp_delta_max,
            temp_delta_step,
            temp_delta_default,
            &_delta_param, "Edge-preserving (gradient) threshold");
        temporal_filter_delta->on_set([this, temporal_filter_delta](float val)
        {
            if (!temporal_filter_delta->is_valid(val))
                throw invalid_value_exception(to_string()
                    << "Unsupported temporal delta: " << val << " is out of range.");

            on_set_delta(val);
        });

        register_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, temporal_filter_alpha);
        register_option(RS2_OPTION_FILTER_SMOOTH_DELTA, temporal_filter_delta);

        on_set_persistence_control(_persistence_param);
        on_set_delta(_delta_param);
        on_set_alpha(_alpha_param);
    }

    rs2::frame temporal_filter::process_frame(const rs2::frame_source& source, const rs2::frame& f)
    {
        update_configuration(f);
        auto tgt = prepare_target_frame(f, source);

        // Temporal filter execution
        if (_extension_type == RS2_EXTENSION_DISPARITY_FRAME)
            temp_jw_smooth<float>(const_cast<void*>(tgt.get_data()), _last_frame.data(), _history.data());
        else
            temp_jw_smooth<uint16_t>(const_cast<void*>(tgt.get_data()), _last_frame.data(), _history.data());

        return tgt;
    }


    void temporal_filter::on_set_persistence_control(uint8_t val)
    {
        std::lock_guard<std::mutex> lock(_mutex);
        _persistence_param = val;
        recalc_persistence_map();
        _last_frame.clear();
        _history.clear();
    }

    void temporal_filter::on_set_alpha(float val)
    {
        std::lock_guard<std::mutex> lock(_mutex);
        _alpha_param = val;
        _one_minus_alpha = 1.f - _alpha_param;
        _cur_frame_index = 0;
        _last_frame.clear();
        _history.clear();
    }

    void temporal_filter::on_set_delta(float val)
    {
        std::lock_guard<std::mutex> lock(_mutex);
        _delta_param = static_cast<uint8_t>(val);
        _cur_frame_index = 0;
        _last_frame.clear();
        _history.clear();
    }

    void  temporal_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());

            //TODO - reject any frame other than depth/disparity
            _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;

            _last_frame.clear();
            _last_frame.resize(_current_frm_size_pixels*_bpp);

            _history.clear();
            _history.resize(_current_frm_size_pixels*_bpp);

        }
    }

    rs2::frame temporal_filter::prepare_target_frame(const rs2::frame& f, const rs2::frame_source& source)
    {
        // Allocate and copy the content of the original Depth 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;
    }

    void temporal_filter::recalc_persistence_map()
    {
        _persistence_map.fill(0);

        for (size_t i = 0; i < _persistence_map.size(); i++)
        {
            unsigned char last_7 = !!(i & 1);  // old
            unsigned char last_6 = !!(i & 2);
            unsigned char last_5 = !!(i & 4);
            unsigned char last_4 = !!(i & 8);
            unsigned char last_3 = !!(i & 16);
            unsigned char last_2 = !!(i & 32);
            unsigned char last_1 = !!(i & 64);
            unsigned char lastFrame = !!(i & 128); // new

            if (_persistence_param == 1)
            {
                int sum = lastFrame + last_1 + last_2 + last_3 + last_4 + last_5 + last_6 + last_7;
                if (sum >= 8)  // valid in eight of the last eight frames
                    _persistence_map[i] = 1;
            }
            else if (_persistence_param == 2) // <--- default choice in current libRS implementation
            {
                int sum = lastFrame + last_1 + last_2;
                if (sum >= 2) // valid in two of the last three frames
                    _persistence_map[i] = 1;
            }
            else if (_persistence_param == 3) // <--- default choice recommended
            {
                int sum = lastFrame + last_1 + last_2 + last_3;
                if (sum >= 2)  // valid in two of the last four frames
                    _persistence_map[i] = 1;
            }
            else if (_persistence_param == 4)
            {
                int sum = lastFrame + last_1 + last_2 + last_3 + last_4 + last_5 + last_6 + last_7;
                if (sum >= 2) // valid in two of the last eight frames
                    _persistence_map[i] = 1;
            }
            else if (_persistence_param == 5)
            {
                int sum = lastFrame + last_1;
                if (sum >= 1) // valid in one of the last two frames
                    _persistence_map[i] = 1;
            }
            else if (_persistence_param == 6)
            {
                int sum = lastFrame + last_1 + last_2 + last_3 + last_4;
                if (sum >= 1)  // valid in one of the last five frames
                    _persistence_map[i] = 1;
            }
            else if (_persistence_param == 7) //  <--- most filling
            {
                int sum = lastFrame + last_1 + last_2 + last_3 + last_4 + last_5 + last_6 + last_7;
                if (sum >= 1) // valid in one of the last eight frames
                    _persistence_map[i] = 1;
            }
            else if (_persistence_param == 8) //  <--- all 1's
            {
                _persistence_map[i] = 1;
            }
            else // all others, including 0, no persistance
            {
            }
        }

        // Convert to credible enough
        std::array<uint8_t, PRESISTENCY_LUT_SIZE> credible_threshold;
        credible_threshold.fill(0);

        for (auto phase = 0; phase < 8; phase++)
        {
            // evaluating last phase
            //int ephase = (phase + 7) % 8;
            unsigned char mask = 1 << phase;
            int i;

            for (i = 0; i < 256; i++) {
                unsigned char pos = (unsigned char)((i << (8 - phase)) | (i >> phase));
                if (_persistence_map[pos])
                    credible_threshold[i] |= mask;
            }
        }
        // Store results
        _persistence_map = credible_threshold;
    }
}