Programming/openCV2015. 10. 14. 21:38

시야각 내 각도 계산

tpos + ((matchLoc.x - 160) * 60 / (180/60) /  320);



해놓고는 왜 이게 대충 근사로 되는지 모르겠네 -_-

일단.. 서보 모터는 180도 움직일 수 있고
이 값이 30~110 까지로 설정이 되지만 trackbar에서는 0~100으로 설정되니 일단 패스

카메라 하나의 시야각은 60도로 근사하고
화면상에 보이는 각도는0~100중 0~33으로 가정하여
계산을 한다
그리고 화면 해상도(클릭 범위)는 0~319로 320 범위로 생각

그러면 클릭 위치를 중심점에서 부터 빼서
(matchLoc.x - (320 / 2)) / 320 로 중심점을 계산하고
음의 방향이 될지 양의 방향이 될지 계산한다.


현재 화면의 화각은 60도로

전체 범위(0~100) 에서 1/3 사이즈로 계산한다.

/ (180 / 60)



전체 수식으로는

현재 화면에서의 비율 * 화면 하나의 각도 * 화면 하나의 서보에 대한 비율


 ( 현재 점 - 중심점 )                                     카메라 영상 각도(60')

----------------------- *  카메라 영상 각도(60') * ----------------------------

카메라 영상폭(320px)                                     서보 이동 각도(180')



이 되려나?

하고 나니 나도 헷갈리네.. ㅠㅠ




---------------

일단은 백업..

한쪽 버전


$ cat cv.cpp

#include <opencv2/core/core.hpp>

#include <opencv2/highgui/highgui.hpp>

#include <opencv2/imgproc/imgproc.hpp>

#include <iostream>


#include <wiringPi.h>


#define MIN_POS 30

#define MAX_POS 110


using namespace cv;

using namespace std;


bool bMouse = true;

Point mpos(160,120);


void CallBackFunc(int event, int x, int y, int flags, void* userdata)

{


        switch(event)

        {

        default:

                break;


        case EVENT_LBUTTONDOWN:

                cout << "EVENT_LBUTTONDOWN" << endl;

                cout << "pos (" << x << "," << y << ")" << endl;

                mpos.x = x;

                mpos.y = y;

                bMouse = true;

                break;

        case EVENT_LBUTTONUP:

                cout << "EVENT_LBUTTONUP" << endl;

                break;

        case EVENT_RBUTTONDOWN:

                cout << "EVENT_RBUTTONDOWN" << endl;

                bMouse = true;

                break;

        }

}


void init_wiringpi()

{

        if(wiringPiSetup() == -1) exit(0);


        pinMode(1, PWM_OUTPUT);

        pwmSetMode(PWM_MODE_MS);

        pwmSetRange(1024);

        pwmSetClock(400);


        pinMode(23, PWM_OUTPUT);

        pwmSetMode(PWM_MODE_MS);

        pwmSetRange(1024);

        pwmSetClock(400);


        pwmWrite(1,     MIN_POS + (MAX_POS - MIN_POS) / 2);

        pwmWrite(23,    MIN_POS + (MAX_POS - MIN_POS) / 2);

}


void setPWM_1(int val)

{

        pwmWrite(23, val);

}


void setPWM_2(int val)

{

        pwmWrite(1, val);

}



void on_trackbar_1(int pos, void *ptr)

{

        int val = MIN_POS + pos * (MAX_POS - MIN_POS) / 100;

        setPWM_1(val);


        cout << "pos 1 [" << pos << ":" << val << "]" << endl;

}


void on_trackbar_2(int pos, void *ptr)

{

        int val = MIN_POS + pos * (MAX_POS - MIN_POS) / 100;

        setPWM_2(val);


        cout << "pos 2 [" << pos << ":" << val << "]" << endl;

}


void on_trackbar_3(int pos, void *ptr)

{

        int val = MIN_POS + pos * (MAX_POS - MIN_POS) / 100;

//      setPWM_1(val);

//      setPWM_2(val);


        setTrackbarPos("track 1", "cam 1", pos);

        setTrackbarPos("track 2", "cam 2", pos);


        cout << "pos 3 [" << pos << ":" << val << "]" << endl;

}


int main(int argc, char** argv)

{

        // opencv 2 style

        VideoCapture cap(0);

        int pos[2];


        init_wiringpi();


        if(!cap.isOpened())

        {

                cout << "No camera detected" << endl;

                return -1;

        }

        else

        {

                cout << "In capture ..." << endl;

                cap.set(CV_CAP_PROP_FRAME_WIDTH, 320);

                cap.set(CV_CAP_PROP_FRAME_HEIGHT, 240);

        }

/*

        VideoCapture cap2(1);

        if(!cap2.isOpened())

        {

                cout << "No camera detected" << endl;

                return -1;

        }

        else

        {

                cout << "In capture ..." << endl;

                cap2.set(CV_CAP_PROP_FRAME_WIDTH, 320);

                cap2.set(CV_CAP_PROP_FRAME_HEIGHT, 240);

        }

*/

        namedWindow("cam 1", WINDOW_AUTOSIZE );

//      namedWindow("cam 2", WINDOW_AUTOSIZE );

        namedWindow("control", WINDOW_AUTOSIZE );

        namedWindow("crop", WINDOW_AUTOSIZE );


        createTrackbar("track 1", "cam 1", &pos[0], 100, on_trackbar_1 );

//      createTrackbar("track 2", "cam 2", &pos[1], 100, on_trackbar_2 );

//      createTrackbar("track 3", "control", &pos[0], 100, on_trackbar_3 );


        setTrackbarPos("track 1", "cam 1", 50);

//      setTrackbarPos("track 2", "cam 2", 50);

//      setTrackbarPos("track 3", "control", 50);


        setMouseCallback("cam 1", CallBackFunc, NULL);


        Mat frame;

        Mat result;

        Mat tMat;

        Mat crop;


        double minVal;

        double maxVal;

        Point minLoc;

        Point maxLoc;

        Point matchLoc;


        for(;;)

        {


                Point pt[2];

                if(!cap.read(frame)) break;

                        pt[0].x = 150;

                        pt[0].y = 110;

                        pt[1].x = pt[0].x + 20;

                        pt[1].y = pt[0].y + 20;


                if(bMouse)

                {

                        tMat = frame.clone();

                        crop = Mat(tMat, Rect(mpos.x - 10, mpos.y - 10, 20, 20));

                        imshow("crop", crop);

                }


                matchTemplate( frame, crop, result, CV_TM_SQDIFF );

                normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() );

                minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );

                matchLoc = minLoc;

                cout << "matchLoc : " << matchLoc << endl;


                rectangle(frame, pt[0], pt[1], Scalar(0,0,0),2);

                rectangle(frame, Point(mpos.x - 10, mpos.y - 10), Point(mpos.x + 10, mpos.y + 10), Scalar(0,0,255),2);

                rectangle(frame, matchLoc, Point( matchLoc.x + 20 , matchLoc.y + 20 ), Scalar(0,255,0), 2);

#define SERVO_ANG       180

#define CAM_FOV         60

                if(bMouse)

                {

                        int tpos = getTrackbarPos("track 1", "cam 1");

//                      int value = tpos + (matchLoc.x - 160) * 3 * 2/ 100;

//                      int value = tpos + (100 / (SERVO_ANG / CAM_FOV)) * (matchLoc.x / 320) ;

                        int value = tpos + ((matchLoc.x - 160) * 60 / 3 /  320);

                        cout << "---- calc : " <<  value << endl;

                        setTrackbarPos("track 1", "cam 1", value);

                        bMouse = false;

                }


                imshow("cam 1", frame);

/*

                if(!cap2.read(frame)) break;

                rectangle(frame, pt[0], pt[1], Scalar(0,0,0),2);


                matchTemplate( frame, crop, result, CV_TM_SQDIFF );

                normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() );

                minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );

                matchLoc = minLoc;

                cout << "matchLoc : " << matchLoc << endl;


                rectangle(frame, matchLoc, Point( matchLoc.x + 20 , matchLoc.y + 20 ), Scalar(0,255,0), 2);

                        imshow("cam 2", frame);

*/

                if(waitKey(30) >= 0) break;

        }


        return 0;

} 



두쪽 버전

$ cat cv.cpp

#include <opencv2/core/core.hpp>

#include <opencv2/highgui/highgui.hpp>

#include <opencv2/imgproc/imgproc.hpp>

#include <iostream>


#include <wiringPi.h>


#define MIN_POS 30

#define MAX_POS 110


using namespace cv;

using namespace std;


bool bMouse = true;

Point mpos(160,120);


void CallBackFunc(int event, int x, int y, int flags, void* userdata)

{


        switch(event)

        {

        default:

                break;


        case EVENT_LBUTTONDOWN:

                cout << "EVENT_LBUTTONDOWN" << endl;

                cout << "pos (" << x << "," << y << ")" << endl;

                mpos.x = x;

                mpos.y = y;

                bMouse = true;

                break;

        case EVENT_LBUTTONUP:

                cout << "EVENT_LBUTTONUP" << endl;

                break;

        case EVENT_RBUTTONDOWN:

                cout << "EVENT_RBUTTONDOWN" << endl;

                bMouse = true;

                break;

        }

}


void init_wiringpi()

{

        if(wiringPiSetup() == -1) exit(0);


        pinMode(1, PWM_OUTPUT);

        pwmSetMode(PWM_MODE_MS);

        pwmSetRange(1024);

        pwmSetClock(400);


        pinMode(23, PWM_OUTPUT);

        pwmSetMode(PWM_MODE_MS);

        pwmSetRange(1024);

        pwmSetClock(400);


        pwmWrite(1,     MIN_POS + (MAX_POS - MIN_POS) / 2);

        pwmWrite(23,    MIN_POS + (MAX_POS - MIN_POS) / 2);

}


void setPWM_1(int val)

{

        pwmWrite(23, val);

}


void setPWM_2(int val)

{

        pwmWrite(1, val);

}



void on_trackbar_1(int pos, void *ptr)

{

        int val = MIN_POS + pos * (MAX_POS - MIN_POS) / 100;

        setPWM_1(val);


        cout << "pos 1 [" << pos << ":" << val << "]" << endl;

}


void on_trackbar_2(int pos, void *ptr)

{

        int val = MIN_POS + pos * (MAX_POS - MIN_POS) / 100;

        setPWM_2(val);


        cout << "pos 2 [" << pos << ":" << val << "]" << endl;

}


void on_trackbar_3(int pos, void *ptr)

{

        int val = MIN_POS + pos * (MAX_POS - MIN_POS) / 100;

        setPWM_1(val);

        setPWM_2(val);


        setTrackbarPos("track 1", "cam 1", pos);

        setTrackbarPos("track 2", "cam 2", pos);


        cout << "pos 3 [" << pos << ":" << val << "]" << endl;

}


int main(int argc, char** argv)

{

        // opencv 2 style

        VideoCapture cap(0);

        int pos[2];


        init_wiringpi();


        if(!cap.isOpened())

        {

                cout << "No camera detected" << endl;

                return -1;

        }

        else

        {

                cout << "In capture ..." << endl;

                cap.set(CV_CAP_PROP_FRAME_WIDTH, 320);

                cap.set(CV_CAP_PROP_FRAME_HEIGHT, 240);

        }


        VideoCapture cap2(1);

        if(!cap2.isOpened())

        {

                cout << "No camera detected" << endl;

                return -1;

        }

        else

        {

                cout << "In capture ..." << endl;

                cap2.set(CV_CAP_PROP_FRAME_WIDTH, 320);

                cap2.set(CV_CAP_PROP_FRAME_HEIGHT, 240);

        }


        namedWindow("cam 1", WINDOW_AUTOSIZE );

        namedWindow("cam 2", WINDOW_AUTOSIZE );

        namedWindow("control", WINDOW_AUTOSIZE );

        namedWindow("crop", WINDOW_AUTOSIZE );


        createTrackbar("track 1", "cam 1", &pos[0], 100, on_trackbar_1 );

        createTrackbar("track 2", "cam 2", &pos[1], 100, on_trackbar_2 );

        createTrackbar("track 3", "control", &pos[0], 100, on_trackbar_3 );


        setTrackbarPos("track 1", "cam 1", 50);

        setTrackbarPos("track 2", "cam 2", 50);

        setTrackbarPos("track 3", "control", 50);


        setMouseCallback("cam 1", CallBackFunc, NULL);


        Mat frame;

        Mat result;

        Mat tMat;

        Mat crop;


        double minVal;

        double maxVal;

        Point minLoc;

        Point maxLoc;

        Point matchLoc;


        for(;;)

        {


                Point pt[2];

                if(!cap.read(frame)) break;

                        pt[0].x = 150;

                        pt[0].y = 110;

                        pt[1].x = pt[0].x + 20;

                        pt[1].y = pt[0].y + 20;


                if(bMouse)

                {

                        tMat = frame.clone();

                        crop = Mat(tMat, Rect(mpos.x - 10, mpos.y - 10, 20, 20));

                        imshow("crop", crop);

                }


                matchTemplate( frame, crop, result, CV_TM_SQDIFF );

                normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() );

                minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );

                matchLoc = minLoc;

                cout << "matchLoc : " << matchLoc << endl;


                rectangle(frame, pt[0], pt[1], Scalar(0,0,0),2);

                rectangle(frame, Point(mpos.x - 10, mpos.y - 10), Point(mpos.x + 10, mpos.y + 10), Scalar(0,0,255),2);

                rectangle(frame, matchLoc, Point( matchLoc.x + 20 , matchLoc.y + 20 ), Scalar(0,255,0), 2);

#define SERVO_ANG       180

#define CAM_FOV         60

                if(bMouse)

                {

                        int tpos = getTrackbarPos("track 1", "cam 1");

                        int value = tpos + ((matchLoc.x - 160) * 60 / 3 /  320);

                        cout << "---- calc : " <<  value << endl;

                        setTrackbarPos("track 1", "cam 1", value);

                }


                imshow("cam 1", frame);


                if(!cap2.read(frame)) break;

                rectangle(frame, pt[0], pt[1], Scalar(0,0,0),2);


                matchTemplate( frame, crop, result, CV_TM_SQDIFF );

                normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() );

                minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );

                matchLoc = minLoc;

                cout << "matchLoc : " << matchLoc << endl;


                rectangle(frame, matchLoc, Point( matchLoc.x + 20 , matchLoc.y + 20 ), Scalar(0,255,0), 2);

                if(bMouse)

                {

                        int tpos = getTrackbarPos("track 2", "cam 2");

                        int value = tpos + ((matchLoc.x - 160) * 60 / 3 /  320);

                        cout << "---- calc : " <<  value << endl;

                        setTrackbarPos("track 2", "cam 2", value);

                        bMouse = false;

                }

                imshow("cam 2", frame);


                if(waitKey(30) >= 0) break;

        }


        return 0;

} 


'Programming > openCV' 카테고리의 다른 글

opencv 카메라 왜곡 수정  (0) 2015.10.20
opencv sift surf  (0) 2015.10.20
openCV + openMP 합치는게 잘 안되네?  (0) 2015.10.11
opencv 마우스 이벤트와 빠르게 그리기  (0) 2015.10.05
opencv 마우스 이벤트 관련 2  (0) 2015.10.05
Posted by 구차니

티스토리툴바