Krgb-thermal support

src/algo/CMakeLists.txt

@@ -2,4 +2,4 @@
 # Copyright(c) 2020 Intel Corporation. All Rights Reserved.
 
 include(${CMAKE_CURRENT_LIST_DIR}/depth-to-rgb-calibration/CMakeLists.txt)
-
+include(${CMAKE_CURRENT_LIST_DIR}/thermal-loop/CMakeLists.txt)

src/algo/depth-to-rgb-calibration/optimizer.cpp

@@ -1557,8 +1557,15 @@ void params::set_depth_resolution( size_t width, size_t height, rs2_ambient_ligh
 {
     AC_LOG( DEBUG, "    depth resolution= " << width << "x" << height );
     // Some parameters are resolution-dependent
-    bool const XGA = (width == 1024 && height == 768);
-    bool const VGA = (width == 640 && height == 480);
+    bool const XGA = ( width == 1024 && height == 768 );
+    bool const VGA = ( width == 640 && height == 480 );
+
+    if( ! XGA && ! VGA )
+    {
+        throw std::runtime_error( to_string() << width << "x" << height
+                                              << " this resolution is not supported" );
+    }
+
     if( XGA )
     {
         AC_LOG( DEBUG, "    changing IR threshold: " << grad_ir_threshold << " -> " << 2.5 << "  (because of resolution)" );
@@ -1601,6 +1608,7 @@ void params::set_depth_resolution( size_t width, size_t height, rs2_ambient_ligh
             }
         }
     }
+
     min_weighted_edge_per_section_depth = 50. * ( 480 * 640 ) / ( width * height );
 }
 
@@ -1615,6 +1623,7 @@ void params::set_rgb_resolution( size_t width, size_t height )
     max_xy_movement_per_calibration[1] = max_xy_movement_per_calibration[2] = 2. * area / hd_area;
     max_xy_movement_from_origin = 20. * area / hd_area;
     min_weighted_edge_per_section_rgb = 0.05 * hd_area / area;
+
 }
 
 calib const & optimizer::get_calibration() const
@@ -1632,35 +1641,14 @@ double optimizer::get_cost() const
     return _params_curr.cost;
 }
 
-static
-void write_to_file( void const * data, size_t cb,
-    std::string const & dir,
-    char const * filename
-)
-{
-    std::string path = dir + filename;
-    std::fstream f( path, std::ios::out | std::ios::binary );
-    if( !f )
-        throw std::runtime_error( "failed to open file:\n" + path );
-    f.write( (char const *) data, cb );
-    f.close();
-}
+
 
 template< typename T >
 void write_obj( std::fstream & f, T const & o )
 {
     f.write( (char const *)&o, sizeof( o ) );
 }
 
-template< typename T >
-void write_vector_to_file( std::vector< T > const & v,
-    std::string const & dir,
-    char const * filename
-)
-{
-    write_to_file( v.data(), v.size() * sizeof( T ), dir, filename );
-}
-
 void write_matlab_camera_params_file(
     rs2_intrinsics const & _intr_depth,
     calib const & rgb_calibration,

src/algo/depth-to-rgb-calibration/optimizer.h

@@ -26,6 +26,7 @@ namespace depth_to_rgb_calibration {
         double step_size = 0;
     };
 
+
     struct params
     {
         params();

src/algo/depth-to-rgb-calibration/utils.cpp

@@ -10,7 +10,17 @@ namespace librealsense {
 namespace algo {
 namespace depth_to_rgb_calibration {
 
-    double get_norma(const std::vector<double3>& vec)
+    void write_to_file( void const * data, size_t cb, std::string const & dir, char const * filename )
+    {
+        std::string path = dir + filename;
+        std::fstream f( path, std::ios::out | std::ios::binary );
+        if( ! f )
+            throw std::runtime_error( "failed to open file:\n" + path );
+        f.write( (char const *)data, cb );
+        f.close();
+    }
+
+    double get_norma( const std::vector< double3 > & vec )
     {
         double sum = 0;
         std::for_each( vec.begin(), vec.end(), [&]( double3 const & v ) { sum += v.get_norm(); } );

src/algo/depth-to-rgb-calibration/utils.h

@@ -4,7 +4,7 @@
 #pragma once
 
 #include "calibration-types.h"
-
+#include <string>
 
 namespace librealsense {
 namespace algo {
@@ -32,6 +32,17 @@ namespace depth_to_rgb_calibration {
     // throw invalid_value_exception if they do.
     void validate_dsm_params( struct rs2_dsm_params const & dsm_params );
 
+    void
+    write_to_file( void const * data, size_t cb, std::string const & dir, char const * filename );
+
+    template < typename T >
+    void write_vector_to_file( std::vector< T > const & v,
+                               std::string const & dir,
+                               char const * filename )
+    {
+        write_to_file( v.data(), v.size() * sizeof( T ), dir, filename );
+    }
+
 }
 }
 }

src/algo/thermal-loop/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-thermal-loop.h"
+        "${CMAKE_CURRENT_LIST_DIR}/l500-thermal-loop.cpp"
+)

src/algo/thermal-loop/l500-thermal-loop.cpp

@@ -0,0 +1,112 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2020 Intel Corporation. All Rights Reserved.
+
+#include "l500-thermal-loop.h"
+#include "../../l500/l500-private.h"
+
+namespace librealsense {
+namespace algo {
+namespace thermal_loop {
+namespace l500 {
+
+
+const int thermal_calibration_table::id = 0x317;
+
+
+thermal_calibration_table::thermal_calibration_table( const std::vector< byte > & data,
+                                                      int resolution )
+    : _resolution( resolution )
+{
+    float const * header_ptr = (float *)( data.data() + sizeof( ivcam2::table_header ) );
+
+    auto expected_size = sizeof( ivcam2::table_header ) + sizeof( thermal_table_header )
+                       + sizeof( thermal_bin ) * resolution;
+
+    if( data.size() != expected_size )
+        throw std::runtime_error( librealsense::to_string()
+                                  << "data size (" << data.size()
+                                  << ") does not meet expected size " << expected_size );
+
+    _header = *(thermal_table_header *)( header_ptr );
+
+    auto data_ptr = (thermal_bin *)( data.data() + sizeof( ivcam2::table_header )
+                                   + sizeof( thermal_table_header ) );
+    bins.assign( data_ptr, data_ptr + resolution );
+}
+
+
+bool operator==( const thermal_calibration_table & lhs, const thermal_calibration_table & rhs )
+{
+    if( lhs.bins.size() != rhs.bins.size() )
+        return false;
+
+    if( lhs._header.max_temp != rhs._header.max_temp || lhs._header.min_temp != rhs._header.min_temp
+        || lhs._header.reference_temp != rhs._header.reference_temp
+        || lhs._header.valid != rhs._header.valid )
+        return false;
+
+    for( auto i = 0; i < rhs.bins.size(); i++ )
+    {
+        if( lhs.bins[i].scale != rhs.bins[i].scale || lhs.bins[i].sheer != rhs.bins[i].sheer
+            || lhs.bins[i].tx != rhs.bins[i].tx || lhs.bins[i].ty != rhs.bins[i].ty )
+            return false;
+    }
+    return true;
+}
+
+
+double thermal_calibration_table::get_thermal_scale( double hum_temp ) const
+{
+    auto scale = bins[_resolution - 1].scale;
+
+    auto temp_range = _header.max_temp - _header.min_temp;
+    // there are 29 bins between min and max temps so 30 equal intervals
+    auto const interval = temp_range / ( _resolution + 1 );
+    // T:   |---|---|---| ... |---|
+    //    min   0   1   2 ... 28  max
+    size_t index = 0;
+    for( double temp = _header.min_temp; index < _resolution;
+            ++index, temp += interval )
+    {
+        auto interval_max = temp + interval;
+        if( hum_temp <= interval_max )
+        {
+            scale = bins[index].scale;
+            break;
+        }
+    }
+
+    // The "scale" is meant to be divided by, but we want something to multiply with!
+    if( scale == 0 )
+        throw std::runtime_error( "invalid 0 scale in thermal table" );
+    return 1. / scale;
+}
+
+
+std::vector< byte > thermal_calibration_table::build_raw_data() const
+{
+    std::vector< float > data;
+    data.resize( sizeof( ivcam2::table_header ) / sizeof( float ), 0 );
+    data.push_back( _header.min_temp );
+    data.push_back( _header.max_temp );
+    data.push_back( _header.reference_temp );
+    data.push_back( _header.valid );
+
+    for( auto i = 0; i < bins.size(); i++ )
+    {
+        data.push_back( bins[i].scale );
+        data.push_back( bins[i].sheer );
+        data.push_back( bins[i].tx );
+        data.push_back( bins[i].ty );
+    }
+
+    std::vector< byte > res;
+    res.assign( (byte *)( data.data() ), (byte *)( data.data() + data.size() ) );
+    return res;
+}
+
+
+}
+}
+}
+}

src/algo/thermal-loop/l500-thermal-loop.h

@@ -0,0 +1,74 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2020 Intel Corporation. All Rights Reserved.
+
+#pragma once
+
+#include <vector>
+#include "../../types.h"
+#include "thermal-calibration-table-interface.h"
+
+namespace librealsense {
+namespace algo {
+namespace thermal_loop {
+namespace l500 {
+
+
+// RGB_Thermal_Info_CalibInfo table
+// -----------------------------------
+//
+// The table contains equally spaced bins between min & max temperature.
+//
+// Each bin has a set of 4 transformation parameters. The transformation maps a point in the RGB
+// image in a given temperature to its expected location in the temperature in which the RGB module
+// was calibrated.
+//
+// Reference at:
+// https://rsconf.intel.com/display/L500/0x317+RGB+Thermal+Table
+//
+#pragma pack( push, 1 )
+class thermal_calibration_table : public thermal_calibration_table_interface
+{
+public:
+    static const int id;
+
+    // The header as it's written raw
+    struct thermal_table_header
+    {
+        float min_temp;
+        float max_temp;
+        float reference_temp;  // not used
+        float valid;           // not used
+    };
+
+    // Each bin data, as it's written in the actual raw table
+    struct thermal_bin
+    {
+        float scale;
+        float sheer; // parameters which affect offset that are not in use
+        float tx;
+        float ty;
+    };
+
+    // Number of bins *between* min and max:
+    //     bin - size = ( max - min ) / ( resolution + 1 )
+    // In the raw calibration table, 29 is implicitly used.
+    size_t _resolution; 
+
+    thermal_table_header _header;
+    std::vector< thermal_bin > bins;
+
+    thermal_calibration_table() = default;
+    thermal_calibration_table( const std::vector< byte > & data, int resolution = 29 );
+
+    double get_thermal_scale( double hum_temp ) const override;
+
+    std::vector< byte > build_raw_data() const override;
+};
+#pragma pack( pop )
+
+bool operator==( const thermal_calibration_table & lhs, const thermal_calibration_table & rhs );
+
+}  // namespace l500
+}  // namespace thermal_loop
+}  // namespace algo
+}  // namespace librealsense

src/algo/thermal-loop/thermal-calibration-table-interface.h

@@ -0,0 +1,25 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2020 Intel Corporation. All Rights Reserved.
+
+#pragma once
+
+#include <vector>
+
+namespace librealsense {
+namespace algo {
+namespace thermal_loop {
+
+
+struct thermal_calibration_table_interface
+{
+    virtual ~thermal_calibration_table_interface() {}
+
+    virtual double get_thermal_scale( double hum_temp ) const = 0;
+
+    virtual std::vector< byte > build_raw_data() const = 0;
+};
+
+
+}  // namespace thermal_loop
+}  // namespace algo
+}  // namespace librealsense