Depth Estimation: Understanding 3D Space

class StereoDepthEstimator:
    """Simple stereo depth estimation."""

    def __init__(self, baseline_m, focal_length_px, fov_degrees=60):
        self.baseline = baseline_m  # meters
        self.focal_length = focal_length_px  # pixels
        self.fov = fov_degrees

    def compute_depth(self, disparity_px):
        """Compute depth from disparity in pixels."""
        if disparity_px <= 0:
            return float('inf')
        return (self.baseline * self.focal_length) / disparity_px

    def compute_depth_map(self, left_img, right_img, block_size=15):
        """Simple block matching stereo (for demonstration)."""
        h, w = left_img.shape[:2]
        depth_map = np.zeros((h, w), dtype=np.float32)

        half_block = block_size // 2

        for y in range(half_block, h - half_block):
            for x in range(half_block, w - half_block):
                left_block = left_img[y-half_block:y+half_block,
                                      x-half_block:x+half_block]

                best_match = 0
                best_disp = 0

                for disp in range(1, 64):  # Search range
                    if x - disp - half_block < 0:
                        break
                    right_block = right_img[y-half_block:y+half_block,
                                            x-disp-half_block:x-disp+half_block]

                    # SAD (Sum of Absolute Differences)
                    sad = np.sum(np.abs(left_block.astype(float) -
                                       right_block.astype(float)))
                    match = 1.0 / (1.0 + sad)

                    if match > best_match:
                        best_match = match
                        best_disp = disp

                depth_map[y, x] = self.compute_depth(best_disp)

        return depth_map

# Demo
estimator = StereoDepthEstimator(
    baseline_m=0.12,      # 12cm between cameras
    focal_length_px=640,   # 640px focal length
    fov_degrees=60
)

# Compute depth for various disparities
disparities = [1, 5, 10, 20, 40, 60]
for d in disparities:
    depth = estimator.compute_depth(d)
    print(f"Disparity: {d:2d}px → Depth: {depth:.2f}m")

class SimpleStereoDepth:
    """Block-matching stereo depth estimation."""

    def __init__(self, baseline=0.12, focal_length=640):
        self.baseline = baseline
        self.focal_length = focal_length

    def compute_disparity(self, left, right, block_size=15, max_disp=64):
        """Compute disparity map using block matching."""
        h, w = left.shape
        disparity = np.zeros((h, w), dtype=np.float32)
        half = block_size // 2

        for y in range(half, h - half):
            for x in range(half, w - half):
                best_sad = float('inf')
                best_disp = 0

                left_block = left[y-half:y+half, x-half:x+half].astype(float)

                for d in range(1, max_disp):
                    rx = x - d
                    if rx - half < 0:
                        break

                    right_block = right[y-half:y+half, rx-half:rx+half].astype(float)
                    sad = np.sum(np.abs(left_block - right_block))

                    if sad < best_sad:
                        best_sad = sad
                        best_disp = d

                disparity[y, x] = best_disp

        return disparity

    def disparity_to_depth(self, disparity):
        """Convert disparity map to depth map in meters."""
        depth = np.zeros_like(disparity)
        mask = disparity > 0
        depth[mask] = (self.baseline * self.focal_length) / disparity[mask]
        return depth

    def filter_depth(self, depth, min_depth=0.2, max_depth=20.0):
        """Filter depth values to valid range."""
        filtered = depth.copy()
        filtered[filtered < min_depth] = 0
        filtered[filtered > max_depth] = 0
        return filtered

# Demo with synthetic images
np.random.seed(42)
h, w = 240, 320

# Create synthetic left/right images with horizontal shift
left = np.random.randint(0, 256, (h, w), dtype=np.uint8)
right = np.zeros_like(left)

# Shift right image based on "depth" (closer = more shift)
depth_map = np.zeros((h, w))
depth_map[50:150, 100:200] = 5.0   # Object at 5m
depth_map[80:120, 200:280] = 10.0  # Object at 10m

for y in range(h):
    for x in range(w):
        if depth_map[y, x] > 0:
            disp = int(0.12 * 640 / depth_map[y, x])
            rx = x - disp
            if 0 <= rx < w:
                right[y, rx] = left[y, x]

stereo = SimpleStereoDepth(baseline=0.12, focal_length=640)
# Note: Full block matching is slow; this is for demonstration
# In practice, use OpenCV: stereo = cv2.StereoSGBM_create(...)
print("Stereo depth estimator initialized")
print(f"Baseline: {stereo.baseline}m, Focal length: {stereo.focal_length}px")