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