ブログ

OpenCVで２つの直線の交点を検出するソースコードを作ったのでシェアします。

このソースコードの動作確認は、ATDE5にUSBカメラを接続して行なっています。 アルゴリズムの検証用途だったため、Armadilloでは動作確認していませんのでご注意ください。

下図は動作確認時のキャプチャです。

Makefile

CROSS_COMPILE ?= arm-linux-gnueabihf-

CC = $(CROSS_COMPILE)cc
CXX = $(CROSS_COMPILE)g++

CFLAGS = -Wall -O2
CXXFLAGS = -Wall -O2

EXEC = vertex
OBJS = vertex.o
LDLIBS := -lopencv_core -lopencv_highgui -lopencv_imgproc -lm

all: $(EXEC)

$(EXEC): $(OBJS)
    $(CC) $(LDFLAGS) -o $@ $(OBJS) $(LDLIBS)

romfs:
    $(ROMFSINST) /usr/bin/$(EXEC)

clean:
    rm -f $(OBJS) $(EXEC)

vertex.cpp

// The "Vertex Detector" program.

#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <iostream>
#include <math.h>
#include <string.h>

using namespace cv;
using namespace std;

const char* wndname = "Vertex Detection Demo";

// helper function:
// finds a cosine of angle between vectors
// from pt0->pt1 and from pt0->pt2
double angle_cos( Point pt1, Point pt2, Point pt0 )
{
    double dx1 = pt1.x - pt0.x;
    double dy1 = pt1.y - pt0.y;
    double dx2 = pt2.x - pt0.x;
    double dy2 = pt2.y - pt0.y;

    if (pt0 == pt1 || pt0 == pt2)
        return 1.0;

    return fabs(dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}

int main(int argc, char *argv[])
{
    namedWindow( wndname, 1 );
    vector<vector<Point> > squares;
    const string source = "/dev/video0";
    VideoCapture inputVideo(CV_CAP_V4L2);

    if (!inputVideo.isOpened())
    {
        cout  << "Could not open the input video: " << source << endl;
        return -1;
    }

    Mat srcimg, dstimg, workimg;
    for(;;)
    {
        inputVideo >> srcimg;
        if (srcimg.empty()) break;

        dstimg = srcimg.clone();
        cvtColor(srcimg, workimg, CV_BGR2GRAY);
        Canny(workimg, workimg, 50, 200, 3, true);

        vector<Vec4i> lines;
        HoughLinesP( workimg, lines, 1, CV_PI/180, 80, 30, 10 );

        if (lines.size() >= 2)
            for( size_t i = 1; i < lines.size(); i++ )
            {
#define P1X lines[i - 1][0]
#define P1Y lines[i - 1][1]
#define P3X lines[i - 1][2]
#define P3Y lines[i - 1][3]
#define P2X lines[i - 0][0]
#define P2Y lines[i - 0][1]
#define P4X lines[i - 0][2]
#define P4Y lines[i - 0][3]

                double s1, s2;
                s1 = ((P4X - P2X) * (P1Y - P2Y) - (P4Y - P2Y) * (P1X - P2X))/2;
                s2 = ((P4X - P2X) * (P2Y - P3Y) - (P4Y - P2Y) * (P2X - P3X))/2;

                Point v;
                v.x = P1X + (P3X - P1X) * s1 / (s1 + s2);
                v.y = P1Y + (P3Y - P1Y) * s1 / (s1 + s2);

                if (v.x < 0 || v.y < 0)
                    continue;

                Point p1, p2;
                p1.x = P1X;
                p1.y = P1Y;
                p2.x = P2X;
                p2.y = P2Y;

                double cosine = angle_cos(p1, p2, v);
                if (cosine > 0.3)
                    continue;

                line( dstimg, Point(lines[i - 1][0], lines[i - 1][1]),
                      Point(lines[i - 1][2], lines[i - 1][3]), Scalar(255,0,0), 3, 8 );
                line( dstimg, Point(lines[i][0], lines[i][1]),
                      Point(lines[i][2], lines[i][3]), Scalar(0,0,255), 3, 8 );

                cout << "(" << v.x << ", " << v.y << "), " << cosine << endl;
                cv::circle(dstimg, v, 10, Scalar(0, 255, 0), 3, 8);
            }

        imshow(wndname, dstimg);
        cvWaitKey(10);
    }

    return 0;
}