diff --git a/scripts/Depth2Disparity/depth2disparity.py b/scripts/Depth2Disparity/depth2disparity.py index 95285fb13eaa4d8990c92b80931d4a1683bdf7fd..948b3472b1c7819169aed7da020aaf7d0afa79ad 100644 --- a/scripts/Depth2Disparity/depth2disparity.py +++ b/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