Измерение расстояния до объекта и его скорости

Технологию, которую я собираюсь Вам представить, я не встречал в найденных мной методах определения расстояния до объекта на изображении. Она не является ни универсальной, ни сложной, суть её заключается в том, что видимое поле (будем считать, что мы используем видеокамеру) калибруется линейкой и затем сопоставляется координата объекта на изображении с отметкой на линейке. То есть измерение ведётся по одной линии или оси. Но нам не нужно хранить отметку на линейке для каждого пикселя, алгоритму для калибровки нужно только знать размер линейки в пикселях и в метрах, а также координату пикселя, который является фактической серединой линейки. Очевидное ограничение — работает только на плоских поверхностях.

Кроме самого метода в статье рассмотрена его реализация на языке Python с использованием библиотеки OpenCV, а также рассмотрены особенности получения изображений с вебкамер в Linux, используя video4linux2 API.

На практике нужно было измерить расстояние до автомобиля и его скорость на каком-нибудь прямом участке дороги. Я использовал длинную рулетку, растягивал её в доль дороги, по середине полотна, затем настраивал камеру так, чтобы вся рулетка как раз входила в поле зрение камеры и была выравнена с осью X изображения. Следующим шагом было положить что-нибудь яркое на середину рулетки, закрепить камеру так, чтобы она никуда не съехала, и записать координаты пикселя этой середины.

Все расчёты сводятся к одной единственной формуле:
l = L*K / ( W/x — 1 + K ), где
 l – искомое расстояние до объекта, м;
 L – длина «линейки», м;
 W – длина «линейки» в пикселях, обычно совпадает с шириной изображения;
 x – координата объекта на изображении;
 K = (W — M) / M – коэффициент, отражающий наклон камеры, здесь M – координата середины «линейки».

В выводе этой формулы мне очень пригодились школьные знания тригонометрии.

График зависимости этой функции приведён на рисунке:

Чем больше наклон камеры, тем круче идёт график. В граничном случае, когда ось камеры направлена перпендикулярно плоскости «линейки» ( M = W / 2), график становится прямой линией.

Но статья была бы слишком короткой, если бы на этом и остановиться. Поэтому я решил сделать демонстрационную программу, которая бы подключалась к вебкамере компьютера и следила бы за каким-нибудь объектом, вычисляя расстояние до него и его скорость. В качестве языка программирования я выбрал Python, язык с очень большим количеством достоинств, для построения графического интерфейса был выбрал фреймворк Tkinter, идущий вместе с Python, так что его не нужно устанавливать отдельно. Для слежения за объектом хорошо подходит OpenCV, я использую версию 2.2, но в репозитории текущей версии ubuntu (10.10) имеется только версия 2.1, а у них API немного изменилось в лучшую сторону и программа под версией 2.1 не заработает. В принципе можно было бы построить всю программу на OpenCV, возложив на неё функции графического интерфейса и захвата изображения, но я хотел отделить её от основной части программы, чтобы можно было если что заменить эту библиотеку на что-нибудь другое или просто убрать, выключив слежение. Я начал перерабатывать старую программу, удаляя всё ненужное, и на моё удивление от программы осталось всего несколько строк с непосредственным расчётом расстояния и скорости, что в принципе было логично, так как в оригинале программа не использует графический интерфейс, следит за автомобилем по другому алгоритму да и вместо вебкамеры используется мегапиксельная сетевая камера с подключением по RTSP.

Что касается получения изображений с вебкамеры, то тут не всё так просто. Под Windows программа использует DirectX для подключения к камере через библиотеку VideoCapture, здесь всё достаточно просто. Но под Linux внятных статей об использовании вебкамер из Python очень мало, а те примеры что есть как правило оказываются неработоспособными из-за какой-нибудь очередной смены API. В прошлом я использовал ffmpeg для этих целей и программа была на C, но ffmpeg это немного «по воробьям из пушки», да и дополнительными зависимостями не хотелось отягощать конечную программу. Можно было воспользоваться OpenCV, которая так же использует ffmpeg, но был выбран путь написания собственной обёртки video4linux2 API для Python.

За основу были взяты исходные коды со страницы какого-то факультета науки. Из них я быстро удалил всё ненужное для моей цели, в итоге оставив два отредактированных файла: V4L2.cpp и V4L2.h. Это собственно и есть минимально необходимое API для подключения к вебкамере. В ходе работы над обёрткой для Python я выяснил, что к video4linux2 устройствам можно обращаться тремя способами: READ, MMAP и STREAM, но с моими вебкамерами работает только MMAP метод. Как выяснилось другие примеры программ, которые у меня не заработали, использовали метод READ.

Также подразумевается, что вебкамера отдаёт изображение в YUYV формате (YUV422), от RGB он отличается тем, что цветовой информации в нём в 2 раза меньше. В YUYV два пикселя кодируются 4 байтами, а в RGB шестью, отсюда экономия в полтора раза. Y — компонента яркости, для каждого пикселя она своя. U и V — цветоразностные компоненты, которые определяют цвет пикселя, так вот каждые два пикселя используют одни и те же значения U и V. Если представить поток байт от вебкамеры в этих обозначениях, то он будет выглядеть как YUYV YUYV YUYV YUYV YUYV YUYV — это 12 пикселей. Выяснить в каком формате у Вас работает вебкамера можно с помощью VLC плеера, открываете захватывающее устройство с его помощью и затем запрашиваете информацию о кодеке, должно быть как на рисунке:

Вот так выглядит исходный код библиотеки для доступа к вебкамере:
main_v4l2.cpp

#include "V4L2.h"
#include <cstring>
#include <iostream>

using namespace std;

extern "C" {

// Specify the video device here
V4L2 v4l2("/dev/video0");

unsigned char *rgbFrame;

float clamp(float num) {
    if (num < 0) num = 0;
    if (num > 255) num = 255;  
    return num;
}

// Convert between YUV and RGB colorspaces
void yuv2rgb(unsigned char y, unsigned char u, unsigned char v, unsigned char &r, unsigned char &g, unsigned char &b) {
    float C = y - 16;
    float D = u - 128;
    float E = v - 128;
    r = (char)clamp(C + ( 1.402 * E )) ;
    g = (char)clamp(C - ( 0.344136 * D + 0.714136 * E )) ;
    b = (char)clamp(C + ( 1.772 * D )) ;
}

unsigned char *getFrame() {
    unsigned char *frame = (unsigned char *)v4l2.getFrame();
    
    int i = 0, k = 0;
    unsigned char Y, U, V, R, G, B;
    for (i=0;i<640*480*2;i+=4) {
        Y = frame[i];
        U = frame[i+1];
        V = frame[i+3];
        yuv2rgb(Y, U, V, R, G, B);
        rgbFrame[k] = R; k++;
        rgbFrame[k] = G; k++;
        rgbFrame[k] = B; k++;
        Y = frame[i+2];
        yuv2rgb(Y, U, V, R, G, B);
        rgbFrame[k] = R; k++;
        rgbFrame[k] = G; k++;
        rgbFrame[k] = B; k++;
    }
    return rgbFrame;
}

void stopCapture() {
    v4l2.freeBuffers();
}

// Call this before using the device
void openDevice() {
    // set format
    struct v4l2_format fmt;
    CLEAR(fmt);
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    // Adjust resolution
    fmt.fmt.pix.width = 640;
    fmt.fmt.pix.height = 480;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    if (!v4l2.set(fmt)) {
        fprintf(stderr, "device does not support used settings.\n");
    }

    v4l2.initBuffers();
    v4l2.startCapture();
    rgbFrame = (unsigned char *)malloc(640*480*3);
}
}

Алгоритм вполне понятен — сначала открываем устройство, имя которого задаётся вначале ("/dev/video0"), а затем на каждый запрос getFrame считываем кадр с вебкамеры, конвертируем его в RGB формат и отдаём ссылку на кадр тому, кто его просил. Я предоставляю также Makefile для быстрой компиляции данной библиотеки, если вам это понадобится.

А вот так выглядит обёртка этой библиотеки для Python:
v4l2.py

from ctypes import *

import Image
import time

lib = cdll.LoadLibrary("linux/libv4l2.so")

class VideoDevice(object):
    def __init__(self):
        lib.openDevice()
        lib.getFrame.restype = c_void_p
        
    def getImage(self):
        buf = lib.getFrame()
        frame = (c_char * (640*480*3)).from_address(buf)
        img = Image.frombuffer('RGB', 
            (640, 480),
            frame,
            'raw',
            'RGB',
            0,
            1)
        return img, time.time()

Как видите абсолютно ничего сложного. Библиотека подключается с помощью модуля ctypes. В написании обёртки не было никаких проблем, за исключением строчки:

frame = (c_char * (640*480*3)).from_address(buf)

К которой я не сразу пришёл. Дело в том, что если считывать данные из getFrame() как c_char_p, то ctypes будет интерпретировать данные как строку с нулевым окончанием, то есть как только в потоке байт встретится ноль — считывание прекратится. Такая же конструкция позволяет чётко задать сколько необходимо считать байт. В нашем случае это всегда фиксированная величина — 640*480*3.

Я не буду здесь приводить исходный код для получения изображения в Windows, но он так же не отличается какой бы то ни было сложностью и расположен он в архиве в папке windows с именем directx.py.

А приведу я лучше исходный код класса слежения за объектом, который, я напоминаю, написан с использованием OpenCV. Я взял за основу пример lkdemo.py, поставляемый вместе с OpenCV и опять же упростил его для наших нужд, походу переделав его в класс:
tracker.py

class Tracker(object):
    "Simple object tracking class"
    def __init__(self):
        self.grey = None
        self.point = None
        self.WIN_SIZE = 10
        
    def target(self, x, y):
        "Tell which object to track"
        # It needs to be an array for the optical flow calculation
        self.point = [(x, y)]

    def takeImage(self, img):
        "Loads and processes next frame"
        # Convert it to IPL Image
        frame = cv.CreateImageHeader(img.size, 8, 3)
        cv.SetData(frame, img.tostring())
        
        if self.grey is None:
            # create the images we need
            self.grey = cv.CreateImage (cv.GetSize (frame), 8, 1)
            self.prev_grey = cv.CreateImage (cv.GetSize (frame), 8, 1)
            self.pyramid = cv.CreateImage (cv.GetSize (frame), 8, 1)
            self.prev_pyramid = cv.CreateImage (cv.GetSize (frame), 8, 1)
        
        cv.CvtColor (frame, self.grey, cv.CV_BGR2GRAY)
        if self.point:
            # calculate the optical flow
            new_point, status, something = cv.CalcOpticalFlowPyrLK (
                self.prev_grey, self.grey, self.prev_pyramid, self.pyramid,
                self.point,
                (self.WIN_SIZE, self.WIN_SIZE), 3,
                (cv.CV_TERMCRIT_ITER|cv.CV_TERMCRIT_EPS, 20, 0.03),
                0)
            # If the point is still alive
            if status[0]:
                self.point = new_point
            else:
                self.point = None
        # swapping
        self.prev_grey, self.grey = self.grey, self.prev_grey
        self.prev_pyramid, self.pyramid = self.pyramid, self.prev_pyramid

Сначала мы должны сказать ему за какой точкой хотим следить, для этого есть метод target. Затем мы даём ему кадр за кадром с помощью метода takeImage, он в свою очередь конвертирует кадр изображения в понятный ему формат, создаёт необходимые для работы алгоритма изображения, переводит кадр из цветного в оттенки серого и затем скармливает это всё функции CalcOpticalFlowPyrLK, которая считает оптический поток пирамидальным методом Лукаса-Канаде. На выходе этой функии мы получаем новые координаты точки, за которой мы следим. Если точка потерялась, то status[0] будет равен нулю. Оптический поток можно посчитать не только для одной точки. Запустите программу lkdemo.py с вебкамерой и посмотрите как хорошо он обрабатывает множество точек.

Скажу ещё про конвертацию изображения из Python Imaging Library в формат OpenCV, дело в том, что OpenCV для цветных изображений использует другой порядок цветовых компонент — BGR, для полной конвертации надо было бы ещё дополнить код строчкой cv.CvtColor(frame, frame, cv.CV_BGR2RGB), но большинству алгоритмов слежения абсолютно всё равно перепутаны у Вас цветовые компоненты или нет, наш же пример вообще использует только чёрно-белые изображения. Поэтому эту строчку можно не включать в код.

Я также не привожу в статье исходный код класса по непосредственному вычислению расстояния, так как там только простейшая математика. Находится он в файле distance_measure.py.

Осталось только показать исходный код основного скрипта, который формирует графический интерфейс и загружает все остальные модули.
main.py

from distance_measure import Calculator
from webcam import WebCam
from tracker import Tracker

from Tkinter import *
import ImageTk as PILImageTk
import time

class GUIFramework(Frame):
    "This is the GUI"
    def __init__(self,master=None):
        Frame.__init__(self,master)
        self.grid(padx=10,pady=10)
        self.distanceLabel = Label(self, text='Distance =')
        self.distanceLabel.grid(row=0, column=0)
        self.speedLabel = Label(self, text='Speed =')
        self.speedLabel.grid(row=0, column=1)
        self.imageLabel = None
        self.cameraImage = None
        self.webcam = WebCam()
        # M = 510, L = 0.5, W = 640
        self.dist_calculator = Calculator(500, 0.5, 640, 1)
        self.tracker = Tracker()
        self.after(100, self.drawImage)

    def updateMeasure(self, x):
        (distance, speed) = self.dist_calculator.calculate(x, time.time())
        self.distanceLabel.config(text = 'Distance = '+str(distance))
        # If you want get km/h instead of m/s just multiply
        # m/s value by 3.6
        #speed *= 3.6
        self.speedLabel.config(text = 'Speed = '+str(speed) + ' m/s')
        
    def imgClicked(self, event):
        """
        On left mouse button click calculate distance and
        tell tracker which object to track
        """
        self.updateMeasure(event.x)
        self.tracker.target(event.x, event.y)
        
    def drawImage(self):
        "Load and display the image"
        img, timestamp = self.webcam.getImage()
        # Pass image to tracker
        self.tracker.takeImage(img)
        if self.tracker.point:
            pt = self.tracker.point[0]
            self.updateMeasure(pt[0])
            # Draw rectangle around tracked point
            img.paste((128, 255, 128), (int(pt[0])-2, int(pt[1])-2, int(pt[0])+2, int(pt[1])+2))
        self.cameraImage = PILImageTk.PhotoImage(img)
        if not self.imageLabel:
            self.imageLabel = Label(self, image = self.cameraImage)
            self.imageLabel.bind("<Button-1>", self.imgClicked)
            self.imageLabel.grid(row=1, column=0, columnspan=2)
        else:
            self.imageLabel.config(image = self.cameraImage)
        # 30 FPS refresh rate
        self.after(1000/30, self.drawImage)

if __name__ == '__main__':
    guiFrame = GUIFramework()
    guiFrame.mainloop()

Как я говорил выше, я выбрал библиотеку Tkinter для создания графического интерфейса, я работал и с другими тулкитами, такими как GTK, QT и, конечно же, wxPython, но их необходимо было ставить дополнительно, в то время как Tkinter работает сразу и он весьма прост в обращении, однако сложного интерфейса на нём, конечно, не создать, но его способностей с лихвой хватает для поставленной задачи. В инициализации класса я создаю сетку grid для расположения в ней других виджетов: двух текстовых полей и одного изображения. С Tkinter мне даже не пришлось отдельно создавать потоки для загрузки изображений с вебкамеры, потомучто есть такой метод after, который позволяет выполнять указанную функцию через определенный промежуток времени. Обновлять текст и изображение у Label можно методом config. Очень просто! Обработка события нажатия кнопки мыши с помощью метода bind переводится методу imgClicked.

Загрузка изображения и его временной отметки осуществляется функцией self.webcam.getImage. Модуль webcam всего лишь навсего загружает соответствующий модуль для работы с вебкамерой в зависимости от того под какой операционной системой сейчас работает программа.

Ещё раз приведу ссылку на архив с программой — distance-measure.
Необходимые пакеты для ubuntu: python, python-imaging, python-imaging-tk, opencv версии 2.2 и build-essential для компиляции обёртки V4L2.
Запускается программа через:
python main.py
Чтобы начать следить за объектом, необходимо на него кликнуть.

На этом всё.

Полезные ссылки

• OpenCV wiki
• Optical Flow
• Video4Linux2
• Цветовая модель YUV
• Краткий обзор Tkinter