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/calibration.h

@@ -144,6 +144,7 @@ namespace depth_to_rgb_calibration {
         calib( calib const & ) = default;
         explicit calib( rs2_intrinsics_double const & rgb_intrinsics, rs2_extrinsics_double const & depth_to_rgb_extrinsics);
         explicit calib( rs2_intrinsics const & rgb_intrinsics, rs2_extrinsics const & depth_to_rgb_extrinsics);
+        explicit calib( rs2_intrinsics_double const & rgb_intrinsics, rs2_extrinsics const & depth_to_rgb_extrinsics );
 
         rs2_intrinsics_double get_intrinsics() const;
         rs2_extrinsics_double get_extrinsics() const;

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

@@ -1559,6 +1559,8 @@ void params::set_depth_resolution( size_t width, size_t height, rs2_ambient_ligh
     // Some parameters are resolution-dependent
     bool const XGA = (width == 1024 && height == 768);
     bool const VGA = (width == 640 && height == 480);
+    bool const ATV = ( width == 1024 && height == 480 ); // speciel resolution uses for calibration
+
     if( XGA )
     {
         AC_LOG( DEBUG, "    changing IR threshold: " << grad_ir_threshold << " -> " << 2.5 << "  (because of resolution)" );
@@ -1601,6 +1603,15 @@ void params::set_depth_resolution( size_t width, size_t height, rs2_ambient_ligh
             }
         }
     }
+
+    if (ATV)
+    {
+        grad_ir_low_th = 1;
+        grad_ir_high_th = 2.5;
+        grad_z_low_th = 0;
+        grad_z_high_th = 80;
+    }
+
     min_weighted_edge_per_section_depth = 50. * ( 480 * 640 ) / ( width * height );
 }
 
@@ -1615,6 +1626,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,11 +1644,11 @@ 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
-)
+void librealsense::algo::depth_to_rgb_calibration::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 );
@@ -1652,15 +1664,6 @@ 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,17 @@ namespace depth_to_rgb_calibration {
         double step_size = 0;
     };
 
+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_to_file( void const * data, size_t cb, std::string const & dir, char const * filename );
+
     struct params
     {
         params();

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,80 @@
+//// License: Apache 2.0. See LICENSE file in root directory.
+//// Copyright(c) 2020 Intel Corporation. All Rights Reserved.
+
+#pragma once
+#include "l500-thermal-loop.h"
+
+namespace librealsense {
+namespace algo {
+namespace thermal_loop {
+namespace l500 {
+
+
+using calib_info = l500_thermal_loop::rgb_thermal_calib_info;
+
+calib_info l500_thermal_loop::parse_thermal_table( const std::vector< byte > & data )
+{
+    rgb_thermal_calib_info res;
+    std::vector< float > float_vac;
+
+    float_vac.assign( (float *)data.data(),
+                      (float *)data.data() + ( data.size() / sizeof( float ) ) );
+
+    res.md.min_temp = float_vac[0];
+    res.md.max_temp = float_vac[1];
+
+    const int meta_data_size = sizeof( calib_info::table_meta_data ) / sizeof( float );
+    const int table_data_size = sizeof( calib_info::table_data ) / sizeof( float );
+
+    if( ( float_vac.size() - meta_data_size ) / table_data_size == calib_info::resolution )
+    {
+        res.md.reference_temp = float_vac[2];
+        res.md.valid = float_vac[3];
+        res.vals.assign( (calib_info::table_data *)( float_vac.data() + meta_data_size ),
+                         (calib_info::table_data *)float_vac.data() + calib_info::resolution + 1 );
+    }
+    else
+    {
+        throw std::runtime_error( librealsense::to_string() << "file size (" << data.size()
+                                                            << ") does not match data size " );
+    }
+
+    return res;
+}
+
+double l500_thermal_loop::get_rgb_current_thermal_scale( const rgb_thermal_calib_info & table,
+                                                         double hum_temp )
+{
+
+    // curr temp is under minimum
+    if( hum_temp <= table.md.min_temp )
+        return 1 / (double)table.vals[0].scale;
+
+    // curr temp is above maximum
+    if( hum_temp >= table.md.max_temp )
+        return 1 / (double)table.vals[rgb_thermal_calib_info::resolution - 1].scale;
+
+    auto temp_range = table.md.max_temp - table.md.min_temp;
+    auto temp_interval = temp_range / ( rgb_thermal_calib_info::resolution + 1 );
+
+    float i = temp_interval;
+    for( int ind = 0; ind < rgb_thermal_calib_info::resolution; ind++ )
+    {
+        if( hum_temp <= i )
+        {
+            return 1 / (double)table.vals[ind].scale;
+        }
+        i += temp_interval;
+    }
+    throw std::runtime_error( librealsense::to_string() << hum_temp << "is not valid " );
+}
+
+fx_fy l500_thermal_loop::correct_thermal_scale( std::pair< double, double > in_calib,
+                                                   double scale )
+{
+    return { in_calib.first * scale, in_calib.second * scale };
+}
+}
+}
+}
+}

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

@@ -0,0 +1,80 @@
+// 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"
+
+namespace librealsense {
+namespace algo {
+namespace thermal_loop {
+namespace l500 {
+
+using fx_fy = std::pair< double, double >;
+
+#pragma pack( push, 1 )
+
+class l500_thermal_loop
+{
+public:
+    struct rgb_thermal_calib_info
+    {
+        static const int table_id = 0x317;
+        static const int resolution = 29;
+
+        struct table_meta_data
+        {
+            float min_temp;
+            float max_temp;
+            float reference_temp;  // not used
+            float valid;           // not used
+        };
+
+        struct table_data
+        {
+            float scale;
+            float p[3];  // parameters which effects offset that are not in use
+        };
+
+        table_meta_data md;
+        std::vector< table_data > vals;
+
+        std::vector< byte > get_raw_data()
+        {
+            std::vector< byte > res;
+            std::vector< float > data;
+            data.push_back( md.min_temp );
+            data.push_back( md.max_temp );
+            data.push_back( md.reference_temp );
+            data.push_back( md.valid );
+
+            for( auto i = 0; i < vals.size(); i++ )
+            {
+                data.push_back( vals[i].scale );
+                data.push_back( vals[i].p[0] );
+                data.push_back( vals[i].p[1] );
+                data.push_back( vals[i].p[2] );
+            }
+
+            res.assign( (byte *)( data.data() ), (byte *)( data.data() + data.size() ) );
+            return res;
+        }
+    };
+#pragma pack( pop )
+
+    static rgb_thermal_calib_info parse_thermal_table( const std::vector< byte > & data );
+
+    static double get_rgb_current_thermal_scale( const rgb_thermal_calib_info & table,
+                                                 double hum_temp );
+
+    static fx_fy correct_thermal_scale( std::pair< double, double > in_calib,
+                                                              double scale );
+};
+// this struct based on RGB_Thermal_Info_CalibInfo table
+
+
+}  // namespace l500
+}  // namespace thermal_loop
+}  // namespace algo
+}  // namespace librealsense

src/depth-to-rgb-calibration.cpp

@@ -8,7 +8,7 @@
 #include "context.h"
 #include "api.h"  // VALIDATE_INTERFACE_NO_THROW
 #include "algo/depth-to-rgb-calibration/debug.h"
-
+#include "algo/thermal-loop/l500-thermal-loop.h"
 
 using namespace librealsense;
 namespace impl = librealsense::algo::depth_to_rgb_calibration;
@@ -24,10 +24,13 @@ depth_to_rgb_calibration::depth_to_rgb_calibration(
     std::vector< impl::yuy_t > const & last_yuy_data,
     impl::algo_calibration_info const & cal_info,
     impl::algo_calibration_registers const & cal_regs,
+    rs2_intrinsics yuy_intrinsics,
+    double scale,
     std::function<void()> should_continue
 )
     : _algo( settings )
-    , _intr( yuy.get_profile().as< rs2::video_stream_profile >().get_intrinsics() )
+    , _intr( yuy_intrinsics )
+    , _intr_with_k_thermal( _intr )
     , _extr(to_raw_extrinsics( depth.get_profile().get_extrinsics_to( yuy.get_profile() )))
     , _from( depth.get_profile().get()->profile )
     , _to( yuy.get_profile().get()->profile )
@@ -41,7 +44,13 @@ depth_to_rgb_calibration::depth_to_rgb_calibration(
         _last_successful_frame_data = last_yuy_data;  // copy -- will be moved to algo
     else if( ! last_yuy_data.empty() )
         AC_LOG( DEBUG, "Not using last successfully-calibrated scene: it's of a different resolution" );
-    impl::calib calibration( _intr, _extr );
+
+    auto fx_fy = algo::thermal_loop::l500::l500_thermal_loop::correct_thermal_scale(
+        { _intr.fx, _intr.fy },
+        scale );
+    _intr_with_k_thermal.fx = fx_fy.first;
+    _intr_with_k_thermal.fy = fx_fy.second;
+    impl::calib calibration( _intr_with_k_thermal, _extr );
 
     CHECK_IF_NEEDS_TO_STOP();
 
@@ -159,8 +168,12 @@ rs2_calibration_status depth_to_rgb_calibration::optimize(
         // cost= " << params_curr.cost );
 
         AC_LOG( DEBUG, "Optimization successful!" );
-        _intr = _algo.get_calibration().get_intrinsics();
-        _intr.model = RS2_DISTORTION_INVERSE_BROWN_CONRADY; //restore LRS model 
+        _intr_with_k_thermal = _algo.get_calibration().get_intrinsics();
+
+        // override only the ppx and ppy the fx and fy are the original
+        _intr.ppx = _intr_with_k_thermal.ppx; 
+        _intr.ppy = _intr_with_k_thermal.ppy;
+        _intr.model = _intr_with_k_thermal.model = RS2_DISTORTION_INVERSE_BROWN_CONRADY;  // restore LRS model 
         _extr = from_raw_extrinsics( _algo.get_calibration().get_extrinsics() );
         _dsm_params = _algo.get_dsm_params();
         _last_successful_frame_data = _algo.get_yuy_data().orig_frame;  // copy -- will be moved to ac_trigger
@@ -177,8 +190,9 @@ rs2_calibration_status depth_to_rgb_calibration::optimize(
 
 void depth_to_rgb_calibration::debug_calibration( char const * prefix )
 {
-    AC_LOG( DEBUG, AC_F_PREC << "    " << prefix << " intr" << _intr );
-    AC_LOG( DEBUG, AC_F_PREC << "    " << prefix << " extr" << _extr );
-    AC_LOG( DEBUG, AC_F_PREC << "    " << prefix << "  dsm" << _dsm_params );
+    AC_LOG( INFO, AC_F_PREC << "    " << prefix << " intr with k-thermal" << _intr_with_k_thermal );
+    AC_LOG( INFO, AC_F_PREC << "    " << prefix << " intr" << _intr );
+    AC_LOG( INFO, AC_F_PREC << "    " << prefix << " extr" << _extr );
+    AC_LOG( INFO, AC_F_PREC << "    " << prefix << "  dsm" << _dsm_params );
 }
 

src/depth-to-rgb-calibration.h

@@ -23,7 +23,8 @@ namespace librealsense
 
         // input/output
         rs2_extrinsics _extr;
-        rs2_intrinsics _intr;
+        rs2_intrinsics _intr; //raw intrinsics for overriding the fw intrinsics
+        rs2_intrinsics _intr_with_k_thermal; // intrinsics with k_thermal for user
         rs2_dsm_params _dsm_params;
         std::vector< algo::depth_to_rgb_calibration::yuy_t > _last_successful_frame_data;
 
@@ -40,11 +41,14 @@ namespace librealsense
             std::vector< algo::depth_to_rgb_calibration::yuy_t > const & last_yuy_data,
             algo::depth_to_rgb_calibration::algo_calibration_info const & cal_info,
             algo::depth_to_rgb_calibration::algo_calibration_registers const & cal_regs,
+            rs2_intrinsics yuy_intrinsics,
+            double scale,
             std::function<void()> should_continue = nullptr
         );
 
         rs2_extrinsics const & get_extrinsics() const { return _extr; }
         rs2_intrinsics const & get_intrinsics() const { return _intr; }
+        rs2_intrinsics const & get_k_thermal_intrinsics() const { return _intr_with_k_thermal; }
         stream_profile_interface * get_from_profile() const { return _from; }
         stream_profile_interface * get_to_profile() const { return _to; }
         rs2_dsm_params const & get_dsm_params() const { return _dsm_params; }