proximity_webcam.py

import cv2
import time
import numpy as np
import open3d as o3d
import pickle
import os
try:
    from ultralytics import YOLO
    YOLO_AVAILABLE = True
except ImportError:
    YOLO_AVAILABLE = False
    print("YOLO not available. Install with: pip install ultralytics")

def load_stereo_calibration(calibration_file="stereo_calibration.pkl"):
    """
    Load stereo calibration parameters from file.
    Returns None if file doesn't exist.
    """
    if os.path.exists(calibration_file):
        with open(calibration_file, 'rb') as f:
            return pickle.load(f)
    return None

def save_stereo_calibration(params, calibration_file="stereo_calibration.pkl"):
    """
    Save stereo calibration parameters to file.
    """
    with open(calibration_file, 'wb') as f:
        pickle.dump(params, f)

def perform_stereo_calibration(left_images, right_images, pattern_size=(9, 6)):
    """
    Perform stereo calibration using chessboard patterns.
    """
    # Try different pattern sizes if the default doesn't work
    pattern_sizes = [(9, 6), (8, 6), (7, 5), (6, 4), (5, 4)]
    
    for pattern in pattern_sizes:
        print(f"Trying chessboard pattern size: {pattern}")
        
        # Prepare object points
        objp = np.zeros((pattern[0] * pattern[1], 3), np.float32)
        objp[:, :2] = np.mgrid[0:pattern[0], 0:pattern[1]].T.reshape(-1, 2)
        
        objpoints = []  # 3d points in real world space
        imgpoints_left = []  # 2d points in left image plane
        imgpoints_right = []  # 2d points in right image plane
        
        valid_pairs = 0
        for i, (left_img, right_img) in enumerate(zip(left_images, right_images)):
            gray_left = cv2.cvtColor(left_img, cv2.COLOR_BGR2GRAY)
            gray_right = cv2.cvtColor(right_img, cv2.COLOR_BGR2GRAY)
            
            # Find chessboard corners with more flexible criteria
            criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
            
            ret_left, corners_left = cv2.findChessboardCorners(
                gray_left, pattern, 
                cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE + cv2.CALIB_CB_FILTER_QUADS
            )
            ret_right, corners_right = cv2.findChessboardCorners(
                gray_right, pattern,
                cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE + cv2.CALIB_CB_FILTER_QUADS
            )
            
            if ret_left and ret_right:
                # Refine corner positions
                corners_left = cv2.cornerSubPix(gray_left, corners_left, (11, 11), (-1, -1), criteria)
                corners_right = cv2.cornerSubPix(gray_right, corners_right, (11, 11), (-1, -1), criteria)
                
                objpoints.append(objp)
                imgpoints_left.append(corners_left)
                imgpoints_right.append(corners_right)
                valid_pairs += 1
                print(f"  Valid pair {valid_pairs} found in image {i+1}")
            else:
                print(f"  No chessboard found in image {i+1} (Left: {ret_left}, Right: {ret_right})")
        
        print(f"Found {valid_pairs} valid calibration pairs with pattern {pattern}")
        
        if valid_pairs >= 8:  # Reduced minimum requirement
            print(f"Using pattern {pattern} with {valid_pairs} valid pairs")
            pattern_size = pattern
            break
    
    if len(objpoints) < 8:
        print(f"Not enough calibration images found! Only {len(objpoints)} valid pairs.")
        print("Tips:")
        print("- Ensure the chessboard is clearly visible in both cameras")
        print("- Try different lighting conditions")
        print("- Make sure the chessboard is not too close or too far")
        print("- Ensure the chessboard is flat and not wrinkled")
        return None
    
    # Calibrate individual cameras
    ret_left, K1, D1, _, _ = cv2.calibrateCamera(objpoints, imgpoints_left, gray_left.shape[::-1], None, None)
    ret_right, K2, D2, _, _ = cv2.calibrateCamera(objpoints, imgpoints_right, gray_right.shape[::-1], None, None)
    
    # Stereo calibration
    ret, K1, D1, K2, D2, R, T, E, F = cv2.stereoCalibrate(
        objpoints, imgpoints_left, imgpoints_right,
        K1, D1, K2, D2, gray_left.shape[::-1],
        criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6),
        flags=cv2.CALIB_FIX_INTRINSIC
    )
    
    # Stereo rectification
    R1, R2, P1, P2, Q, _, _ = cv2.stereoRectify(
        K1, D1, K2, D2, gray_left.shape[::-1], R, T,
        alpha=0, newImageSize=gray_left.shape[::-1]
    )
    
    return {
        'K1': K1, 'D1': D1, 'K2': K2, 'D2': D2,
        'R': R, 'T': T, 'R1': R1, 'R2': R2,
        'P1': P1, 'P2': P2, 'Q': Q,
        'image_size': gray_left.shape[::-1]
    }

def get_color_by_distance(face_area, min_area=1000, max_area=50000):
    """
    Returns a color based on face area (distance indicator).
    Blue for far, green for medium, red for close.
    """
    # Normalize the face area to a 0-1 range
    normalized = min(max((face_area - min_area) / (max_area - min_area), 0), 1)
    
    # Create color gradient: Blue -> Green -> Red
    if normalized < 0.5:
        # Blue to Green
        ratio = normalized * 2
        blue = int(255 * (1 - ratio))
        green = int(255 * ratio)
        red = 0
    else:
        # Green to Red
        ratio = (normalized - 0.5) * 2
        blue = 0
        green = int(255 * (1 - ratio))
        red = int(255 * ratio)
    
    return (blue, green, red)

def get_object_color(class_name):
    """
    Returns a color for different object classes.
    """
    colors = {
        'person': (0, 255, 0),      # Green
        'car': (255, 0, 0),         # Blue
        'truck': (255, 0, 255),     # Magenta
        'bus': (0, 255, 255),       # Yellow
        'bicycle': (255, 255, 0),   # Cyan
        'motorcycle': (128, 0, 128), # Purple
        'chair': (255, 165, 0),     # Orange
        'couch': (255, 192, 203),   # Pink
        'tv': (128, 128, 128),      # Gray
        'laptop': (0, 128, 255),    # Light Blue
        'cell phone': (255, 20, 147), # Deep Pink
        'book': (139, 69, 19),      # Brown
        'bottle': (0, 100, 0),      # Dark Green
        'cup': (255, 215, 0),       # Gold
    }
    return colors.get(class_name, (255, 255, 255))  # Default white

def detect_objects_and_people(frame, yolo_model, face_cascade, camera_index, depth=None):
    """
    Detect both objects using YOLO and faces using Haar cascades.
    Returns the frame with annotations and detection info.
    """
    detections = []
    
    # YOLO object detection
    if YOLO_AVAILABLE and yolo_model is not None:
        try:
            results = yolo_model(frame, verbose=False)
            
            for result in results:
                boxes = result.boxes
                if boxes is not None:
                    for box in boxes:
                        # Get bounding box coordinates
                        x1, y1, x2, y2 = box.xyxy[0].cpu().numpy()
                        confidence = box.conf[0].cpu().numpy()
                        class_id = int(box.cls[0].cpu().numpy())
                        class_name = yolo_model.names[class_id]
                        
                        if confidence > 0.5:  # Confidence threshold
                            x1, y1, x2, y2 = int(x1), int(y1), int(x2), int(y2)
                            w, h = x2 - x1, y2 - y1
                            
                            color = get_object_color(class_name)
                            
                            # Draw bounding box
                            cv2.rectangle(frame, (x1, y1), (x2, y2), color, 2)
                            
                            # Add label with confidence
                            label = f"{class_name}: {confidence:.2f}"
                            cv2.putText(frame, label, (x1, y1-10), 
                                       cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)
                            
                            # Add depth info if available
                            if depth is not None and camera_index == 0:
                                center_x = (x1 + x2) // 2
                                center_y = (y1 + y2) // 2
                                if 0 <= center_y < depth.shape[0] and 0 <= center_x < depth.shape[1]:
                                    obj_depth = depth[center_y, center_x]
                                    if obj_depth > 0:
                                        cv2.putText(frame, f"Depth: {obj_depth:.2f}m", 
                                                   (x1, y2+20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1)
                            
                            detections.append({
                                'type': 'object',
                                'class': class_name,
                                'confidence': confidence,
                                'bbox': (x1, y1, w, h),
                                'depth': obj_depth if depth is not None and camera_index == 0 else None
                            })
        except Exception as e:
            print(f"YOLO detection error: {e}")
    
    # Face detection using Haar cascades
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    faces = face_cascade.detectMultiScale(gray, 1.1, 4)
    
    for (x, y, w, h) in faces:
        face_area = w * h
        color = get_color_by_distance(face_area)
        
        # Draw rectangle with proximity-based color
        cv2.rectangle(frame, (x, y), (x+w, y+h), color, 3)
        
        # Add distance indicator text
        distance_text = "CLOSE" if face_area > 30000 else "MEDIUM" if face_area > 10000 else "FAR"
        cv2.putText(frame, f"FACE: {distance_text}", (x, y-10), 
                   cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2)
        
        # Add depth info if available
        if depth is not None and camera_index == 0:
            face_center_x = x + w // 2
            face_center_y = y + h // 2
            if 0 <= face_center_y < depth.shape[0] and 0 <= face_center_x < depth.shape[1]:
                face_depth = depth[face_center_y, face_center_x]
                if face_depth > 0:
                    cv2.putText(frame, f"Depth: {face_depth:.2f}m", (x, y+h+40), 
                               cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1)
        else:
            # Add area debug info for non-depth cameras
            cv2.putText(frame, f"Area: {face_area}", (x, y+h+20), 
                       cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1)
        
        detections.append({
            'type': 'face',
            'area': face_area,
            'bbox': (x, y, w, h),
            'depth': face_depth if depth is not None and camera_index == 0 else None
        })
    
    return frame, detections

def detect_proximity_and_open_webcam():
    """
    AI-powered object and person identifier using dual cameras.
    Detects objects using YOLO and faces using Haar cascades.
    Supports stereo vision for depth estimation and 3D reconstruction.
    """
    
    # Load the face detection classifier
    face_cascade = cv2.CascadeClassifier(cv2.data.haarcascades + 'haarcascade_frontalface_default.xml')
    
    # Initialize YOLO model
    yolo_model = None
    if YOLO_AVAILABLE:
        try:
            print("Loading YOLO model...")
            yolo_model = YOLO('yolov8n.pt')  # Using nano version for speed
            print("YOLO model loaded successfully!")
        except Exception as e:
            print(f"Failed to load YOLO model: {e}")
            yolo_model = None
    else:
        print("YOLO not available. Only face detection will be used.")
    
    # Initialize cameras (need exactly 2 for stereo)
    cameras = []
    backends = [cv2.CAP_DSHOW, cv2.CAP_MSMF, cv2.CAP_ANY]
    
    # Try to detect and initialize cameras 0 and 1 only
    for camera_index in range(2):
        cap = None
        for backend in backends:
            print(f"Trying to open camera {camera_index} with backend: {backend}")
            cap = cv2.VideoCapture(camera_index, backend)
            if cap.isOpened():
                # Test if we can actually read a frame
                ret, test_frame = cap.read()
                if ret:
                    print(f"Successfully opened camera {camera_index} with backend: {backend}")
                    cameras.append({'cap': cap, 'index': camera_index, 'backend': backend})
                    break
                else:
                    cap.release()
                    cap = None
            else:
                if cap:
                    cap.release()
                cap = None
        
        # If we couldn't open this camera, stop trying higher indices
        if cap is None:
            break
    
    if len(cameras) < 2:
        print("Error: Need exactly 2 cameras for stereo vision")
        print("Please check:")
        print("1. Both cameras are connected")
        print("2. Cameras are not being used by another application")
        print("3. Camera permissions are enabled")
        return
    
    print(f"Found {len(cameras)} camera(s)")
    
    # Load or prompt for stereo calibration
    stereo_params = load_stereo_calibration()
    if stereo_params is None:
        print("No stereo calibration found. Please calibrate cameras first.")
        print("Place a chessboard pattern in view and press 'c' to capture calibration images.")
        print("Capture at least 8-15 images from different angles and distances.")
        print("Make sure the chessboard:")
        print("- Is clearly visible in both camera views")
        print("- Is well-lit and not in shadow")
        print("- Is held flat (not bent or wrinkled)")
        print("- Fills a good portion of the camera view")
    
    # Initialize stereo matcher
    stereo_matcher = cv2.StereoBM_create(numDisparities=64, blockSize=15)
    
    # Initialize 3D reconstruction components
    tsdf = None
    pose = np.eye(4)
    first_frame = True
    
    # Calibration mode variables
    calibration_mode = stereo_params is None
    calibration_images_left = []
    calibration_images_right = []
    
    # Set up zoom control for camera 1 (assuming it has zoom capability)
    zoom_level = 1.0
    zoom_step = 0.1
    min_zoom = 0.5
    max_zoom = 3.0
    
    if calibration_mode:
        print("Calibration mode active.")
        print("Controls:")
        print("  'c' - Capture calibration image pair")
        print("  's' - Start calibration process")
        print("  'q' - Quit")
    else:
        print("AI Object and Person Identifier started.")
        print("Features:")
        print("  - YOLO object detection" if yolo_model else "  - Face detection only")
        print("  - Stereo depth estimation" if stereo_params else "  - No depth (calibration needed)")
        print("  - 3D reconstruction")
        print("Controls:")
        print("  'q' - Quit")
        print("  '+' or '=' - Zoom in (Camera 1)")
        print("  '-' - Zoom out (Camera 1)")
        print("  'r' - Reset zoom (Camera 1)")
        print("  'd' - Toggle depth view")
        print("  '3' - Show 3D reconstruction")
        print("  'i' - Toggle detection info")
    
    webcam_active = False
    no_face_start_time = None
    delay_before_close = 3.0  # seconds to wait before closing webcam when no face detected
    show_depth = False
    show_info = True
    detection_stats = {'objects': 0, 'faces': 0, 'total_detections': 0}
    
    while True:
        any_face_detected = False
        
        # Read from both cameras
        ret_left, left_img = cameras[0]['cap'].read()
        ret_right, right_img = cameras[1]['cap'].read()
        
        if not ret_left or not ret_right:
            print("Warning: Could not read frames from cameras, skipping...")
            continue
        
        # Handle calibration mode
        if calibration_mode:
            cv2.imshow('Left Camera (Calibration)', left_img)
            cv2.imshow('Right Camera (Calibration)', right_img)
            
            key = cv2.waitKey(1) & 0xFF
            if key == ord('c'):
                calibration_images_left.append(left_img.copy())
                calibration_images_right.append(right_img.copy())
                print(f"Captured calibration pair {len(calibration_images_left)}")
            elif key == ord('s') and len(calibration_images_left) >= 8:
                print("Starting calibration...")
                stereo_params = perform_stereo_calibration(calibration_images_left, calibration_images_right)
                if stereo_params:
                    save_stereo_calibration(stereo_params)
                    print("Calibration completed and saved!")
                    calibration_mode = False
                    # Initialize rectification maps
                    map1x, map1y = cv2.initUndistortRectifyMap(
                        stereo_params['K1'], stereo_params['D1'], 
                        stereo_params['R1'], stereo_params['P1'], 
                        stereo_params['image_size'], cv2.CV_32F
                    )
                    map2x, map2y = cv2.initUndistortRectifyMap(
                        stereo_params['K2'], stereo_params['D2'], 
                        stereo_params['R2'], stereo_params['P2'], 
                        stereo_params['image_size'], cv2.CV_32F
                    )
                else:
                    print("Calibration failed!")
            elif key == ord('q'):
                break
            continue
        
        # Stereo processing (only if calibrated)
        if stereo_params:
            # Rectify images
            left_rectified = cv2.remap(left_img, map1x, map1y, cv2.INTER_LINEAR)
            right_rectified = cv2.remap(right_img, map2x, map2y, cv2.INTER_LINEAR)
            
            # Compute disparity
            gray_left = cv2.cvtColor(left_rectified, cv2.COLOR_BGR2GRAY)
            gray_right = cv2.cvtColor(right_rectified, cv2.COLOR_BGR2GRAY)
            disparity = stereo_matcher.compute(gray_left, gray_right).astype(np.float32) / 16.0
            
            # Convert disparity to depth
            focal_length = stereo_params['P1'][0, 0]
            baseline = abs(stereo_params['T'][0])
            depth = (focal_length * baseline) / (disparity + 1e-6)
            depth[disparity <= 0] = 0
            
            # Use rectified images for display
            frames_to_process = [(left_rectified, 0), (right_rectified, 1)]
        else:
            frames_to_process = [(left_img, 0), (right_img, 1)]
            depth = None
        
        # Process each camera frame with AI detection
        for frame, camera_index in frames_to_process:
            
            # Perform AI object and person detection
            frame, detections = detect_objects_and_people(frame, yolo_model, face_cascade, camera_index, depth)
            
            # Check if any people/faces detected
            faces_detected = any(d['type'] == 'face' for d in detections)
            people_detected = any(d['type'] == 'object' and d['class'] == 'person' for d in detections)
            
            if faces_detected or people_detected:
                any_face_detected = True
                
                # Person detected - activate webcam
                if not webcam_active:
                    print("Person detected! Activating AI detection...")
                    webcam_active = True
                
                no_face_start_time = None  # Reset the timer
            
            # Update detection statistics
            if show_info:
                current_objects = len([d for d in detections if d['type'] == 'object'])
                current_faces = len([d for d in detections if d['type'] == 'face'])
                detection_stats['objects'] = current_objects
                detection_stats['faces'] = current_faces
                detection_stats['total_detections'] = current_objects + current_faces
                
                # Display detection info
                info_y = 60
                cv2.putText(frame, f"Objects: {current_objects}", (10, info_y), 
                           cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2)
                cv2.putText(frame, f"Faces: {current_faces}", (10, info_y + 25), 
                           cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2)
                cv2.putText(frame, f"Total: {detection_stats['total_detections']}", (10, info_y + 50), 
                           cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2)
            
            # Add camera identifier
            cv2.putText(frame, f"AI Camera {camera_index}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
            
            # Apply zoom to camera 1 if it's the movable camera
            if camera_index == 1 and zoom_level != 1.0:
                height, width = frame.shape[:2]
                
                # Calculate crop dimensions for zoom
                crop_width = int(width / zoom_level)
                crop_height = int(height / zoom_level)
                
                # Calculate center crop coordinates
                start_x = (width - crop_width) // 2
                start_y = (height - crop_height) // 2
                end_x = start_x + crop_width
                end_y = start_y + crop_height
                
                # Crop and resize to original size for zoom effect
                cropped = frame[start_y:end_y, start_x:end_x]
                frame = cv2.resize(cropped, (width, height))
                
                # Add zoom level indicator
                cv2.putText(frame, f"Zoom: {zoom_level:.1f}x", (10, height-20), 
                           cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
            
            # Show the webcam feed for this camera
            if webcam_active:
                if show_depth and depth is not None and camera_index == 0:
                    # Show depth map for left camera
                    depth_normalized = cv2.normalize(depth, None, 0, 255, cv2.NORM_MINMAX, cv2.CV_8U)
                    depth_colored = cv2.applyColorMap(depth_normalized, cv2.COLORMAP_JET)
                    cv2.imshow(f'AI Depth Map - Camera {camera_index}', depth_colored)
                else:
                    cv2.imshow(f'AI Object Detector - Camera {camera_index}', frame)
        
        # 3D reconstruction integration
        if stereo_params and depth is not None and any_face_detected:
            try:
                # Create Open3D images
                o3d_depth = o3d.geometry.Image(depth.astype(np.float32))
                o3d_color = o3d.geometry.Image(cv2.cvtColor(left_rectified, cv2.COLOR_BGR2RGB))
                
                # Create RGBD image
                rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
                    o3d_color, o3d_depth,
                    convert_rgb_to_intensity=False,
                    depth_trunc=3.0,
                    depth_scale=1.0
                )
                
                # Camera intrinsics
                height, width = left_rectified.shape[:2]
                fx = stereo_params['P1'][0, 0]
                fy = stereo_params['P1'][1, 1]
                cx = stereo_params['P1'][0, 2]
                cy = stereo_params['P1'][1, 2]
                intrinsics = o3d.camera.PinholeCameraIntrinsic(width, height, fx, fy, cx, cy)
                
                # Initialize TSDF volume on first frame
                if first_frame:
                    tsdf = o3d.pipelines.integration.ScalableTSDFVolume(
                        voxel_length=4e-3,
                        sdf_trunc=0.04,
                        color_type=o3d.pipelines.integration.TSDFVolumeColorType.RGB8
                    )
                    first_frame = False
                
                # Integrate into TSDF
                tsdf.integrate(rgbd, intrinsics, np.linalg.inv(pose))
                
            except Exception as e:
                print(f"3D reconstruction error: {e}")
        
        # Handle case when no faces detected on any camera
        if not any_face_detected:
            if webcam_active:
                if no_face_start_time is None:
                    no_face_start_time = time.time()
                elif time.time() - no_face_start_time > delay_before_close:
                    print("No people detected on any camera. Closing AI detection...")
                    webcam_active = False
                    cv2.destroyAllWindows()
        
        # Check for keyboard commands
        key = cv2.waitKey(1) & 0xFF
        if key == ord('q'):
            break
        elif key == ord('+') or key == ord('='):
            # Zoom in on camera 1
            if zoom_level < max_zoom:
                zoom_level = min(zoom_level + zoom_step, max_zoom)
                print(f"Zoom level: {zoom_level:.1f}x")
        elif key == ord('-'):
            # Zoom out on camera 1
            if zoom_level > min_zoom:
                zoom_level = max(zoom_level - zoom_step, min_zoom)
                print(f"Zoom level: {zoom_level:.1f}x")
        elif key == ord('r'):
            # Reset zoom on camera 1
            zoom_level = 1.0
            print("Zoom reset to 1.0x")
        elif key == ord('d'):
            # Toggle depth view
            show_depth = not show_depth
            print(f"Depth view: {'ON' if show_depth else 'OFF'}")
        elif key == ord('i'):
            # Toggle detection info
            show_info = not show_info
            print(f"Detection info: {'ON' if show_info else 'OFF'}")
        elif key == ord('3') and tsdf is not None:
            # Show 3D reconstruction
            try:
                mesh = tsdf.extract_triangle_mesh()
                mesh.compute_vertex_normals()
                o3d.visualization.draw_geometries([mesh])
            except Exception as e:
                print(f"Error showing 3D reconstruction: {e}")
    
    # Cleanup
    for camera in cameras:
        camera['cap'].release()
    cv2.destroyAllWindows()
    print("AI Object and Person Identifier closed.")

if __name__ == "__main__":
    detect_proximity_and_open_webcam()