import cv2 import numpy as np from typing import List, Tuple def detect_horizontal_lines(image: np.ndarray) -> List[Tuple[int, float]]: """ Detect horizontal lines in the image for horizon detection. Returns list of (y_position, strength) tuples. Strength represents how prominent the line is (0-1). """ if len(image.shape) == 3: gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY) else: gray = image # Apply Canny edge detection edges = cv2.Canny(gray, 50, 150, apertureSize=3) # Use Hough Lines to detect lines lines = cv2.HoughLines(edges, 1, np.pi / 180, threshold=150) horizontal_lines = [] if lines is not None: for rho, theta in lines[:, 0]: # Check if line is approximately horizontal # Horizontal lines have theta near 90 degrees (pi/2) angle_diff = abs(theta - np.pi / 2) if angle_diff < np.pi / 6: # Within 30 degrees of horizontal # Calculate y position from rho and theta if abs(np.sin(theta)) > 1e-6: y = int(rho / np.sin(theta)) # Normalize strength based on rho magnitude strength = min(1.0, len(lines) / 500.0) horizontal_lines.append((y, strength)) # Sort by strength descending horizontal_lines.sort(key=lambda x: x[1], reverse=True) return horizontal_lines[:10] def detect_vertical_edges(image: np.ndarray) -> List[Tuple[int, int, float]]: """ Detect regions with strong vertical edges (potential doors/corners). Returns list of (x_start, x_end, strength) tuples. """ if len(image.shape) == 3: gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY) else: gray = image height, width = gray.shape # Apply Sobel operator for vertical edges sobel_x = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=3) sobel_x = np.abs(sobel_x) sobel_x = (sobel_x / sobel_x.max() * 255).astype(np.uint8) # Threshold to get strong edges _, binary = cv2.threshold(sobel_x, 50, 255, cv2.THRESH_BINARY) # Sum vertically to get edge profile vertical_profile = np.sum(binary, axis=0) / height # Find peaks in vertical profile (regions with many vertical edges) # Smooth the profile kernel = np.ones(20) / 20 smoothed = np.convolve(vertical_profile, kernel, mode="same") # Find regions above threshold threshold = 0.15 # 15% of pixels have vertical edges regions = [] in_region = False start = 0 for x in range(width): if smoothed[x] > threshold and not in_region: start = x in_region = True elif smoothed[x] <= threshold and in_region: in_region = False strength = min(1.0, np.mean(smoothed[start:x]) / 0.5) regions.append((start, x, float(strength))) if in_region: strength = min(1.0, np.mean(smoothed[start:]) / 0.5) regions.append((start, width, float(strength))) return regions def estimate_door_positions(image: np.ndarray) -> List[Tuple[float, float]]: """ Estimate door positions in an equirectangular image. Scans horizontal strips looking for vertical edge pairs (door frames). Returns list of (yaw_center, confidence) tuples. Yaw is in degrees (0-360), confidence is 0-1. """ if len(image.shape) == 3: gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY) else: gray = image height, width = gray.shape # Focus on the middle horizontal band (where doors typically appear) band_top = int(height * 0.25) band_bottom = int(height * 0.75) band = gray[band_top:band_bottom, :] # Apply Canny edge detection edges = cv2.Canny(band, 50, 150) # Use Hough Lines to detect vertical lines lines = cv2.HoughLines(edges, 1, np.pi / 180, threshold=100) vertical_lines = [] if lines is not None: for rho, theta in lines[:, 0]: # Check if line is approximately vertical # Vertical lines have theta near 0 or pi if theta < np.pi / 6 or theta > 5 * np.pi / 6: # Calculate x position if abs(np.cos(theta)) > 1e-6: x = int(rho / np.cos(theta)) if 0 <= x < width: vertical_lines.append(x) if len(vertical_lines) < 2: return [] # Sort unique x positions vertical_lines = sorted(set(vertical_lines)) # Look for pairs of vertical lines that could form door frames # Typical door width in equirectangular: ~20-60 pixels (adjustable) min_door_width = int(width * 0.02) # 2% of image width max_door_width = int(width * 0.10) # 10% of image width doors = [] for i in range(len(vertical_lines)): for j in range(i + 1, len(vertical_lines)): door_width = vertical_lines[j] - vertical_lines[i] if min_door_width <= door_width <= max_door_width: center_x = (vertical_lines[i] + vertical_lines[j]) // 2 yaw = (center_x / width) * 360.0 # Confidence based on how well-defined the door frame is # More edge pixels between the frames = higher confidence door_region = edges[:, vertical_lines[i]:vertical_lines[j]] edge_density = np.sum(door_region) / door_region.size confidence = min(1.0, edge_density / 128.0) doors.append((yaw, confidence)) # Merge nearby detections and take highest confidence if not doors: return [] doors.sort(key=lambda x: x[0]) merged = [] current_group = [doors[0]] for door in doors[1:]: if abs(door[0] - current_group[-1][0]) < 15: # Within 15 degrees current_group.append(door) else: # Take highest confidence from group best = max(current_group, key=lambda x: x[1]) merged.append(best) current_group = [door] best = max(current_group, key=lambda x: x[1]) merged.append(best) return merged def detect_floor_plane(image: np.ndarray) -> int: """ Estimate the horizon y-position in the image. Returns the y-coordinate of the estimated horizon. """ if len(image.shape) == 3: gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY) else: gray = image height, width = gray.shape # Use horizontal line detection lines = detect_horizontal_lines(image) if lines: # Pick the strongest horizontal line in the middle half of the image mid_quarter = height // 4 mid_three_quarter = 3 * height // 4 filtered = [(y, s) for y, s in lines if mid_quarter <= y <= mid_three_quarter] if filtered: # Return the y-position of the strongest line filtered.sort(key=lambda x: x[1], reverse=True) return filtered[0][0] # Fallback: assume horizon is at 40% from the top (common for equirectangular) return int(height * 0.4)