Week 6 Object Detection Complete - tom-howard/com2009 GitHub Wiki

Week 6 OpenCV Basics

Object Detection Node Complete

Here is a complete worked example of the object_detection.py node that you should have developed during The Object Detection Exercise. Also included here is an illustration of how to use the cv2.circle() method to create a marker on an image illustrating the centroid of the detected feature, as discussed here.

The Code

#!/usr/bin/env python3

import rospy
from pathlib import Path

import cv2
from cv_bridge import CvBridge, CvBridgeError

from sensor_msgs.msg import Image

node_name = "object_detection_node"
rospy.init_node(node_name)
print(f"Launched the '{node_name}' node. Currently waiting for an image...")
rate = rospy.Rate(5)

base_image_path = Path("/home/student/myrosdata/week6_images")
base_image_path.mkdir(parents=True, exist_ok=True)

cvbridge_interface = CvBridge()

waiting_for_image = True

def show_and_save_image(img, img_name):
    full_image_path = base_image_path.joinpath(f"{img_name}.jpg")

    cv2.imshow(img_name, img)
    cv2.waitKey(0)

    cv2.imwrite(str(full_image_path), img)
    print(f"Saved an image to '{full_image_path}'\n"
        f"image dims = {img.shape[0]}x{img.shape[1]}px\n"
        f"file size = {full_image_path.stat().st_size} bytes")

def camera_cb(img_data):
    global waiting_for_image  
    try:
        cv_img = cvbridge_interface.imgmsg_to_cv2(img_data, desired_encoding="bgr8")
    except CvBridgeError as e:
        print(e)

    if waiting_for_image == True:
        height, width, channels = cv_img.shape

        print(f"Obtained an image of height {height}px and width {width}px.")

        show_and_save_image(cv_img, img_name = "step1_original")

        crop_width = width - 400
        crop_height = 400
        crop_y0 = int((width / 2) - (crop_width / 2))
        crop_z0 = int((height / 2) - (crop_height / 2))
        cropped_img = cv_img[crop_z0:crop_z0+crop_height, crop_y0:crop_y0+crop_width]

        show_and_save_image(cropped_img, img_name = "step2_cropping")

        hsv_img = cv2.cvtColor(cropped_img, cv2.COLOR_BGR2HSV)
        lower_threshold = (115, 225, 100)
        upper_threshold = (130, 255, 255)
        img_mask = cv2.inRange(hsv_img, lower_threshold, upper_threshold)

        show_and_save_image(img_mask, img_name = "step3_image_mask")

        filtered_img = cv2.bitwise_and(cropped_img, cropped_img, mask = img_mask)

        m = cv2.moments(img_mask)
        cy = m['m10'] / (m['m00'] + 1e-5)
        cz = m['m01'] / (m['m00'] + 1e-5)
        cv2.circle(filtered_img, (int(cy), int(cz)), 10, (0, 0, 255), 2)

        show_and_save_image(filtered_img, img_name = "step4_filtered_image")

        waiting_for_image = False

rospy.Subscriber("/camera/rgb/image_raw", Image, camera_cb)

while waiting_for_image:
    rate.sleep()

cv2.destroyAllWindows()

The Code Explained:

Everything here should be familiar to you from the previous exercise, except for this new bit:

m = cv2.moments(img_mask)
cy = m['m10'] / (m['m00'] + 1e-5)
cz = m['m01'] / (m['m00'] + 1e-5)
cv2.circle(filtered_img, (int(cy), int(cz)), 10, (0, 0, 255), 2)

Here, we are obtaining the moments of our colour blob by providing the boolean representation of it (i.e. the img_mask) to the cv2.moments() function.

Then, we are determining where the central point of this colour blob is located by calculating the cy and cz coordinates of it. This provides us with pixel coordinates relative to the top left-hand corner of the image.

Finally, We can use the cv2.circle() function to draw a circle on our image at the centroid location so that we can visualise it. Into this function we pass:

  1. The image that we want the circle to be drawn on. In this case: filtered_img.
  2. The location that we want the circle to be placed, specifying the horizontal and vertical pixel coordinates respectively: (int(cy), int(cz)).
  3. How big we want the circle to be: here we specify a radius of 10 pixels.
  4. The colour of the circle, specifying this using a Blue-Green-Red colour space: (0, 0, 255) (i.e.: pure red in this case)
  5. Finally, the thickness of the line that will be used to draw the circle, in pixels.