diff --git a/scripts/Depth2Disparity/depth_to_disparity.py b/scripts/Depth2Disparity/depth_to_disparity.py
index a0e3abef1f3339b7700595452986e0b1ba0e9694..d13d3dc369c7f39d170123eedc2370a53092e195 100644
--- a/scripts/Depth2Disparity/depth_to_disparity.py
+++ b/scripts/Depth2Disparity/depth_to_disparity.py
@@ -12,7 +12,7 @@ import matplotlib.pyplot as plt
import matplotlib
matplotlib.use("TkAgg") # Use TkAgg for GUI rendering
import warnings
-warnings.filterwarnings( "ignore", module = "matplotlib\..*" )
+warnings.filterwarnings( "ignore")
@@ -55,13 +55,13 @@ class Depth2Disparity:
print(f"Absolute depth map saved to: {abs_depth_path}\n")
# Convert depth map to disparity map
- #print("Converting abs depth to disparity ...")
- #disparity_map = self._depth2disparity(abs_depth_map)
+ print("Converting abs depth to disparity ...")
+ disparity_map, disparity_map_normalized = self._depth2disparity(abs_depth_map)
- ## Determine the output path for the disparity map
- #disparity_dir = os.path.dirname(rel_depth_path)
- #disparity_base = "disp_map.png"#"depth_e.png"
- #disparity_path = os.path.join(disparity_dir, disparity_base)
+ # Determine the output path for the disparity map
+ disparity_dir = os.path.dirname(rel_depth_path)
+ disparity_base = "disp_map.png"#"depth_e.png"
+ disparity_path = os.path.join(disparity_dir, disparity_base)
## Check if an existing disparity map needs to be renamed
##if os.path.exists(disparity_path):
@@ -71,10 +71,12 @@ class Depth2Disparity:
## if os.path.exists(old_depth_path):
## os.remove(old_depth_path)
## os.rename(disparity_path, old_depth_path)
- ## Save the new disparity map
- ##print(f"Saving new disparity map to: {disparity_path}")
- ##cv2.imwrite(disparity_path, (disparity_map).astype(np.uint8))
-
+
+ # Save the new disparity map
+ print(f"Saving new disparity map to: {disparity_path}")
+ cv2.imwrite(disparity_path, disparity_map_normalized)
+
+
# Debug message for GUI or logs
print("Complete")
@@ -84,27 +86,20 @@ class Depth2Disparity:
# Normalize depth map
print("Normalizing depth map...")
#rel_depth_map = rel_depth_map.astype(np.float32) / 255.0
+ rel_depth_map = (rel_depth_map - rel_depth_map.min()) / (rel_depth_map.max() - rel_depth_map.min())
- plt.imshow(rel_depth_map, cmap='gray')
- plt.colorbar()
- plt.show()
+ #plt.imshow(rel_depth_map, cmap='gray')
+ #plt.colorbar()
+ #plt.show()
- #print("\tStoring relative depth value at coordinates")
# Get the relative depth values at the two points
rel_value1 = float(rel_depth_map[coord1[1], coord1[0]]) # (y, x)
rel_value2 = float(rel_depth_map[coord2[1], coord2[0]])
print(f"rel1: {rel_value1}, rel2: {rel_value2}, dist1: {dist1}, dist2: {dist2}")
- #if rel_value1 > rel_value2:
- # coord1, coord2 = coord2, coord1
- # dist1, dist2 = dist2, dist1
-
-
- #print("\tPerforming linear transformation")
+
# Calculate the linear transformation: depth = a * rel_depth + b
- numerator = dist2 - dist1
- denominator = rel_value2 - rel_value1
- a = numerator / denominator
+ a = (dist2 - dist1) / (rel_value2 - rel_value1)
print("Calculated a: ", a)
b = dist1 - a * rel_value1
print("Calculated b: ", b)
@@ -114,33 +109,34 @@ class Depth2Disparity:
abs_depth_map = a * rel_depth_map + b
print("Applied transformation to entire depth map")
- plt.imshow(abs_depth_map, cmap='gray')
- plt.colorbar()
- plt.show()
-
+ abs_depth_map = np.maximum(abs_depth_map, 0.01)
+ #plt.imshow(abs_depth_map, cmap='gray')
+ #plt.colorbar()
+ #plt.show()
+
# this should not be normalised, the values in the array should equate to literal distances
return abs_depth_map
def _depth2disparity(self, abs_depth_map):
- # Get image dimensions
+ # Get image dimensions
+ # Get image dimensions
height, width = abs_depth_map.shape
- # Calculate angular coordinates for each pixel
+ # Calculate step sizes
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)
+ # Iterate over each pixel
for i in range(height):
- theta_t = i * unit_h * np.pi # Latitude angle for the row
+ # Calculate latitude angle for the row
+ theta_t = i * unit_h * np.pi
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]
@@ -150,20 +146,22 @@ class Depth2Disparity:
continue
try:
- # Compute denominator with stability checks
- epsilon = 1e-8 # Small value to prevent division instability
+ # Compute denominator for the formula
+ epsilon = 1e-8 # Small value to prevent instability
tan_theta_t = np.tan(theta_t) if np.abs(np.cos(theta_t)) > epsilon else np.sign(np.sin(theta_t)) * np.inf
-
denominator = r_t * np.sin(theta_t) + r_t * np.cos(theta_t) * tan_theta_t
+
+ # Avoid invalid denominator values
if denominator <= 0:
disparity_map[i, j] = 0
continue
- # Calculate angular disparity (d)
+ # Calculate angular disparity
angle_disp = np.arctan(self.baseline / denominator) - theta_t
# Convert angular disparity to pixel disparity
- disparity_map[i, j] = (angle_disp / (unit_h * np.pi) - self.disp_offset) * self.disp_scale
+ pixel_disp = (angle_disp / (unit_h * np.pi) - self.disp_offset) * self.disp_scale
+ disparity_map[i, j] = pixel_disp
except ZeroDivisionError:
disparity_map[i, j] = 0
@@ -172,7 +170,12 @@ class Depth2Disparity:
plt.colorbar()
plt.show()
- return disparity_map
+ # Normalize disparity map for visualization
+ disparity_map_normalized = (disparity_map - disparity_map.min()) / (disparity_map.max() - disparity_map.min())
+ disparity_map_normalized *= 255
+ disparity_map_normalized = disparity_map_normalized.astype(np.uint8)
+
+ return disparity_map, disparity_map_normalized
\ No newline at end of file