two_frame_threading.py

import cv2
import threading
import queue
import time
import numpy as np

from superpointclass import SuperPointDescriptors

class CameraProcessor:
    def __init__(self, camera_index=0, max_queue_size=30):# 30 is default value
        self.camera_index = camera_index
        self.frame_queue = queue.Queue(maxsize=max_queue_size)
        self.result_queue = queue.Queue()
        
        # Control flags
        self.capture_running = False
        self.processing_running = False
        
        # Initialize camera
        self.cap = cv2.VideoCapture(camera_index)
        if not self.cap.isOpened():
            raise ValueError(f"Cannot open camera {camera_index}")
        
        # Set camera properties (optional)
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
        self.cap.set(cv2.CAP_PROP_FPS, 30)
        
        print(f"Camera initialized: {int(self.cap.get(cv2.CAP_PROP_FRAME_WIDTH))}x{int(self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT))}")

        # SuperPoint object
        self.superpoint = SuperPointDescriptors()
        self.resize_H = 240
        self.resize_W = 320

    
    def capture_frames(self):
        """Thread function to capture frames from camera"""
        print("Frame capture thread started")
        frame_count = 0
        
        while self.capture_running:
            ret, frame = self.cap.read()
            
            if not ret:
                print("Failed to capture frame")
                break
            
            # Add timestamp to frame
            timestamp = time.time()
            frame_data = {
                'frame': frame,
                'timestamp': timestamp,
                'frame_id': frame_count
            }

            # Always drop old frames, keep only latest
            # Use this technique but follow it up 
            # with queue.put_nowait() NOT just queue.put()
            while not self.frame_queue.empty():
                try:
                    self.frame_queue.get_nowait()
                except queue.Empty:
                    break
            
            try:
                # Put frame in queue (non-blocking)
                #self.frame_queue.put(frame_data, timeout=0.1)
                self.frame_queue.put_nowait(frame_data)

                frame_count += 1
            except queue.Full:
                # If queue is full, skip this frame
                print(f"Frame queue full, dropping frame {frame_count}")
        
        print("Frame capture thread stopped")
    
    def process_frames(self):
        """Thread function to process captured frames"""
        print("Frame processing thread started")
        
        while self.processing_running:
            try:
                # Get frame from queue
                frame_data = self.frame_queue.get(timeout=1.0)
                
                # Process the frame
                processed_result = self.process_single_frame(frame_data)
                
                # Store result
                try:
                    self.result_queue.put(processed_result, timeout=0.1)
                except queue.Full:
                    print("Result queue full, dropping result")
                
                # Mark task as done
                self.frame_queue.task_done()
                
            except queue.Empty:
                # No frame available, continue
                continue
            except Exception as e:
                print(f"Error processing frame: {e}")
        
        print("Frame processing thread stopped")

    def resize_image(self, image, height, width):
        
        img_size = [height, width]
        interp = cv2.INTER_AREA
        resize_image = cv2.resize(image, (img_size[1], img_size[0]), interpolation=interp)
        return resize_image
    
    def process_single_frame(self, frame_data):
        """Process a single frame - customize this method for your needs"""
        frame = frame_data['frame']
        frame_id = frame_data['frame_id']
        timestamp = frame_data['timestamp']
        
        # Example processing: Convert to grayscale and detect edges
        gray_1ch = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        gray_3ch = cv2.cvtColor(gray_1ch, cv2.COLOR_GRAY2BGR)

        resize_gray_ch1 = self.resize_image(gray_1ch, self.resize_H, self.resize_W)
        keypoints1, desc1 = self.superpoint.get_keypoints(resize_gray_ch1)
        keypoints2, desc2 = self.superpoint.get_keypoints(resize_gray_ch1)
        matches = self.superpoint.get_matches(desc1, desc2)

    
        # Create processed frame with annotations
        processed_frame = gray_3ch.copy()
       
        processing_time = time.time() - timestamp
        
        return {
            'frame_id': frame_id,
            'original_frame': frame,
            'processed_frame': processed_frame,
            'gray_image': gray_1ch,
            #'edges_frame': cv2.cvtColor(edges, cv2.COLOR_GRAY2BGR),
            #'edge_count': edge_count,
            #'contour_count': contour_count,
            'processing_time': processing_time,
            'timestamp': timestamp
        }
    
    def start(self):
        """Start capture and processing threads"""
        self.capture_running = True
        self.processing_running = True
        
        # Create and start threads
        self.capture_thread = threading.Thread(target=self.capture_frames, name="CaptureThread")
        self.process_thread = threading.Thread(target=self.process_frames, name="ProcessThread")

        self.capture_thread.start()
        self.process_thread.start()
        
        print("Camera processor started")
    
    def stop(self):
        """Stop capture and processing threads"""
        print("Stopping camera processor...")
        
        # Stop threads
        self.capture_running = False
        self.processing_running = False
        
        # Wait for threads to finish
        if hasattr(self, 'capture_thread'):
            self.capture_thread.join()
        if hasattr(self, 'process_thread'):
            self.process_thread.join()
        
        # Release camera
        self.cap.release()
        
        print("Camera processor stopped")
    
    def get_latest_result(self):
        """Get the most recent processing result"""
        try:
            return self.result_queue.get_nowait()
        except queue.Empty:
            return None
    
    def display_results(self):
        """Display processing results in real-time"""
        print("Starting display loop (Press 'q' to quit)")
        
        while True:
            result = self.get_latest_result()
            
            if result:
                # Display original and processed frames side by side
                original = result['original_frame']
                processed = result['processed_frame']
                #edges = result['edges_frame']
                
                # Resize frames for display
                height = 240
                width = int(height * original.shape[1] / original.shape[0])
                
                original_small = cv2.resize(original, (width, height))
                #processed_small = cv2.resize(processed, (width, height))
                #edges_small = cv2.resize(edges, (width, height))
                
                # Combine frames horizontally
                #combined = np.hstack([original_small, processed_small])
                
                cv2.imshow('Camera Processing', original_small)
                        
            # Check for quit key
            if cv2.waitKey(1) & 0xFF == ord('q'):
                break
            
            # Small delay to prevent excessive CPU usage
            time.sleep(0.01)
        
        cv2.destroyAllWindows()

def main():
    """Main function to run the camera processor"""
    try:
        # Create camera processor
        processor = CameraProcessor(camera_index=0, max_queue_size=5)
        
        # Start processing
        processor.start()
        
        # Display results (blocks until user quits)
        #
        # Tried making display_results a different thread
        # but didnt work because opencv imshow GUI. always need
        # to run from the main thread
        processor.display_results()
        
    except ValueError as e:
        print(f"Error: {e}")
        print("Make sure you have a camera connected")
    except KeyboardInterrupt:
        print("\nInterrupted by user")
    finally:
        # Clean up
        if 'processor' in locals():
            processor.stop()

if __name__ == "__main__":
    main()