Merge PR #10 (before Shavuot) from aangerma/DSM

Dsm
nohayassin · May 30, 2020 · 87d3a6e · 87d3a6e
2 parents 426d113 + df7dab8
commit 87d3a6e
Show file tree

Hide file tree

Showing 14 changed files with 1,416 additions and 263 deletions.
diff --git a/src/algo/depth-to-rgb-calibration/CMakeLists.txt b/src/algo/depth-to-rgb-calibration/CMakeLists.txt
@@ -17,6 +17,8 @@ target_sources(${LRS_TARGET}
         "${CMAKE_CURRENT_LIST_DIR}/optimizer.h"
         "${CMAKE_CURRENT_LIST_DIR}/k-to-dsm.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/k-to-dsm.h"
+        "${CMAKE_CURRENT_LIST_DIR}/utils.cpp"
+        "${CMAKE_CURRENT_LIST_DIR}/utils.h"
         "${CMAKE_CURRENT_LIST_DIR}/rotation-in-angles.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/valid-scene.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/valid-results.cpp"

diff --git a/src/algo/depth-to-rgb-calibration/calibration-types.h b/src/algo/depth-to-rgb-calibration/calibration-types.h
@@ -18,6 +18,22 @@ namespace depth_to_rgb_calibration {
         double & operator [] ( int i ) { return (&x)[i]; }
         bool operator == (const double3 d) { return x == d.x && y == d.y && z == d.z; }
         bool operator != (const double3 d) { return !(*this == d); }
+        double3 operator-(const double3& other) {
+            return { x - other.x, y - other.y , z - other.z };
+        }
+
+        double get_norm()
+        {
+            return x*x + y * y + z * z;
+        }
+
+        double3 operator*(const double& scalar) {
+            return { x * scalar , y * scalar , z * scalar };
+        }
+
+        double operator*(const double3& s) {
+            return x * s.x + y * s.y + z * s.z;
+        }
     };
 
     struct double2
@@ -30,8 +46,18 @@ namespace depth_to_rgb_calibration {
 
         struct double3x3
         {
-                double mat[3][3];
-                double3x3 transpose()
+           /* double3x3(double arr[9])
+            {
+                for (auto i = 0; i < 3; i++)
+                {
+                    for (auto j = 0; j < 3; j++)
+                    {
+                        mat[i][j] = arr[i * 3 + j];
+                    }
+                }
+            }*/
+            double mat[3][3];
+            double3x3 transpose()
                 {
                         double3x3 res = { 0 };
 
@@ -45,7 +71,7 @@ namespace depth_to_rgb_calibration {
                         return res;
                 }
 
-                double3x3 operator*(const double3x3& other)
+            double3x3 operator*(const double3x3& other)
                 {
                         double3x3 res = { 0 };
 
@@ -64,7 +90,7 @@ namespace depth_to_rgb_calibration {
                         return res;
                 }
 
-                double3 operator*(const double3& other)
+            double3 operator*(const double3& other)
                 {
                         double3 res = { 0 };
 
@@ -74,18 +100,18 @@ namespace depth_to_rgb_calibration {
                         return res;
                 }
 
-                std::vector<double> to_vector()
-                {
-                        std::vector<double> res;
-                        for (auto i = 0; i < 3; i++)
-                        {
-                                for (auto j = 0; j < 3; j++)
-                                {
-                                        res.push_back(mat[i][j]);
-                                }
-                        }
-                        return res;
-                }
+            std::vector<double> to_vector()
+            {
+                    std::vector<double> res;
+                    for (auto i = 0; i < 3; i++)
+                    {
+                            for (auto j = 0; j < 3; j++)
+                            {
+                                    res.push_back(mat[i][j]);
+                            }
+                    }
+                    return res;
+            }
         };
 
     enum direction :uint8_t

diff --git a/src/algo/depth-to-rgb-calibration/frame-data.h b/src/algo/depth-to-rgb-calibration/frame-data.h
@@ -60,6 +60,8 @@ namespace depth_to_rgb_calibration {
         rs2_intrinsics_double orig_intrinsics;
         rs2_intrinsics_double new_intrinsics;
         rs2_dsm_params orig_dsm_params;
+        /*algo_calibration_registers algo_calibration_registers;
+        regs regs;*/
         float depth_units;
 
         std::vector< z_t > frame;