OpenCVで２つの直線の交点を検出

at_nakai

2014年6月13日 13時47分

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;
}

ブログ

OpenCVで２つの直線の交点を検出

製品

Makefile

vertex.cpp