clean code

IntelRealSense · Jan 7, 2025 · d26ec5c · d26ec5c
1 parent fddfd7c
commit d26ec5c
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 33 deletions.
diff --git a/src/proc/align.cpp b/src/proc/align.cpp
@@ -39,6 +39,7 @@ namespace librealsense
     void align_images(const rs2_intrinsics& depth_intrin, const rs2_extrinsics& depth_to_other,
         const rs2_intrinsics& other_intrin, GET_DEPTH get_depth, TRANSFER_PIXEL transfer_pixel)
     {
+        auto start = std::chrono::high_resolution_clock::now();
         // Iterate over the pixels of the depth image
 #pragma omp parallel for schedule(dynamic)
         for (int depth_y = 0; depth_y < depth_intrin.height; ++depth_y)
@@ -52,12 +53,9 @@ namespace librealsense
                     // Map the top-left corner of the depth pixel onto the other image
                     float depth_pixel[2] = { depth_x - 0.5f, depth_y - 0.5f }, depth_point[3], other_point[3], other_pixel[2];
 
-                    auto start = std::chrono::high_resolution_clock::now();
+
                     rs2_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
-                    auto end = std::chrono::high_resolution_clock::now();
-                    auto duration = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start);
-                    // Output the duration
-                    std::cout << duration.count() << "\n";
+
 
                     rs2_transform_point_to_point(other_point, &depth_to_other, depth_point);
                     rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
@@ -86,6 +84,10 @@ namespace librealsense
                 }
             }
         }
+        auto end = std::chrono::high_resolution_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start);
+        // Output the duration
+        std::cout << duration.count() << "\n";
     }
 
     align::align(rs2_stream to_stream) : align(to_stream, "Align")

diff --git a/src/rs.cpp b/src/rs.cpp
@@ -4164,31 +4164,6 @@ void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrinsics* i
             }
 
         }
-        if (intrin->model == RS2_DISTORTION_KANNALA_BRANDT4)
-        {
-            float rd = sqrtf(x * x + y * y);
-            if (rd < FLT_EPSILON)
-            {
-                rd = FLT_EPSILON;
-            }
-
-            float theta = rd;
-            float theta2 = rd * rd;
-            for (int i = 0; i < 4; i++)
-            {
-                float f = theta * (1 + theta2 * (intrin->coeffs[0] + theta2 * (intrin->coeffs[1] + theta2 * (intrin->coeffs[2] + theta2 * intrin->coeffs[3])))) - rd;
-                if (fabs(f) < FLT_EPSILON)
-                {
-                    break;
-                }
-                float df = 1 + theta2 * (3 * intrin->coeffs[0] + theta2 * (5 * intrin->coeffs[1] + theta2 * (7 * intrin->coeffs[2] + 9 * theta2 * intrin->coeffs[3])));
-                theta -= f / df;
-                theta2 = theta * theta;
-            }
-            float r = tan(theta);
-            x *= r / rd;
-            y *= r / rd;
-        }
         if (intrin->model == RS2_DISTORTION_FTHETA)
         {
             float rd = sqrtf(x * x + y * y);
@@ -4201,11 +4176,30 @@ void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrinsics* i
             y *= r / rd;
         }
     }
-    else
+    if (intrin->model == RS2_DISTORTION_KANNALA_BRANDT4)
     {
+        float rd = sqrtf(x * x + y * y);
+        if (rd < FLT_EPSILON)
+        {
+            rd = FLT_EPSILON;
+        }
 
-        //int i = 0;
-        //LOG_ERROR("i=" << i);
+        float theta = rd;
+        float theta2 = rd * rd;
+        for (int i = 0; i < 4; i++)
+        {
+            float f = theta * (1 + theta2 * (intrin->coeffs[0] + theta2 * (intrin->coeffs[1] + theta2 * (intrin->coeffs[2] + theta2 * intrin->coeffs[3])))) - rd;
+            if (fabs(f) < FLT_EPSILON)
+            {
+                break;
+            }
+            float df = 1 + theta2 * (3 * intrin->coeffs[0] + theta2 * (5 * intrin->coeffs[1] + theta2 * (7 * intrin->coeffs[2] + 9 * theta2 * intrin->coeffs[3])));
+            theta -= f / df;
+            theta2 = theta * theta;
+        }
+        float r = tan(theta);
+        x *= r / rd;
+        y *= r / rd;
     }
 
     point[0] = depth * x;