Merge branch 'AC2' into AC2_merge

.gitignore

@@ -30,6 +30,7 @@ DerivedData
 librealsense.xc/build
 
 *~
+*.TMP
 *.a
 *.o
 *.so

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

@@ -321,9 +321,9 @@ krt p_matrix::decompose() const
     std::vector<double> inv_k_square_vac( 9, 0 );
     inv( k_square.to_vector().data(), inv_k_square_vac.data() );
 
-    double3x3 inv_k_square = { inv_k_square_vac[0], inv_k_square_vac[1], inv_k_square_vac[2],
-                               inv_k_square_vac[3], inv_k_square_vac[4], inv_k_square_vac[5],
-                               inv_k_square_vac[6], inv_k_square_vac[7], inv_k_square_vac[8] };
+    double3x3 inv_k_square = { inv_k_square_vac[0], inv_k_square_vac[3], inv_k_square_vac[6],
+                               inv_k_square_vac[1], inv_k_square_vac[4], inv_k_square_vac[7],
+                               inv_k_square_vac[2], inv_k_square_vac[5], inv_k_square_vac[8] };
 
     //%KInv = cholesky3x3(KSquareInv)';% Cholsky decomposition 3 by 3. returns a lower triangular matrix 3x3. Equal to inv(Krgb')
     auto k_inv = cholesky3x3( inv_k_square ).transpose();

src/algo/depth-to-rgb-calibration/frame-data.h

@@ -105,6 +105,7 @@ namespace depth_to_rgb_calibration {
         std::vector<double > valid_edge_sub_pixel_y;
         std::vector<double> sub_points;
         std::vector<double3> vertices;
+        std::vector<double3> orig_vertices;
         std::vector<double> vertices3;
         std::vector<double2> uvmap;
         std::vector< byte > is_inside;

src/algo/depth-to-rgb-calibration/k-to-dsm.h

@@ -29,12 +29,59 @@ namespace depth_to_rgb_calibration {
 
     struct los_shift_scaling
     {
-        float los_scaling_x;
-        float los_scaling_y;
-        float los_shift_x;
-        float los_shift_y;
+        double los_scaling_x;
+        double los_scaling_y;
+        double los_shift_x;
+        double los_shift_y;
     };
 
+    struct rs2_dsm_params_double
+    {
+        rs2_dsm_params_double(rs2_dsm_params obj)
+            :timestamp(obj.timestamp),
+            version(obj.version),
+            model(obj.model),
+            h_scale(obj.h_scale),
+            v_scale(obj.v_scale),
+            h_offset(obj.h_offset),
+            v_offset(obj.v_offset),
+            rtd_offset(obj.rtd_offset),
+            temp_x2(obj.temp_x2){} //todo: flags
+
+        rs2_dsm_params_double() = default;
+
+        operator rs2_dsm_params()
+        {
+            rs2_dsm_params res;
+
+            res.timestamp = timestamp;
+            res.version = version;
+            res.model = model;
+            for (auto i = 0; i < 5; i++)
+                res.flags[i] = flags[i];
+
+            res.h_scale = h_scale;
+            res.v_scale = v_scale;
+            res.h_offset = h_offset;
+            res.v_offset = v_offset;
+            res.rtd_offset = rtd_offset;
+            res.temp_x2 = temp_x2;
+            return res;
+        }
+
+        unsigned long long timestamp;   /**< system_clock::time_point::time_since_epoch().count() */
+        unsigned short version;         /**< MAJOR<<12 | MINOR<<4 | PATCH */
+        unsigned char model;            /**< rs2_dsm_correction_model */
+        unsigned char flags[5];         /**< TBD, now 0s */
+        double        h_scale;          /**< the scale factor to horizontal DSM scale thermal results */
+        double        v_scale;          /**< the scale factor to vertical DSM scale thermal results */
+        double        h_offset;         /**< the offset to horizontal DSM offset thermal results */
+        double        v_offset;         /**< the offset to vertical DSM offset thermal results */
+        double        rtd_offset;       /**< the offset to the Round-Trip-Distance delay thermal results */
+        unsigned char temp_x2;          /**< the temperature recorded times 2 (ldd for depth; hum for rgb) */
+        unsigned char reserved[11];
+    };
+
 #pragma pack(push, 1)
     // This table is read from FW and is used in the optimizer
     struct algo_calibration_info
@@ -118,12 +165,13 @@ namespace depth_to_rgb_calibration {
         float    FRMWhumidApdTempDiff;
         uint8_t  reserved[94];
     };
+
     struct algo_calibration_registers
     {
-        float EXTLdsmXscale;
-        float EXTLdsmYscale;
-        float EXTLdsmXoffset;
-        float EXTLdsmYoffset;
+        double EXTLdsmXscale;
+        double EXTLdsmYscale;
+        double EXTLdsmXoffset;
+        double EXTLdsmYoffset;
     };
 #pragma pack(pop)
 
@@ -136,29 +184,36 @@ namespace depth_to_rgb_calibration {
         std::vector<double2> los_orig;
 
     };
+    struct iteration_data_collect;
 
     class k_to_DSM
     {
     public:
-        k_to_DSM(const rs2_dsm_params& orig_dsm_params,
+        k_to_DSM(const rs2_dsm_params_double& orig_dsm_params,
             algo::depth_to_rgb_calibration::algo_calibration_info const & cal_info,
             algo::depth_to_rgb_calibration::algo_calibration_registers const & cal_regs,
             const double& max_scaling_step);
 
-        algo_calibration_registers apply_ac_res_on_dsm_model(const rs2_dsm_params& ac_data, const algo_calibration_registers& regs, const ac_to_dsm_dir& type);
+        algo_calibration_registers apply_ac_res_on_dsm_model(const rs2_dsm_params_double& ac_data,
+            const algo_calibration_registers& regs, 
+            const ac_to_dsm_dir& type);
 
-        los_shift_scaling convert_ac_data_to_los_error(const algo_calibration_registers& algo_calibration_registers, const rs2_dsm_params& ac_data);
+        los_shift_scaling convert_ac_data_to_los_error(const algo_calibration_registers& algo_calibration_registers, 
+            const rs2_dsm_params_double& ac_data);
 
         pre_process_data pre_processing(const algo_calibration_info& regs,
-            const rs2_dsm_params& ac_data,
+            const rs2_dsm_params_double& ac_data,
             const algo_calibration_registers& algo_calibration_registers,
             const rs2_intrinsics_double& k_raw,
             const std::vector<uint8_t>& relevant_pixels_image);
 
-        rs2_dsm_params convert_new_k_to_DSM(
+        //return rs2_dsm_params and new vertices
+        rs2_dsm_params_double convert_new_k_to_DSM(
             const rs2_intrinsics_double& old_k,
             const rs2_intrinsics_double& new_k,
-            const z_frame_data& z);
+            const z_frame_data& z,
+            std::vector<double3>& new_vertices,
+            iteration_data_collect* data = nullptr);
 
         const pre_process_data& get_pre_process_data() const;
 
@@ -168,8 +223,8 @@ namespace depth_to_rgb_calibration {
             algo_calibration_registers const &dsm_regs,
             rs2_intrinsics_double const & k_raw);
 
-        rs2_dsm_params convert_los_error_to_ac_data(
-            const rs2_dsm_params& ac_data,
+        rs2_dsm_params_double convert_los_error_to_ac_data(
+            const rs2_dsm_params_double& ac_data,
             const algo_calibration_registers& dsm_regs,
             double2 los_shift, double2 los_scaling);
 
@@ -189,7 +244,8 @@ namespace depth_to_rgb_calibration {
         std::vector<double3> convert_los_to_norm_vertices(
             algo::depth_to_rgb_calibration::algo_calibration_info const & regs,
             algo_calibration_registers const &dsm_regs,
-            std::vector<double2> los);
+            std::vector<double2> los,
+            iteration_data_collect* data = nullptr);
 
         std::vector<double3> calc_relevant_vertices(const std::vector<uint8_t>& relevant_pixels_image, 
             const rs2_intrinsics_double& k);
@@ -207,7 +263,7 @@ namespace depth_to_rgb_calibration {
         double2 _new_los_scaling;
 
         //input camera params
-        rs2_dsm_params _ac_data;
+        rs2_dsm_params_double _ac_data;
         algo::depth_to_rgb_calibration::algo_calibration_info _regs;
         algo::depth_to_rgb_calibration::algo_calibration_registers _dsm_regs;