пятница, 14 сентября 2012 г.

Делаем дальномер из лазерной указки и старой веб-камеры с помощью OpenCV


Когда-то давно, когда различные ультразвуковые дальномеры не были столь доступны, у меня возникла идея сделать свой оптический прибор из подручных материалов. Спустя несколько лет я наткнулся на этот проект. Я его немного переделал - так появилась данная статья.

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

Как известно, любая веб-камера состоит из линзы с определённым фокусным расстоянием и матрицы на которую проецируется изображение.

Фото взято отсюда с http://robot-develop.org/archives/2026

Для удобства, мы немного переделаем наше построение. Вся схема умещается на изображении ниже.





Далее вы можете видеть готовую установку. Я сделал её из подручных материалов, а именно, из пластика от лазерного диска, лазерной указки и старой веб-камеры.

После того как сборка установки была закончена, я решил снять зависимость, чтобы проверить мои предположения. График вы видите ниже.




Для пробы я даже подобрал логарифмическую функцию для аппроксимации. Она даёт удовлетворительные результаты, но ещё лучше использовать точную формулу. Сложность состоит в отсутствии точных каллибровочных данных камеры. О решении этой проблемы в моей следующей статье.

Теперь переходим к программной части. Программа для  оценки расстояний предельно проста. В общем-то её можно немного доработать, но для пущей ясности представлю её в изначальном виде.


// ---------------------------------------------------------
    Techncreo Rangefinder by Zhukov Alex, 2011
// ---------------------------------------------------------
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <math.h>

// простая аппроксимация зависимости
double getdistance(int i)
{
    return -10.603*log(i*1.0)+76.769;
}


/*
// на самом деле это должно выглядеть так

double getdistance(int i)
{
    double Beta=0.002840909; // в моём случае
    double h=19.0;
    double fi=20/180*3.1415;

    return h/(tan(fi)+Beta*i);
}
*/

int main(int argc, char* argv[])
{

    //int itcount=4; только для cvThreshold
    // получаем любую подключённую камеру
    CvCapture* capture = cvCreateCameraCapture(CV_CAP_ANY);
    assert( capture );

    // Устанавливаем разрешение
    cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, 640);
    cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, 480);

    // узнаем ширину и высоту кадра для порядка(в некоторых случаях нужные параметры не устанавливаются)
    double width = cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH);
    double height = cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT);
    printf("[i] %.0f x %.0f\n", width, height );

    IplImage* frame=0; // кадр из камеры
    IplImage* Rframe=0;// с градациями серого

    cvNamedWindow("capture", CV_WINDOW_AUTOSIZE);

    printf("[i] press Enter for capture image and Esc for quit!\n\n");

    int counter=0;
    char filename[512];

    double framemin=0;
    double framemax=0;
    double framemid=0;

    frame = cvQueryFrame( capture );
    Rframe = cvCreateImage( cvGetSize(frame), IPL_DEPTH_8U, 1 );

    CvPoint  p;

    // Задаём параметры надписи
    CvPoint pt = cvPoint( 10, 20 );
    CvFont font;
    cvInitFont( &font, CV_FONT_HERSHEY_COMPLEX,0.5, 0.5, 0, 1, CV_AA);

    // используя шрифт выводим на картинку текст
    double distance=0;
    std::ostringstream dist;

    while(true){

        // получаем кадр
        frame = cvQueryFrame( capture );

        // выбираем из frame красный канал в Rframe
        cvSplit(frame,0,0,Rframe,0);
       

        cvMinMaxLoc(Rframe, &framemin, &framemax, 0, &p);
        // координаты самой яркой точки

        printf("[R] %d x %d\n", p.x, p.y);

        // тут можно побаловаться с Threshold'ом
        //framemid=(framemin+framemax)/2;
        //for(int i=0;i<=itcount;i++) framemid=framemid+(framemax-framemid)/2;
        //cvThreshold(Rframe, Rframe, framemid, framemax, CV_THRESH_TOZERO);
   
        dist<<"Distance: "<<getdistance(p.x);
        cvPutText(frame, dist.str().c_str(), pt, &font, CV_RGB(200, 20, 20) );
        dist.str("");

        // показываем кадр
        cvShowImage("capture", frame);


        char c = cvWaitKey(33);

        if (c == 27) { // нажата ESC  - выходим
            break;
        }
        else if(c == 13) { // Enter -  сохраняем кадр в файл
            sprintf(filename, "Image%d.jpg", counter);
            printf("[i] capture... %s\n", filename);
            cvSaveImage(filename, frame);
            counter++;
        }
    }
    // освобождаем ресурсы

    //cvReleaseImage(&frame);// внезапно
    //cvReleaseImage(&Rframe);//попытка освободить дескрипторы приводит в ошибке
    cvReleaseCapture( &capture );
    cvDestroyWindow("capture");
    return 0;
}




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

Интересным продолжением этой темы был бы монтаж лазера на патформу с шаговым мотором. Но об этом в другой раз.

Для интересующихся OpenCV есть замечательный туториал тут.


Жду ИМЕННО ВАШИХ откликов и комментариев.

12 комментариев:

  1. Заебись мне понравилось, я все думаю насчет мобильного приложения. Но правда, у телефона нет встроенного лазера. Но есть динамик. Я вот даже не знаю.

    ОтветитьУдалить
    Ответы
    1. Встроенный лазер, да без проблем.
      Уже проектор встроенный года как 3 функционирует.

      Удалить
  2. Что такое U index{0}?
    Как определяем угол phi?
    Где этот кусок с phi в программе?

    ОтветитьУдалить
    Ответы
    1. U - количество пикселей от середины матрицы.
      Соответственно Uo - поправка этого значения.

      Угол phi - угол между вектором направления камеры и лазерного луча.

      В программе представлена только логарифмическая аппроксимация аналитически найденного закона. Для использования точной формулы нужна калибровка камеры(об этом моя следующая статья).

      Удалить
  3. Очень полезная статья, спасибо. Как раз сидим сейчас занимаемся чем то похожим!

    ОтветитьУдалить
    Ответы
    1. Я рад, что понравилось. Делитесь идеями, может ещё чего интересного смогу подсказать.

      Удалить
    2. На лазерах делаем систему контроля станка. В данном контексте: при помощи (нескольких комплектов) камеры и 2х лазеров нужно определять расстояния до рабочей поверхности. Пробовали через преобразования Хафа, да работает непредсказуемо.

      Удалить
  4. Кстати, у Вас верно происходит деление на каналы? cvSplit(frame,Rframe,0,0,0);
    В этом http://robocraft.ru/blog/computervision/365.html материале например:
    cvSplit( rgb, b_plane, g_plane, r_plane, 0 );

    ОтветитьУдалить
    Ответы
    1. Да, ошибочка вышла, спасибо за исправление. В большинстве случаев именно так.
      BGR - стандарт для трёхцветных изображений в OpenCV. Но, вообще-то, каналы могут быть расположены произвольным образом в зависимости от используемого формата.

      Удалить
  5. Скажите пожалуйста, как Вы подбирали логарифмическую функцию?

    ОтветитьУдалить
    Ответы
    1. Честно говоря, я просто вписал несколько простейших функций в набор точек(координата на изображении и расстояние), который получил экспериментально. Логарифм давал наименьшее отклонение.
      Тогда мне было удобно делать это в Excel, добавив линию тренда. Думаю, там используется метод наименьших квадратов.

      Удалить
  6. Я мало в этом понимаю, но вот вопрос. Скажите пожалуйста, какой режим измерения - постоянный трекинг или разовый замер? Если постоянный, то можно ли остановить измерение с компа? И если опять таки постоянное измерение, то будет ли реагировать на приближение и удаление объекта, на его вибрацию? Наверно это зависит от скорости реагирования камеры. На картинке расстояние показано в см и 4 знака после запятой, это значит что можно сотую и даже тысячную миллиметра замерить (при одинаковой погрешности двух измерений в пределах изменения расстояния на 1 мм разница измерений будет адекватной). Просто меня интересует не расстояние до объекта, а разница между расстояниями до приближающегося или удаляющегося объекта при малых его перемещениях.

    ОтветитьУдалить