<commit_before>// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2019 Intel Corporation. All Rights Reserved.

#include "zero-order.h"
#include <iomanip>
#include "l500/l500-depth.h"

const double METER_TO_MM = 1000;

namespace librealsense
{
    enum zero_order_invalidation_options
    {
        RS2_OPTION_FILTER_ZO_IR_THRESHOLD = static_cast<rs2_option>(RS2_OPTION_COUNT + 0), /**< IR min threshold used by zero order filter */
        RS2_OPTION_FILTER_ZO_RTD_HIGH_THRESHOLD = static_cast<rs2_option>(RS2_OPTION_COUNT + 1), /**< RTD high threshold used by zero order filter */
        RS2_OPTION_FILTER_ZO_RTD_LOW_THRESHOLD = static_cast<rs2_option>(RS2_OPTION_COUNT + 2), /**< RTD low threshold used by zero order filter */
        RS2_OPTION_FILTER_ZO_BASELINE = static_cast<rs2_option>(RS2_OPTION_COUNT + 3), /**< baseline of camera used by zero order filter */
        RS2_OPTION_FILTER_ZO_PATCH_SIZE = static_cast<rs2_option>(RS2_OPTION_COUNT + 4), /**< patch size used by zero order filter */
        RS2_OPTION_FILTER_ZO_MAX_VALUE = static_cast<rs2_option>(RS2_OPTION_COUNT + 5), /**< z max value used by zero order filter */
        RS2_OPTION_FILTER_ZO_IR_MIN_VALUE = static_cast<rs2_option>(RS2_OPTION_COUNT + 6), /**< ir min value used by zero order filter */
        RS2_OPTION_FILTER_ZO_THRESHOLD_OFFSET = static_cast<rs2_option>(RS2_OPTION_COUNT + 7), /**< threshold offset used by zero order filter */
        RS2_OPTION_FILTER_ZO_THRESHOLD_SCALE = static_cast<rs2_option>(RS2_OPTION_COUNT + 8) /**< threshold scale used by zero order filter */
    };

    double get_pixel_rtd(const rs2::vertex& v, int baseline)
    {
        auto x = (double)v.x*METER_TO_MM;
        auto y = (double)v.y*METER_TO_MM;
        auto z = (double)v.z*METER_TO_MM;
       
        auto rtd = sqrt(x*x + y*y + z*z) + sqrt((x - baseline) *(x - baseline) + y*y + z*z);
        return v.z ? rtd : 0;
    }

    void z2rtd(const rs2::vertex* vertices, double* rtd, const rs2_intrinsics& intrinsics, int baseline)
    {
        for (auto i = 0;i < intrinsics.height*intrinsics.width; i++)
        {
            rtd[i] = get_pixel_rtd(vertices[i], baseline);           
        }
    }
    
    template<typename T>
    std::vector <T> get_zo_point_values(const T* frame_data_in, const rs2_intrinsics& intrinsics, int zo_point_x, int zo_point_y, int patch_r)
    {
        std::vector<T> values;
        values.reserve((patch_r + 2UL) *(patch_r + 2UL));

        for (auto i = zo_point_y - 1 - patch_r; i <= (zo_point_y + patch_r) && i < intrinsics.height; i++)
        {
            for (auto j = (zo_point_x - 1 - patch_r); j <= (zo_point_x + patch_r) && i < intrinsics.width; j++)
            {
                values.push_back(frame_data_in[i*intrinsics.width + j]);
            }
        }

        return values;
    }

    template<typename T>
    T get_zo_point_value(std::vector<T>& values)
    {
        std::sort(values.begin(), values.end());

        if ((values.size()) % 2 == 0)
        {
            return (values[values.size() / 2 - 1] + values[values.size() / 2]) / 2;
        }
        else if (values.size() > 0)
            return values[values.size() / 2];

        return 0;
    }

    bool try_get_zo_rtd_ir_point_values(const double* rtd, const uint16_t* depth_data_in, const uint8_t* ir_data, 
        const rs2_intrinsics& intrinsics, const zero_order_options& options, int zo_point_x, int zo_point_y,
        double *rtd_zo_value, uint8_t* ir_zo_data)
    {
        if (zo_point_x - options.patch_size < 0 || zo_point_x + options.patch_size >= intrinsics.width ||
            zo_point_y - options.patch_size < 0 || zo_point_y + options.patch_size >= intrinsics.height)
            return false;

        auto values_rtd = get_zo_point_values(rtd, intrinsics, zo_point_x, zo_point_y, options.patch_size);
        auto values_ir = get_zo_point_values(ir_data, intrinsics, zo_point_x, zo_point_y, options.patch_size);
        auto values_z = get_zo_point_values(depth_data_in, intrinsics, zo_point_x, zo_point_y, options.patch_size);

        for (auto i = 0; i < values_rtd.size(); i++)
        {
            if ((values_z[i] / 8.0) > options.z_max || (values_ir[i] < options.ir_min))
            {
                values_rtd[i] = 0;  
                values_ir[i] = 0;
            }       
        }

        values_rtd.erase(std::remove_if(values_rtd.begin(), values_rtd.end(), [](double val)
        {
            return val == 0;
        }), values_rtd.end());

        values_ir.erase(std::remove_if(values_ir.begin(), values_ir.end(), [](uint8_t val)
        {
            return val == 0;
        }), values_ir.end());

        if (values_rtd.empty() || values_ir.empty())
            return false;

        *rtd_zo_value = get_zo_point_value(values_rtd);
        *ir_zo_data = get_zo_point_value(values_ir);

        return true;
    }

    template<class T>
    void detect_zero_order(const double * rtd, const uint16_t* depth_data_in, const uint8_t* ir_data, T zero_pixel,
       const rs2_intrinsics& intrinsics, const zero_order_options& options,
       double zo_value, uint8_t iro_value)
    {
        const double ir_dynamic_range = 256.0;

        double r = std::exp((ir_dynamic_range / 2.0 + options.threshold_offset - iro_value) / (double)options.threshold_scale);

        double res = (1.0 + r);
        double i_threshold_relative = options.ir_threshold / res;
        for (auto i = 0; i < intrinsics.height*intrinsics.width; i++)
        {
            double rtd_val = rtd[i];
            uint8_t ir_val = ir_data[i];

            bool zero = (depth_data_in[i] > 0) && 
                        (ir_val < i_threshold_relative) &&
                        (rtd_val > (zo_value - options.rtd_low_threshold)) && 
                        (rtd_val < (zo_value + options.rtd_high_threshold));

            zero_pixel(i, zero);
        }
    }

    template<class T>
    bool zero_order_invalidation(const uint16_t * depth_data_in, const uint8_t * ir_data, T zero_pixel,
        const rs2::vertex* vertices,
        rs2_intrinsics intrinsics,
        const zero_order_options& options, int zo_point_x, int zo_point_y)
    {
        std::vector<double> rtd(size_t(intrinsics.height)*intrinsics.width);
        z2rtd(vertices, rtd.data(), intrinsics, int(options.baseline));
        double rtd_zo_value; // \\todo Not sure why try_get_zo_rtd_ir_point_values() returns a double and detect_zero_order() uses a float. Which is best representation?
        uint8_t ir_zo_value;

        if (try_get_zo_rtd_ir_point_values(rtd.data(), depth_data_in, ir_data, intrinsics, 
            options,zo_point_x, zo_point_y, &rtd_zo_value, &ir_zo_value))
        {
            detect_zero_order(rtd.data(), depth_data_in, ir_data, zero_pixel, intrinsics,
                options, float(rtd_zo_value), ir_zo_value);
            return true;
        }
        return false;
    }

    zero_order::zero_order(std::shared_ptr<bool_option> is_enabled_opt)
       : generic_processing_block("Zero Order Fix"), _first_frame(true), _is_enabled_opt(is_enabled_opt),
        _resolutions_depth { 0 }
    {
        auto ir_threshold = std::make_shared<ptr_option<uint8_t>>(
            0,
            255,
            1,
            115,
            &_options.ir_threshold,
            "IR threshold");
        ir_threshold->on_set([ir_threshold](float val)
        {
            if (!ir_threshold->is_valid(val))
                throw invalid_value_exception(to_string()
                    << "Unsupported ir threshold " << val << " is out of range.");
        });

        register_option(static_cast<rs2_option>(RS2_OPTION_FILTER_ZO_IR_THRESHOLD), ir_threshold);

        auto rtd_high_threshold = std::make_shared<ptr_option<uint16_t>>(
            0,
            400,
            1,
            200,<commit_msg>Remove the \\todo comment. Just use doubles. Removed an unnecssary float cast. Use +2ULL in the values.reserve((patch_r + 2UL) *(patch_r + 2UL)) line. This promotes each operand to a unsigned long long which matches the size_t used by reserved() and avoids compiler warnings.<commit_after>// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2019 Intel Corporation. All Rights Reserved.

#include "zero-order.h"
#include <iomanip>
#include "l500/l500-depth.h"

const double METER_TO_MM = 1000;

namespace librealsense
{
    enum zero_order_invalidation_options
    {
        RS2_OPTION_FILTER_ZO_IR_THRESHOLD = static_cast<rs2_option>(RS2_OPTION_COUNT + 0), /**< IR min threshold used by zero order filter */
        RS2_OPTION_FILTER_ZO_RTD_HIGH_THRESHOLD = static_cast<rs2_option>(RS2_OPTION_COUNT + 1), /**< RTD high threshold used by zero order filter */
        RS2_OPTION_FILTER_ZO_RTD_LOW_THRESHOLD = static_cast<rs2_option>(RS2_OPTION_COUNT + 2), /**< RTD low threshold used by zero order filter */
        RS2_OPTION_FILTER_ZO_BASELINE = static_cast<rs2_option>(RS2_OPTION_COUNT + 3), /**< baseline of camera used by zero order filter */
        RS2_OPTION_FILTER_ZO_PATCH_SIZE = static_cast<rs2_option>(RS2_OPTION_COUNT + 4), /**< patch size used by zero order filter */
        RS2_OPTION_FILTER_ZO_MAX_VALUE = static_cast<rs2_option>(RS2_OPTION_COUNT + 5), /**< z max value used by zero order filter */
        RS2_OPTION_FILTER_ZO_IR_MIN_VALUE = static_cast<rs2_option>(RS2_OPTION_COUNT + 6), /**< ir min value used by zero order filter */
        RS2_OPTION_FILTER_ZO_THRESHOLD_OFFSET = static_cast<rs2_option>(RS2_OPTION_COUNT + 7), /**< threshold offset used by zero order filter */
        RS2_OPTION_FILTER_ZO_THRESHOLD_SCALE = static_cast<rs2_option>(RS2_OPTION_COUNT + 8) /**< threshold scale used by zero order filter */
    };

    double get_pixel_rtd(const rs2::vertex& v, int baseline)
    {
        auto x = (double)v.x*METER_TO_MM;
        auto y = (double)v.y*METER_TO_MM;
        auto z = (double)v.z*METER_TO_MM;
       
        auto rtd = sqrt(x*x + y*y + z*z) + sqrt((x - baseline) *(x - baseline) + y*y + z*z);
        return v.z ? rtd : 0;
    }

    void z2rtd(const rs2::vertex* vertices, double* rtd, const rs2_intrinsics& intrinsics, int baseline)
    {
        for (auto i = 0;i < intrinsics.height*intrinsics.width; i++)
        {
            rtd[i] = get_pixel_rtd(vertices[i], baseline);           
        }
    }
    
    template<typename T>
    std::vector <T> get_zo_point_values(const T* frame_data_in, const rs2_intrinsics& intrinsics, int zo_point_x, int zo_point_y, int patch_r)
    {
        std::vector<T> values;
        values.reserve((patch_r + 2ULL) *(patch_r + 2ULL));

        for (auto i = zo_point_y - 1 - patch_r; i <= (zo_point_y + patch_r) && i < intrinsics.height; i++)
        {
            for (auto j = (zo_point_x - 1 - patch_r); j <= (zo_point_x + patch_r) && i < intrinsics.width; j++)
            {
                values.push_back(frame_data_in[i*intrinsics.width + j]);
            }
        }

        return values;
    }

    template<typename T>
    T get_zo_point_value(std::vector<T>& values)
    {
        std::sort(values.begin(), values.end());

        if ((values.size()) % 2 == 0)
        {
            return (values[values.size() / 2 - 1] + values[values.size() / 2]) / 2;
        }
        else if (values.size() > 0)
            return values[values.size() / 2];

        return 0;
    }

    bool try_get_zo_rtd_ir_point_values(const double* rtd, const uint16_t* depth_data_in, const uint8_t* ir_data, 
        const rs2_intrinsics& intrinsics, const zero_order_options& options, int zo_point_x, int zo_point_y,
        double *rtd_zo_value, uint8_t* ir_zo_data)
    {
        if (zo_point_x - options.patch_size < 0 || zo_point_x + options.patch_size >= intrinsics.width ||
            zo_point_y - options.patch_size < 0 || zo_point_y + options.patch_size >= intrinsics.height)
            return false;

        auto values_rtd = get_zo_point_values(rtd, intrinsics, zo_point_x, zo_point_y, options.patch_size);
        auto values_ir = get_zo_point_values(ir_data, intrinsics, zo_point_x, zo_point_y, options.patch_size);
        auto values_z = get_zo_point_values(depth_data_in, intrinsics, zo_point_x, zo_point_y, options.patch_size);

        for (auto i = 0; i < values_rtd.size(); i++)
        {
            if ((values_z[i] / 8.0) > options.z_max || (values_ir[i] < options.ir_min))
            {
                values_rtd[i] = 0;  
                values_ir[i] = 0;
            }       
        }

        values_rtd.erase(std::remove_if(values_rtd.begin(), values_rtd.end(), [](double val)
        {
            return val == 0;
        }), values_rtd.end());

        values_ir.erase(std::remove_if(values_ir.begin(), values_ir.end(), [](uint8_t val)
        {
            return val == 0;
        }), values_ir.end());

        if (values_rtd.empty() || values_ir.empty())
            return false;

        *rtd_zo_value = get_zo_point_value(values_rtd);
        *ir_zo_data = get_zo_point_value(values_ir);

        return true;
    }

    template<class T>
    void detect_zero_order(const double * rtd, const uint16_t* depth_data_in, const uint8_t* ir_data, T zero_pixel,
       const rs2_intrinsics& intrinsics, const zero_order_options& options,
       double zo_value, uint8_t iro_value)
    {
        const double ir_dynamic_range = 256.0;

        double r = std::exp((ir_dynamic_range / 2.0 + options.threshold_offset - iro_value) / (double)options.threshold_scale);

        double res = (1.0 + r);
        double i_threshold_relative = options.ir_threshold / res;
        for (auto i = 0; i < intrinsics.height*intrinsics.width; i++)
        {
            double rtd_val = rtd[i];
            uint8_t ir_val = ir_data[i];

            bool zero = (depth_data_in[i] > 0) && 
                        (ir_val < i_threshold_relative) &&
                        (rtd_val > (zo_value - options.rtd_low_threshold)) && 
                        (rtd_val < (zo_value + options.rtd_high_threshold));

            zero_pixel(i, zero);
        }
    }

    template<class T>
    bool zero_order_invalidation(const uint16_t * depth_data_in, const uint8_t * ir_data, T zero_pixel,
        const rs2::vertex* vertices,
        rs2_intrinsics intrinsics,
        const zero_order_options& options, int zo_point_x, int zo_point_y)
    {
        std::vector<double> rtd(size_t(intrinsics.height)*intrinsics.width);
        z2rtd(vertices, rtd.data(), intrinsics, int(options.baseline));
        double rtd_zo_value; 
        uint8_t ir_zo_value;

        if (try_get_zo_rtd_ir_point_values(rtd.data(), depth_data_in, ir_data, intrinsics, 
            options,zo_point_x, zo_point_y, &rtd_zo_value, &ir_zo_value))
        {
            detect_zero_order(rtd.data(), depth_data_in, ir_data, zero_pixel, intrinsics,
                options, rtd_zo_value, ir_zo_value);
            return true;
        }
        return false;
    }

    zero_order::zero_order(std::shared_ptr<bool_option> is_enabled_opt)
       : generic_processing_block("Zero Order Fix"), _first_frame(true), _is_enabled_opt(is_enabled_opt),
        _resolutions_depth { 0 }
    {
        auto ir_threshold = std::make_shared<ptr_option<uint8_t>>(
            0,
            255,
            1,
            115,
            &_options.ir_threshold,
            "IR threshold");
        ir_threshold->on_set([ir_threshold](float val)
        {
            if (!ir_threshold->is_valid(val))
                throw invalid_value_exception(to_string()
                    << "Unsupported ir threshold " << val << " is out of range.");
        });

        register_option(static_cast<rs2_option>(RS2_OPTION_FILTER_ZO_IR_THRESHOLD), ir_threshold);

        auto rtd_high_threshold = std::make_shared<ptr_option<uint16_t>>(
            0,
            400,
            1,
            200,
{'fraction_non_alphanumeric': 0.11062646184967444, 'fraction_numerical': 0.016878437322207472, 'mean_word_length': 2.5913734392735526, 'pattern_counts': {'":': 0, '<': 105, '<?xml version=': 0, '>': 67, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '626825', 'n_tokens_mistral': 5567, 'n_tokens_neox': 5309, 'n_words': 1104}