How to Build a Face Tracking Pan Tilt Camera with OpenMV

2022-02-21 | By ShawnHymel

License: Attribution

Face tracking is an incredibly powerful technique used to identify the location (and possibly movement) of faces in an image. In this tutorial, we will show you how to use servos to physically track a face seen by an OpenMV Cam.

Hardware Build

You will need the following hardware components:

OpenMV Cam (one of H7, H7 R2, H7 PLUS)
Mini Maestro Servo Controller or Micro Maestro Servo Controller
Jumper Wires (M/F if you’re using the long headers on the OpenMV)
Servo motors (to match the pan/tilt mount, see note below)

You will need to make or buy a pan/tilt mount for your camera. I 3D-printed the following mount: https://www.thingiverse.com/thing:708819

You will also need some servos to attach to the mount. Pay attention to the sizing of the mount, as different mounts work with different servo motors. For the sub-micro G9 servo mount, I used something similar to these servos: https://www.digikey.com/en/products/detail/pololu-corporation/2818/10450036

Drill any necessary holes required to put the pan/tilt mount on a base, as you will likely find that it will not stand on its own. You will also likely want to drill holes to the camera portion of the mount so you can attach the OpenMV Cam.

Solder headers to the OpenMV Cam. I used the long female headers, as I wanted the option to add backpacks to the Cam module later.

Assemble the pan/tilt mount as per the instructions found on the designer’s or manufacturer’s page. Attach the mount to a base (I used a simple aluminum plate with drilled holes), and attach the OpenMV cam to the front.

Some pan/tilt mounts, like the one I used, do not have a way to attach the pan servo to the tilt part of the mount. Use some poster putty or hot glue to make a temporary connection.

Poster putty used to attach mount to servo

If you have longer jumper wires, you can place the Micro Maestro board off to the side. However, with shorter jumper wires, you will need to place the Micro Maestro board on the pan portion of the mount. Once again, use poster putty or hot glue for a temporary connection.

Wiring

Use the provided jumper to connect the VIN and VSRV pins on the bottom-right of the Micro Maestro board. This will allow you to power both the electronics and servos with a single power supply.

Connect the servo wires and jumper wires as follows. Note that you will need to provide separate power to the Micro Maestro board through the USB mini connector or through the + and - pins on the bottom-right corner of the board (by the jumper).

Face tracking servo connections

The following guides show the pinouts for the various boards:

Software

Navigate to https://openmv.io/. Download and install the latest OpenMV IDE. Accept all the defaults and install the necessary drivers.

Paste the following code into the OpenMV IDE:

Copy Code

import pyb
import sensor
import image
import time

# Status LED
led = pyb.LED(3)

# Pan servo settings
servo_pan_ch = 1        # Pan servo channel
servo_pan_speed = 0.8   # Relative movement multiplier
pulse_pan_min = 1000    # Pan minimum pulse (microseconds)
pulse_pan_max = 2000    # Pan maximum pulse (microseconds)

# Tilt servo settings
servo_tilt_ch = 0       # Tilt servo channel
servo_tilt_speed = 0.8  # Relative movement multiplier
pulse_tilt_min = 1000   # Tilt minimum pulse (microseconds)
pulse_tilt_max = 2000   # Tilt maximum pulse (microseconds)

# Other settings
threshold_x = 20        # Num pixels BB center x can be from CENTER_X
threshold_y = 20        # Num pixels BB center y can be from CENTER_Y
dir_x = 1               # Direction of servo movement (1 or -1)
dir_y = -1              # Direction of servo movement (1 or -1)
maestro_uart_ch = 1     # UART channel connected to Maestro board
maestro_num_ch = 12     # Number of servo channels on the Maestro board
baud_rate = 9600        # Baud rate of Mini Maestro servo controller
speed_limit = 15        # Speed limit (0.25 us)/(10 ms) of servos
accel_limit = 15        # Acceleration limit (0.25 us)/(10 ms)/(80 ms) of servos
speed_limit_min = 0     # Speed limit minimum
speed_limit_max = 10000 # Speed limit maximum
accel_limit_min = 0     # Acceleration limit minimum
accel_limit_max = 255   # Acceleration limit maximum

# Commands (for talking to Maestro servo controller)
baud_detect_byte = 0xAA;
maestro_dev_num = 12;
cmd_set_target = 0x04
cmd_set_speed = 0x07
cmd_set_accel = 0x09

###############################################################################
# Functions

def servo_send_cmd(cmd, ch, payload):
    """
    Send generic compact protocol command to servo controller:
    | cmd | ch | msg lsb | msg msb |
    """

    # Check that channel is in range
    if (ch < 0) or (ch >= maestro_num_ch):
        return

    # Construct message
    msg = bytearray()
    msg.append(baud_detect_byte)
    msg.append(maestro_dev_num)
    msg.append(cmd)
    msg.append(ch)
    msg.append(payload & 0x7F)
    msg.append((payload >> 7) & 0x7F)

    # Send a message
    uart.write(msg)

def servo_set_target(ch, pulse):
    """
    Write pulse width (in microseconds) to given channel to control servo.
    """

    # Pulse number is 4x pulse width (in microseconds)
    p_num = 4 * int(pulse)

    # Send command to servo controller
    servo_send_cmd(cmd_set_target, ch, p_num)

def servo_set_speed_limit(ch, speed):
    """
    Set speed limit of servo on a given channel.
    """

    # Check to make sure speed is in range
    speed = max(speed, speed_limit_min)
    speed = min(speed, speed_limit_max)

    # Send command to servo controller
    servo_send_cmd(cmd_set_speed, ch, speed)

def servo_set_accel_limit(ch, accel):
    """
    Set accel limit of servo on a given channel.
    """

    # Check to make sure speed is in range
    speed = max(accel, accel_limit_min)
    speed = min(accel, accel_limit_max)

    # Send command to servo controller
    servo_send_cmd(cmd_set_accel, ch, accel)

###############################################################################
# Main

# Configure camera
sensor.reset()
sensor.set_contrast(3)
sensor.set_gainceiling(16)
sensor.set_framesize(sensor.QVGA)
sensor.set_pixformat(sensor.GRAYSCALE)

# Get center x, y of camera image
WIDTH = sensor.width()
HEIGHT = sensor.height()
CENTER_X = int(WIDTH / 2 + 0.5)
CENTER_Y = int(HEIGHT / 2 + 0.5)

# Pour a bowl of serial
uart = pyb.UART(maestro_uart_ch, baud_rate)

# Start clock
clock = time.clock()

# Create cascade for finding faces
face_cascade = image.HaarCascade("frontalface", stages=25)

# Set servo speed limits on servos
servo_set_speed_limit(servo_pan_ch, speed_limit)
servo_set_speed_limit(servo_tilt_ch, speed_limit)

# Set servo accel limits on servos
servo_set_accel_limit(servo_pan_ch, accel_limit)
servo_set_accel_limit(servo_tilt_ch, accel_limit)

# Initial servo positions
servo_pos_x = int(((pulse_pan_max - pulse_pan_min) / 2) + pulse_pan_min)
servo_pos_y = int(((pulse_tilt_max - pulse_tilt_min) / 2) + pulse_tilt_min)

# Superloop
while(True):

    # Take timestamp (for calculating FPS)
    clock.tick()

    # Take photo
    img = sensor.snapshot()

    # Find faces in image
    objects = img.find_features(face_cascade, threshold=0.75, scale_factor=1.25)

    # Find largest face in image
    largest_face_size = 0
    largest_face_bb = None
    for r in objects:

        # Find largest bounding box
        face_size = r[2] * r[3]
        if (face_size > largest_face_size):
            largest_face_size = face_size
            largest_face_bb = r

        # Draw bounding boxes around all faces
        img.draw_rectangle(r)

    # Find distance from center of face to center of frame
    if largest_face_bb is not None:

        # Turn on status LED
        led.on()

        # Print out the largest face info
        print("Face:", largest_face_bb)

        # Find x, y of center of largest face in image
        face_x = largest_face_bb[0] + int((largest_face_bb[2]) / 2 + 0.5)
        face_y = largest_face_bb[1] + int((largest_face_bb[3]) / 2 + 0.5)

        # Draw line from center of face to center of frame
        img.draw_line(CENTER_X, CENTER_Y, face_x, face_y)

        # Figure out how far away from center the face is (minus the dead zone)
        diff_x = face_x - CENTER_X
        if abs(diff_x) <= threshold_x:
            diff_x = 0
        diff_y = face_y - CENTER_Y
        if abs(diff_y) <= threshold_y:
            diff_y = 0

        # Calculate the relative position the servos should move to based on distance
        mov_x = dir_x * servo_pan_speed * diff_x
        mov_y = dir_y * servo_tilt_speed * diff_y

        # Adjust camera position left/right and up/down
        servo_pos_x = servo_pos_x + mov_x
        servo_pos_y = servo_pos_y + mov_y

        # Constrain servo positions to range of servos
        servo_pos_x = max(servo_pos_x, pulse_pan_min)
        servo_pos_x = min(servo_pos_x, pulse_pan_max)
        servo_pos_y = max(servo_pos_y, pulse_pan_min)
        servo_pos_y = min(servo_pos_y, pulse_pan_max)

        # Set pan/tilt
        print("Moving to X:", int(servo_pos_x), "Y:", int(servo_pos_y))
        servo_set_target(servo_pan_ch, servo_pos_x)
        servo_set_target(servo_tilt_ch, servo_pos_y)


    # If there are no faces, don't do anything
    else:

        # Turn off status LED
        led.off()

    # Print FPS
    print("FPS:", clock.fps())

Code Explanation

Let’s take a few moments to examine what’s happening in the face tracking code.

The servo_send_cmd() function constructs a message to send to the Micro Maestro board over UART. If you look at the Serial Servo Commands section of the Maestro user guide, you can see the various commands supported by the Maestro board. We’re only going to use a few: set target, set speed, and set acceleration.

We construct each message by simply putting together a list of bytes in Python before using the built-in uart library to transmit the whole bytearray:

Copy Code

# Construct message
msg = bytearray()
msg.append(baud_detect_byte)
msg.append(maestro_dev_num)
msg.append(cmd)
msg.append(ch)
msg.append(payload & 0x7F)
msg.append((payload >> 7) & 0x7F)

# Send a message
uart.write(msg)

By default, the Maestro boards are configured with autobaud detection. You must send 0xAA as the first byte of every message so that this mechanism will detect and set the correct baud rate. You can change this with the Maestro Control Center, which will allow you to use the Compact Protocol.

The next 3 functions each call servo_send_cmd() with the required command byte and perform any necessary pre-processing for the payload:

servo_set_target() moves the servo on channel ch to a position given by pulse. The payload is 4 * pulse (where pulse is the pulse width in microseconds)
servo_set_speed_limit() sets the maximum speed of a given servo channel
servo_set_accel_limi() sets the maximum acceleration of a given servo channel

You can play with the maximum speed and acceleration limits to make the servos move in particular ways (e.g. more lifelike motions). Rather than needing to handle all the calculations to limit a servo’s speed in our OpenMV, we can just let the Maestro board do it for us!

Before the main loop, we set up the camera, UART, and Haar Cascade for finding faces. We also set the servo_set_speed_limit and servo_set_speed_limit to make the servo motions smoother:

Copy Code

# Create cascade for finding faces
face_cascade = image.HaarCascade("frontalface", stages=25)

# Set speed limits on servos
servo_set_speed_limit(servo_pan_ch, speed_limit)
servo_set_speed_limit(servo_tilt_ch, speed_limit)

# Set acceleration limits on servos
servo_set_accel_limit(servo_pan_ch, accel_limit)
servo_set_accel_limit(servo_tilt_ch, accel_limit)

In the main loop, we capture an image from the camera and use the cascade to identify any faces in the image:

Copy Code

# Find faces in image
objects = img.find_features(face_cascade, threshold=0.75, scale_factor=1.25)

The find_features() function returns a list of bounding boxes that identify all of the faces in the image. We can loop through them to identify the largest one (by area):

Copy Code

# Find largest face in image
largest_face_size = 0
largest_face_bb = None
for r in objects:
    
    # Find largest bounding box
    face_size = r[2] * r[3]
    if (face_size > largest_face_size):
        largest_face_size = face_size
        largest_face_bb = r

    # Draw bounding boxes around all faces
    img.draw_rectangle(r)

From there, we find the center of the largest face in the list:

Copy Code

# Find x, y of center of largest face in image
face_x = largest_face_bb[0] + int((largest_face_bb[2]) / 2 + 0.5)
face_y = largest_face_bb[1] + int((largest_face_bb[3]) / 2 + 0.5)

Then, we figure out how far away that face is from the center of the frame (in the X and Y direction). We set that difference to 0 if the face is within a threshold (e.g. 20 pixels from the center). This creates a deadzone so that the servos do not oscillate while looking at a face.

Copy Code

# Figure out how far away from center the face is (minus the dead zone)
diff_x = face_x - CENTER_X
if abs(diff_x) <= threshold_x:
    diff_x = 0
diff_y = face_y - CENTER_Y
if abs(diff_y) <= threshold_y:
    diff_y = 0

Rather than figure out the exact degrees that a servo should move to face the center of the largest bounding box, we arbitrarily set a “speed” value that we multiply the number of pixels by:

Copy Code

# Caclulate how fast the servo should move based on distance
mov_x = dir_x * servo_pan_speed * diff_x
mov_y = dir_y * servo_tilt_speed * diff_y

In this example, servo_pan_speed is 0.8 and servo_tilt_speed is also 0.8. The “dir” values are either 1 or -1, and they allow us to control the direction of a servo (depending on how it was mounted):

The current servo position (servo_pos_x and servo_pos_y) are updated with these differences and clamped to the minimum (1000) or maximum (2000) pulse value:

Copy Code

# Adjust camera position left/right and up/down
servo_pos_x = servo_pos_x + mov_x
servo_pos_y = servo_pos_y + mov_y
       
# Constrain servo positions to range of servos
servo_pos_x = max(servo_pos_x, pulse_pan_min)
servo_pos_x = min(servo_pos_x, pulse_pan_max)
servo_pos_y = max(servo_pos_y, pulse_tilt_min)
servo_pos_y = min(servo_pos_y, pulse_tilt_max)

Finally, the servos are given the new position values:

Copy Code

# Set pan/tilt
print("Moving to X:", int(servo_pos_x), "Y:", int(servo_pos_y))
servo_set_target(servo_pan_ch, servo_pos_x)
servo_set_target(servo_tilt_ch, servo_pos_y)

Due to the speed and acceleration limits, the servos will move much more slowly to these targets, which makes our face tracking robot smoother and less jerky!

Run

Make sure you the have servos connected to your Maestro board have 5V and that your OpenMV Cam is plugged in via USB. Click the Connect button in the bottom-left of the OpenMV IDE and then click the Start button to run the script. You should see your pan-tilt mount spring to life and begin tracking faces!

Tracking a face in OpenMV

Going Further

This is just the beginning of some fun projects! You can track faces during a conference call while someone walks around the room. Or maybe you want to track faces to let someone know they’re too close. Jayy has used this face tracking in a few of his companion bots, like Helen!

Add all DigiKey Parts to Cart

Have questions or comments? Continue the conversation on TechForum, DigiKey's online community and technical resource.

Visit TechForum

Arduino | 3D Printing | Raspberry Pi

How to Build a Face Tracking Pan Tilt Camera with OpenMV

2022-02-21 | By ShawnHymel

信息

帮助

联系我们

关注我们