Code details

Main Code Components

1. Camera Setup and Initialization

The code begins by setting up the Picamera2 camera object to capture frames. More details at Camera Setup.

2. OpenCV Trackbars for Real-Time Adjustments

Trackbars are created to control color filtering, thresholding, and morphology parameters. These trackbars allow users to adjust settings dynamically, which is particularly useful for fine-tuning the image processing parameters.

# Create OpenCV window
cv2.namedWindow("Color, Threshold, Morphology Filter")

# Create trackbars for color filtering
cv2.createTrackbar("Blue", window_name, blue_filter, max_value, lambda x: None)
cv2.createTrackbar("Green", window_name, green_filter, max_value, lambda x: None)
cv2.createTrackbar("Red", window_name, red_filter, max_value, lambda x: None)

# Create trackbars for thresholding
cv2.createTrackbar('Threshold Type', window_name, threshold_type, max_threshold_type, lambda x: None)
cv2.createTrackbar('Threshold Value', window_name, threshold_value, max_threshold_value, lambda x: None)

# Create trackbars for morphology operations
cv2.createTrackbar('Element Shape', window_name, kernel_shape, max_elem, lambda x: None)
cv2.createTrackbar('Kernel Size', window_name, kernel_size, max_kernel_size, lambda x: None)

# Create trackbars for amplitude threshold for computing period
cv2.createTrackbar('Amplitude percent', window_name, default_ampl_thrs, max_ampl_thrs, lambda x: None)

3. Threading and Queue System

The code uses two threads to handle frame capturing and processing independently, allowing for smooth real-time operation. Queues are used to manage data flow between these threads, preventing bottlenecks and ensuring synchronization.

# Initialize queues for communication between threads
frame_queue = queue.Queue(maxsize=10)
processed_queue = queue.Queue(maxsize=10)

4. Signal Handling

To allow for a graceful exit, the code registers a signal handler to stop the camera, close OpenCV and Matplotlib windows, and exit the program cleanly when Ctrl+C or a termination signal is received.

def signal_handler(sig, frame):
    print("\nExiting gracefully...")
    picam2.stop()
    cv2.destroyAllWindows()
    plt.close('all')
    sys.exit(0)

# Set up signal handling for SIGINT and SIGTERM
signal.signal(signal.SIGINT, signal_handler)
signal.signal(signal.SIGTERM, signal_handler)

Code Structure and Thread Functions

1. Capture Thread (capture_frames)

This thread captures frames from the camera and places them in the frame_queue along with a timestamp. It only pauses when the queue is full, ensuring that captured frames are continuously available for processing.

def capture_frames():
    while True:
        frame = picam2.capture_array()
        if frame_queue.full():
            continue
        frame_time = time() - init_time
        frame_queue.put((frame, frame_time))

2. Processing Thread (process_frames)

The processing thread retrieves frames from frame_queue and performs color filtering, thresholding, and morphological transformations on each frame. It calculates the pendulum’s centroid, which is added to the processed_queue for display and further analysis.

def process_frames():
    while True:
        frame, frame_time = frame_queue.get()
        if frame is None:
            break

        # ... Image processing part (More on Image Processing wiki part)

        # Calculate centroid
        try:
            M = cv2.moments(dilated_im)
            cX = int(M["m10"] / M["m00"])
            cY = int(M["m01"] / M["m00"])
            cv2.circle(dilated_im, (cX, cY), 5, (0, 0, 255), -1)
        except ZeroDivisionError:
            cX, cY = -1, -1

        # Send processed frame to main thread
        if processed_queue.full():
            continue 
        processed_queue.put((dilated_im, cX, frame_time))

3. Main Thread (Plotting and Display)

The main thread retrieves processed frames from processed_queue and updates the position plot in real-time. It calculates the amplitude and period of the pendulum’s motion and estimates the gravitational constant g based on these values.

while True:
    try:
        # Retrieve processed frame and centroid data
        processed_frame,  cX, current_time = processed_queue.get(timeout=1)
        
        # Plotting the centroid positions over time
        if cX >= 0:
            pos.append(cX)
            timev.append(current_time)
            ampl_perct = cv2.getTrackbarPos('Amplitude percent', window_name) / max_ampl_thrs

            # Detecting oscillations by checking time intervals between two maximums
            if len(pos) > 10:
                max_pos = pos[-1]
                min_pos = max_pos
                for i in range(1, 10):
                    max_pos = max(max_pos, pos[-i])
                    min_pos = min(min_pos, pos[-i])
                amplitude = max_pos - min_pos
                ampl_abs = max_pos - ampl_perct * amplitude

                # Calculating g based on last 10 oscillations
                if len(periods) >= 10:
                    avg_period = sum(periods) / len(periods)
                    g = 4 * np.pi**2 * L / avg_period**2
                    print(f"Estimated gravity: {g:.4f} m/s²")
                    
            # Plot
            plt.clf()
            plt.plot(timev, pos, color='b')
            plt.axhline(y=ampl_abs, color='r', linestyle='--', label='Amplitude Threshold') 
            plt.title("Centroid X position over time")
            plt.xlabel("Time (s)")
            plt.ylabel("X Position")
            plt.draw()
            plt.pause(0.001)

        # Display processed image using OpenCV
        cv2.imshow(window_name, processed_frame) 
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    except queue.Empty:
        continue

Amplitude and Period Calculation

1. Amplitude Threshold

   • The amplitude threshold is determined by the "Amplitude percent" trackbar, allowing the program to detect peaks based on a percentage of the calculated amplitude.

2. Detecting Oscillation Peaks

   • The code calculates the amplitude by finding the maximum and minimum positions over recent frames. This difference represents the pendulum's full swing.
   • Peaks are detected based on the threshold to identify full oscillations, which are used to calculate periods.

   

   The amplitude threshold is the line in red.

3. Estimating Gravity

   • The gravitational constant g is estimated based on the average period of the last 10 oscillations:

$$g = \frac{4 \pi^2 L}{T_{avg}^2}$$

Where $L$ is the pendulum length (0.7 meters), and T_avg is the average period.