Add Max Usable Range feature

common/model-views.cpp

@@ -527,18 +527,19 @@ namespace rs2
                 if (ImGui::Checkbox(label.c_str(), &bool_value))
                 {
                     res = true;
-                    value = bool_value ? 1.0f : 0.0f;
+                    model.add_log(to_string() << "Setting " << opt << " to "
+                        << (bool_value? "1.0" : "0.0") << " (" << (bool_value ? "ON" : "OFF") << ")");
                     try
                     {
-                        model.add_log(to_string() << "Setting " << opt << " to "
-                            << value << " (" << (bool_value ? "ON" : "OFF") << ")");
-                        endpoint->set_option(opt, value);
-                        *invalidate_flag = true;
+                        endpoint->set_option(opt, bool_value ? 1.f : 0.f);
                     }
                     catch (const error& e)
                     {
                         error_message = error_to_string(e);
                     }
+                    // Only update the cached value once set_option is done! That way, if it doesn't change anything...
+                    try { value = endpoint->get_option(opt); } catch( ... ) {}
+                    *invalidate_flag = true;
                 }
                 if (ImGui::IsItemHovered() && desc)
                 {
@@ -2883,29 +2884,65 @@ namespace rs2
                 ss << " 0x" << std::hex << static_cast<int>(round(val)) << " =";
             }
 
+            bool show_max_range = false;
             if (texture->get_last_frame().is<depth_frame>())
             {
-                auto meters = texture->get_last_frame().as<depth_frame>().get_distance(x, y);
+                auto meters = texture->get_last_frame().as<depth_frame>().get_distance(x, y);             
 
                 if (viewer.metric_system)
                     ss << std::dec << " " << std::setprecision(3) << meters << " meters";
                 else
                     ss << std::dec << " " << std::setprecision(3) << meters / FEET_TO_METER << " feet";
+
+                auto ds = sensor_from_frame(texture->get_last_frame())->as<depth_sensor>();
+
+                if (viewer.draw_max_usable_range)
+                {
+                    if (ds.supports(RS2_OPTION_ENABLE_MAX_USABLE_RANGE))
+                    {
+                        if (ds.get_option(RS2_OPTION_ENABLE_MAX_USABLE_RANGE) == 1.0)
+                        {
+                            show_max_range = true;
+                            auto mur_sensor = ds.as<max_usable_range_sensor>();
+                            auto max_usable_range = mur_sensor.get_max_usable_depth_range();
+                            const float MIN_RANGE = 3.0f;
+                            const float MAX_RANGE = 9.0f;
+                            // display maximu, usable range in range 3-9 [m] at 1 [m] resolution (rounded)
+                            auto max_usable_range_rounded = round(std::min(std::max(max_usable_range, MIN_RANGE), MAX_RANGE));
+
+                            if (viewer.metric_system)
+                                ss << std::dec << "\nMax usable range: " << std::setprecision(1) << max_usable_range_rounded << " meters";
+                            else
+                                ss << std::dec << "\nMax usable range: " << std::setprecision(1) << max_usable_range_rounded / FEET_TO_METER << " feet";
+                        }
+                    }
+                }
             }
 
             std::string msg(ss.str().c_str());
 
             ImGui_ScopePushFont(font);
 
             // adjust windows size to the message length
-
+            auto new_line_start_idx = msg.find_first_of('\n');
+            int footer_vertical_size = 35;
             auto width = float(msg.size() * 8);
+
+            // adjust width according to the longest line
+            if (show_max_range)
+            {
+                footer_vertical_size = 50;
+                auto first_line_size = msg.find_first_of('\n') + 1;
+                auto second_line_size = msg.substr(new_line_start_idx).size();
+                width = std::max(first_line_size, second_line_size) * 8;
+            }
+
             auto align = 20;
             width += align - (int)width % align;
 
-            ImVec2 pos{ stream_rect.x + 5, stream_rect.y + stream_rect.h - 35 };
+            ImVec2 pos{ stream_rect.x + 5, stream_rect.y + stream_rect.h - footer_vertical_size };
             ImGui::GetWindowDrawList()->AddRectFilled({ pos.x, pos.y },
-                { pos.x + width, pos.y + 30 }, ImColor(dark_sensor_bg));
+                { pos.x + width, pos.y + footer_vertical_size - 5 }, ImColor(dark_sensor_bg));
 
             ImGui::SetCursorScreenPos({ pos.x + 10, pos.y + 5 });
 
@@ -6169,6 +6206,11 @@ namespace rs2
                             {
                                 if (serialize && opt == RS2_OPTION_VISUAL_PRESET)
                                     continue;
+                                if (opt == RS2_OPTION_ENABLE_MAX_USABLE_RANGE && !viewer.draw_max_usable_range)
+                                {
+                                    continue;
+                                }
+
                                 if (sub->draw_option(opt, dev.is<playback>() || update_read_only_options, error_message, *viewer.not_model))
                                 {
                                     get_curr_advanced_controls = true;

common/viewer.h

@@ -188,6 +188,8 @@ namespace rs2
 
         std::shared_ptr<updates_model> updates;
 
+        bool draw_max_usable_range = true;
+
     private:
 
         void check_permissions();

include/librealsense2/h/rs_option.h

@@ -103,6 +103,7 @@ extern "C" {
         RS2_OPTION_SEQUENCE_SIZE, /**< HDR Sequence size */
         RS2_OPTION_SEQUENCE_ID, /**< HDR Sequence ID - 0 is not HDR; sequence ID for HDR configuration starts from 1 */
         RS2_OPTION_HUMIDITY_TEMPERATURE, /**< Humidity temperature [Deg Celsius]*/
+        RS2_OPTION_ENABLE_MAX_USABLE_RANGE, /**< Turn on/off the maximum usable depth sensor range given the amount of ambient light in the scene */
         RS2_OPTION_COUNT /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */
     } rs2_option;
 

include/librealsense2/h/rs_sensor.h

@@ -647,6 +647,12 @@ void rs2_reset_sensor_calibration( rs2_sensor const * sensor, rs2_error** error
 */
 void rs2_set_motion_device_intrinsics(const rs2_sensor* sensor, const rs2_stream_profile* profile, const rs2_motion_device_intrinsic* intrinsics, rs2_error** error);
 
+/** When called on a depth sensor, this method will return the maximum range of the camera given the amount of ambient light in the scene
+* \param[in] sensor      depth sensor
+* \param[out] error      if non-null, receives any error that occurs during this call, otherwise, errors are ignored
+* \return                the max usable range in meters
+*/
+float rs2_get_max_usable_depth_range(rs2_sensor const * sensor, rs2_error** error);
 
 #ifdef __cplusplus
 }

include/librealsense2/h/rs_types.h

@@ -212,6 +212,7 @@ typedef enum rs2_extension
     RS2_EXTENSION_CALIBRATED_SENSOR,
     RS2_EXTENSION_HDR_MERGE,
     RS2_EXTENSION_SEQUENCE_ID_FILTER,
+    RS2_EXTENSION_MAX_USABLE_RANGE_SENSOR,
     RS2_EXTENSION_COUNT
 } rs2_extension;
 const char* rs2_extension_type_to_string(rs2_extension type);

include/librealsense2/hpp/rs_sensor.hpp

@@ -724,5 +724,33 @@ namespace rs2
             error::handle( e );
         }
     };
+
+    class max_usable_range_sensor : public sensor
+    {
+    public:
+        max_usable_range_sensor(sensor s)
+            : sensor(s.get())
+        {
+            rs2_error* e = nullptr;
+            if (rs2_is_sensor_extendable_to(_sensor.get(), RS2_EXTENSION_MAX_USABLE_RANGE_SENSOR, &e) == 0 && !e)
+            {
+                _sensor.reset();
+            }
+            error::handle(e);
+        }
+
+        operator bool() const { return _sensor.get() != nullptr; }
+
+        /** Retrieves the maximum range of the camera given the amount of ambient light in the scene.
+        * \return max usable range in meters
+        */
+        float get_max_usable_depth_range() const
+        {
+            rs2_error* e = nullptr;
+            auto res = rs2_get_max_usable_depth_range(_sensor.get(), &e);
+            error::handle(e);
+            return res;
+        }
+    };
 }
 #endif // LIBREALSENSE_RS2_SENSOR_HPP

src/CMakeLists.txt

@@ -132,4 +132,5 @@ target_sources(${LRS_TARGET}
         "${CMAKE_CURRENT_LIST_DIR}/calibrated-sensor.h"
         "${CMAKE_CURRENT_LIST_DIR}/serializable-interface.h"
         "${CMAKE_CURRENT_LIST_DIR}/depth-to-rgb-calibration.h"
+        "${CMAKE_CURRENT_LIST_DIR}/max-usable-range-sensor.h"
 )

src/algo/CMakeLists.txt

@@ -3,3 +3,4 @@
 
 include(${CMAKE_CURRENT_LIST_DIR}/depth-to-rgb-calibration/CMakeLists.txt)
 include(${CMAKE_CURRENT_LIST_DIR}/thermal-loop/CMakeLists.txt)
+include(${CMAKE_CURRENT_LIST_DIR}/max-usable-range/CMakeLists.txt)

src/algo/max-usable-range/CMakeLists.txt

@@ -0,0 +1,7 @@
+# License: Apache 2.0. See LICENSE file in root directory.
+# Copyright(c) 2020 Intel Corporation. All Rights Reserved.
+target_sources(${LRS_TARGET}
+    PRIVATE
+        "${CMAKE_CURRENT_LIST_DIR}/l500/max-usable-range.h"
+        "${CMAKE_CURRENT_LIST_DIR}/l500/max-usable-range.cpp"
+)

src/algo/max-usable-range/l500/max-usable-range.cpp

@@ -0,0 +1,24 @@
+//// License: Apache 2.0. See LICENSE file in root directory.
+//// Copyright(c) 2020 Intel Corporation. All Rights Reserved.
+
+#include "max-usable-range.h"
+
+// Algo parameters
+static const float PROCESSING_GAIN = 1.75f;
+static const float THERMAL = 74.5f;
+static const float INDOOR_MAX_RANGE = 9.0f;
+
+// Based off of code in RS5-8358
+float librealsense::algo::max_usable_range::l500::max_usable_range(float nest)
+{
+    const float normalized_nest = nest / 16.0f;
+
+    auto expected_max_range = INDOOR_MAX_RANGE;
+
+    if (normalized_nest > THERMAL)
+    {
+        expected_max_range = 31000.0f * pow(normalized_nest, -2.0f) * PROCESSING_GAIN;
+    }
+
+    return expected_max_range;
+}

src/algo/max-usable-range/l500/max-usable-range.h

@@ -0,0 +1,22 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2020 Intel Corporation. All Rights Reserved.
+
+#pragma once
+
+#include <types.h>
+
+
+namespace librealsense {
+namespace algo {
+namespace max_usable_range {
+namespace l500 {
+
+// Calculate the maximum usable range based on current ambient light conditions
+// Input: noise estimation value
+// Output: maximum usable range [m]
+float max_usable_range( float nest );
+
+}  // namespace l500
+}  // namespace max_range
+}  // namespace algo
+}  // namespace librealsense

src/l500/l500-depth.cpp

@@ -20,6 +20,7 @@
 #include "ac-trigger.h"
 #include "algo/depth-to-rgb-calibration/debug.h"
 #include "algo/depth-to-rgb-calibration/utils.h"  // validate_dsm_params
+#include "algo/max-usable-range/l500/max-usable-range.h" 
 
 
 #define MM_TO_METER 1/1000
@@ -354,6 +355,46 @@ namespace librealsense
         AC_LOG( INFO, "Depth sensor calibration has been reset" );
     }
 
+    float l500_depth_sensor::get_max_usable_depth_range() const
+    {
+        using namespace algo::max_usable_range;
+
+        if( !supports_option( RS2_OPTION_ENABLE_MAX_USABLE_RANGE) )
+            throw librealsense::wrong_api_call_sequence_exception( "max usable range option is not supported" );
+
+        if( get_option( RS2_OPTION_ENABLE_MAX_USABLE_RANGE).query() != 1.0f )
+            throw librealsense::wrong_api_call_sequence_exception( "max usable range option is not on" );
+
+        if( ! is_streaming() )
+        {
+            throw librealsense::wrong_api_call_sequence_exception("depth sensor is not streaming!");
+        }
+
+       float nest = static_cast<float>(_owner->get_temperatures().nest_avg);
+
+       return l500::max_usable_range(nest);
+    }
+
+    // We want to disable max-usable-range when not in a particular preset:
+    bool l500_depth_sensor::is_max_range_preset() const
+    {
+        auto res = _owner->_hw_monitor->send(command(ivcam2::IRB, 0x6C, 0x2, 0x1));
+
+        if (res.size() < sizeof(uint8_t))
+        {
+            throw invalid_value_exception(
+                to_string() << "Gain trim FW command failed: size expected: " << sizeof(uint8_t)
+                << " , size received: " << res.size());
+        }
+
+        int gtr = static_cast<int>(res[0]);
+        int apd = static_cast<int>(get_option(RS2_OPTION_AVALANCHE_PHOTO_DIODE).query());
+        int laser_power = static_cast<int>(get_option(RS2_OPTION_LASER_POWER).query());
+        int max_laser_power = static_cast<int>(get_option(RS2_OPTION_LASER_POWER).get_range().max);
+
+        return ((apd == 9) && (gtr == 0) && (laser_power == max_laser_power)); // indicates max_range preset
+    }
+
 
     float l500_depth_sensor::read_baseline() const
     {

src/l500/l500-depth.h

@@ -18,10 +18,10 @@
 #include "error-handling.h"
 #include "l500-options.h"
 #include "calibrated-sensor.h"
+#include "max-usable-range-sensor.h"
 
 namespace librealsense
 {
-
     class l500_depth : public virtual l500_device
     {
     public:
@@ -92,6 +92,7 @@ namespace librealsense
         , public virtual depth_sensor
         , public virtual l500_depth_sensor_interface
         , public calibrated_sensor
+        , public max_usable_range_sensor
     {
     public:
         explicit l500_depth_sensor(
@@ -218,6 +219,8 @@ namespace librealsense
             return *_owner->_calib_table;
         }
 
+        float get_max_usable_depth_range() const override;
+
         void create_snapshot(std::shared_ptr<depth_sensor>& snapshot) const override
         {
             snapshot = std::make_shared<depth_sensor_snapshot>(get_depth_scale());
@@ -254,7 +257,10 @@ namespace librealsense
         void open(const stream_profiles& requests) override;
         void stop() override;
         float get_depth_offset() const;
+        bool is_max_range_preset() const;
+
     private:
+
         action_delayer _action_delayer;
         l500_device * const _owner;
         float _depth_units;

src/l500/l500-device.cpp

@@ -210,6 +210,9 @@ namespace librealsense
 
         if( _fw_version >= firmware_version( "1.5.0.0" ) )
         {
+            auto enable_max_usable_range = std::make_shared<max_usable_range_option>(this);
+            depth_sensor.register_option(RS2_OPTION_ENABLE_MAX_USABLE_RANGE, enable_max_usable_range);
+
             // TODO may not need auto-cal if there's no color sensor, like on the rs500...
             _autocal = std::make_shared< ac_trigger >( *this, _hw_monitor );