Урок 19

Введение: Данный урок, является первым из группы уроков на использование компьютерного зрения для работы с полезной нагрузкой полигона. В прошлых уроках по компьютерному зрению мы изучали теоретические аспекты обработки изображений и немного попрактиковались на "сферическом коне в вакууме" - желтом шарике. В этом уроке мы перейдем к более сложным задачам больше приближенным к реальности и первой из них будет определение положения шлагбаума на изображении.

В рамках данного урока мы также как и ранее получим изображение с камеры робота, переведем его в HSV и наложим маску. Собственно на этом вся предобработка изображения будет закончена. А далее для решения семантической задачи определения поднят ли шлагбаум на видео или опущен, мы будем сначала детектировать полоски шлагбаума на изображении в виде контуров, а затем аппроксимировать прямой линией координаты центров этих контуров по методу наименьших квадратов. И в результате по значению коэффициентов функции определяющих аппроксимирующую прямую мы и будем делать вывод как расположен шлагбаум - более параллельно или более перпендикулярно. Для визуализации результатов анализа изображения, мы наложим найденные контуры и рассчитанную по МНК линию на изначальное изображение и опубликуем это изображение в специальный топик.

Теория:

Подготовка: Данную задачу можно решать в комбинации полигона "Дорога". Непосредственно перед запуском программы попросите учащихся подвести робота к шлагбауму.

Задача: Определить координаты центра желтого шарика на изображении и вывести их в консоль.

Общий алгоритм решения:

1. Получить изображение с камеры робота
2. Преобразовать его в HSV (взять алгоритм из предыдущего урока)
3. Определить цветовые границы
4. Наложить цветовую маску - красного цвета (взять алгоритм из предыдущего урока)
5. Выделить элементы полосок шлагбаума
6. Найти формулу прямой аппроксимирующей центры найденных полосок по методу наименьших квадратов
7. Оценить как расположена аппроксимирующая прямая, если параллельно оси X - то значит шлагбаум закрыт, если перпендикулярно - значит открыт.
8. Опубликовать результаты оценки состояния шлагбаума в отдельный топик

Решение: Возьмем весь код программы прошлого урока, реализующий наложение маски на изображение и сохраним его в новом файле.

import rospy
import numpy as np
import cv2
from cv_bridge import CvBridge

from std_msgs.msg import String
from sensor_msgs.msg import CompressedImage


class BarrierDetector():

    def __init__(self):
        self.hue_red_l = 90
        self.hue_red_h = 180
        self.saturation_red_l = 128
        self.saturation_red_h = 255
        self.lightness_red_l = 90
        self.lightness_red_h = 255

        self.sub_image_original = rospy.Subscriber('/front_camera/compressed', CompressedImage, self.cbGetImage, queue_size = 1)
        self.detect_pub = rospy.Publisher('/barrier_detect', String, queue_size = 1)
        self.image_pub = rospy.Publisher('/barrier_image/compressed', CompressedImage, queue_size = 1)

        self.counter = 0

        self.cvBridge = CvBridge()

        while not rospy.is_shutdown():
            rospy.sleep(1)
            self.BarrierDetect()

    def cbGetImage(self, image_msg):
        # Change the frame rate by yourself. Now, it is set to 1/3 (10fps). 
        # Unappropriate value of frame rate may cause huge delay on entire recognition process.
        # This is up to your computer's operating power.
        if self.counter % 3 != 0:
            self.counter += 1
            return
        else:
            self.counter = 1

        np_arr = np.frombuffer(image_msg.data, np.uint8)
        self.cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)

    def ApplyRedMaskOnIm(self):
        image = np.copy(self.cv_image)
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

        # define range of red color in HSV
        lower_red = np.array([self.hue_red_l, self.saturation_red_l, self.lightness_red_l])
        upper_red = np.array([self.hue_red_h, self.saturation_red_h, self.lightness_red_h])

        # Threshold the HSV image to get only red colors
        mask = cv2.inRange(hsv, lower_red, upper_red)
        return mask

    def FindRectOnBarrier(self, mask):
        params=cv2.SimpleBlobDetector_Params()
        # Change thresholds
        params.minThreshold = 200
        params.maxThreshold = 255

        params.filterByColor = True
        params.blobColor = 255

        # Filter by Area.
        params.filterByArea = True
        params.minArea = 300
        params.maxArea = 5000

        # Filter by Circularity
        params.filterByCircularity = False

        # Filter by Convexity
        params.filterByConvexity = False

        det=cv2.SimpleBlobDetector_create(params)
        keypts=det.detect(mask)
        if len(keypts) >= 2:
            return keypts
        else:
            return []

    def FindTheLineWithMSE(self, keypts):
        #precalc
        if keypts:
            C1,C2,C3,C4 = 0,0,0,0
            for keypt in keypts:
                #sum(X)
                C1 += keypt.pt[0]
                #sum(Y)
                C2 += keypt.pt[1]
                #sum(X*X)
                C3 += keypt.pt[0]*keypt.pt[0]
                #sum(X*Y)
                C4 += keypt.pt[0]*keypt.pt[1]
            #n = number of points
            n = len(keypts)
            #Line сoefficients calculation according to MSE formulas 
            a = (C4*n - C1*C2) / (n*C3 - C1*C1)
            b = (C2- C1*a) / n #for drawing purpose only. "a" is enough for detection.
            return a,b
        else:
            return 0,0

    def isBarrierClosed(self, a,b, rects):
        if a < -2:
            return "Open"
        elif a == 0:
            return "Unknown"
        else:
            return "Closed"

    def ResultPub(self, result):
        r = String()
        r.data = result
        self.detect_pub.publish(r)

    def ImPub(self, rects, a, b):
        x_1, y_1 = 0,0
        x_2, y_2 = 1,1
        if rects:
            x_1, y_1 = int(rects[0].pt[0]), int(rects[0].pt[0]*a + b)
            x_2, y_2 = int(rects[-1].pt[0]), int(rects[-1].pt[0]*a + b)
        image_to_publish = cv2.drawKeypoints(self.cv_image,rects,np.array([]),(0,255,255),cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
        image_to_publish = cv2.line(image_to_publish, (x_1, y_1),(x_2, y_2), (255,0,0), 5)
        #cv2.imwrite("/home/pi/projects/3.jpg", image_to_publish)
        self.image_pub.publish(self.cvBridge.cv2_to_compressed_imgmsg(image_to_publish,"jpg"))

    def BarrierDetect(self):
        cv_image_mask = self.ApplyRedMaskOnIm()
        cv_image_mask = cv2.GaussianBlur(cv_image_mask,(5,5),0)
        rects= self.FindRectOnBarrier(cv_image_mask)
        a,b = self.FindTheLineWithMSE(rects)
        barrier_is_closed = self.isBarrierClosed(a,b, rects)
        self.ResultPub(barrier_is_closed)
        self.ImPub(rects, a ,b)

    def main(self):
        rospy.spin()

if __name__ == '__main__':
    rospy.init_node('detect_barrier')
    b = BarrierDetector()
    b.main()

Проверка решения:

Попросите учащихся сохранить программу на роботе, подвести робота к шлагбауму и запустить программу. После запуска программы, при помощи веб-интерфейса, открывайте и закрывайте шлагбаум перед роботом. Попросите учащихся через Discord продемонстрировать вывод в консоль значения данных из топика, в который публикуются результаты определения состояния шлагбаума.

Итак, мы с вами применили встроенные в OpenCV функции по обработке изображения, а так же написали наш собственный семантический алгоритм проверки состояния шлагбаума основанный на методе наименьших квадратов.