First attempt and actual depth2disparity conversion

scripts/Depth2Disparity/depth2disparity.py

@@ -5,6 +5,7 @@
 
 
 import argparse
+import numpy as np
 import cv2
 
 def relative2abs(rel_depth_map, coord1, dist1, coord2, dist2):
@@ -22,19 +23,44 @@ def relative2abs(rel_depth_map, coord1, dist1, coord2, dist2):
     # this should not be normalised, the values in the array should equate to literal distances
     return abs_depth_map
 
-def depth2disparity(_abs_depth_map):
-    # Calculate disparity map from depth map
-    
-    # random values
-    baseline = 0.54  # Baseline distance in meters
-    focal_length = 0.05  # Focal length in meters
-    
-    # Calculate disparity map
-    disparity_map = baseline * focal_length / _abs_depth_map
-    
-    # normalise disparity map between 0 and 255
-    disparity_map = (disparity_map - disparity_map.min()) / (disparity_map.max() - disparity_map.min()) * 255    
+def depth2disparity(abs_depth_map, baseline=0.176, disp_scale=2.0, disp_offset=-120):
+  
+    # Get image dimensions
+    height, width = abs_depth_map.shape
+
+    # Calculate angular coordinates for each pixel
+    unit_w = 2.0 / width  # Longitude step size
+    unit_h = 1.0 / height  # Latitude step size
     
+    # Initialize disparity map
+    disparity_map = np.zeros_like(abs_depth_map, dtype=np.float32)
+
+    for i in range(height):
+        theta_t = i * unit_h * np.pi  # Latitude angle for the row
+
+        for j in range(width):
+            # Longitude angle (not strictly needed for disparity calculation)
+            phi = j * unit_w * np.pi
+
+            # Retrieve the absolute depth (r_t) for this pixel
+            r_t = abs_depth_map[i, j]
+
+            # Avoid division by zero for invalid or infinite depth values
+            if r_t <= 0:
+                disparity_map[i, j] = 0
+                continue
+
+            # Calculate angular disparity (d) using the spherical depth-to-disparity formula
+            try:
+                angle_disp = np.arctan(
+                    baseline / (r_t * np.sin(theta_t) + r_t * np.cos(theta_t) * np.tan(theta_t))
+                ) - theta_t
+
+                # Convert angular disparity to pixel disparity
+                disparity_map[i, j] = (angle_disp / (unit_h * np.pi) - disp_offset) * disp_scale
+            except ZeroDivisionError:
+                disparity_map[i, j] = 0
+
     return disparity_map