開放式視覺伺服平臺源代碼_上位機（初步版本，未對細節做修訂）

/***********************************************************************
 * 開放式視覺伺服平臺測試例程
 * 作者:李智超(西北工業大學)
 * 如需轉載，請註明出處
 * 測試環境爲Ubuntu13.04 默認g++

 * Originally created 15 June 2013
 * Copyleft (c) 2013, Vicent_Lee
 * http://blog.csdn.net/u010305560
 ***********************************************************************/


#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <iostream>
#include <ctype.h>
#include <stdio.h>    // Standard input/output definitions 
#include <stdlib.h> 
#include <string.h>   // String function definitions 
#include <unistd.h>   // for usleep()
#include <stdint.h>
#include <fcntl.h>    // File control definitions 
#include <errno.h>    // Error number definitions 
#include <termios.h>  // POSIX terminal control definitions 
#include <sys/ioctl.h>
#include <math.h>
using namespace cv;
using namespace std;

int serialport_init(const char* serialport, int baud);
int serialport_close(int fd);
int serialport_write(int fd, const char* str);
int serialport_flush(int fd);
void error(const char* msg);
void send_pos_error(int fd, float x, float y);
void open_serial();
void help();
void onMouse( int event, int x, int y, int, void* );


Mat image;

bool backprojMode = false; //表示是否要進入反向投影模式，ture表示準備進入反向投影模式
bool selectObject = false;//代表是否在選要跟蹤的初始目標，true表示正在用鼠標選擇
int trackObject = 0; //代表跟蹤目標標識 trackObject初始化爲0,或者按完鍵盤的'c'鍵後也爲0，當鼠標單擊鬆開後爲-1
bool showHist = true;//是否顯示直方圖
Point origin;//用於保存鼠標選擇第一次單擊時點的位置
Rect selection;//用於保存鼠標選擇的矩形框
int vmin = 10, vmax = 256, smin = 30;


char serialport[20];
int baudrate = 19200; 
int fd=-1;

const char* keys = 
{
    "{1|  | 0 | camera number}"
};

/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
int speed_max=20;//servo_rotate_speed=2*speed_max 度/s
int P=10;//PID pararmeter
int xc=1560;
int yc=1590;
int xc_last=1560;
int yc_last=1590;
double x_tune=0;
double y_tune=0;
int send_delay_cnt=0;
double x_tune_last=0;
double y_tune_last=0;
/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/


int main( int argc, const char** argv )
{
    help();

    VideoCapture cap; //定義一個攝像頭捕捉的類對象
    Rect trackWindow; //trackWindow爲鼠標選擇的區域
    RotatedRect trackBox;//定義一個旋轉的矩陣類對象
    int hsize = 16;
    float hranges[] = {0,180};//hranges在後面的計算直方圖函數中要用到
    const float* phranges = hranges;
    CommandLineParser parser(argc, argv, keys);//命令解析器函數
    int camNum = parser.get<int>("1");     
    
    cap.open(camNum);//直接調用成員函數打開攝像頭

    if( !cap.isOpened() )
    {
        help();
        cout << "***Could not initialize capturing...***\n";
        cout << "Current parameter's value: \n";
        parser.printParams();
        return -1;
    }

   // namedWindow( "Histogram", CV_WINDOW_AUTOSIZE );
    namedWindow( "CamShift Demo", CV_WINDOW_AUTOSIZE );
    setMouseCallback( "CamShift Demo", onMouse, 0 );//消息響應機制
    createTrackbar( "Vmin", "CamShift Demo", &vmin, 256, 0 );//createTrackbar函數的功能是在對應的窗口創建滑動條，滑動條Vmin,vmin表示滑動條的值，最大爲256
    createTrackbar( "Vmax", "CamShift Demo", &vmax, 256, 0 );//最後一個參數爲0代表沒有調用滑動拖動的響應函數
    createTrackbar( "Smin", "CamShift Demo", &smin, 256, 0 );//vmin,vmax,smin初始值分別爲10,256,30

    Mat frame, hsv, hue, mask, hist, histimg = Mat::zeros(640, 480, CV_8UC3), backproj;
    bool paused = false;
	int  cnt_write;
int test=0;
	open_serial();

    for(;;)
    {
		try{
			
        if( !paused )//沒有暫停
        {
            cap >> frame;//從攝像頭抓取一幀圖像並輸出到frame中
			if(test++%30==0){printf("frame total:%d*********1s*************\r\n",test);} //test for speed ,the result is 30fps
            if( frame.empty() )
                break;
        }

        frame.copyTo(image);
        
        if( !paused )//沒有按暫停鍵
        {
            cvtColor(image, hsv, CV_BGR2HSV);//將rgb攝像頭幀轉化成hsv空間的

            if( trackObject )
            {
                int _vmin = vmin, _vmax = vmax;

                //inRange函數的功能是檢查輸入數組每個元素大小是否在2個給定數值之間，可以有多通道,mask保存0通道的最小值，也就是h分量
//這裏利用了hsv的3個通道，比較h,0~180,s,smin~256,v,min(vmin,vmax),max(vmin,vmax)。如果3個通道都在對應的範圍內，則
//mask對應的那個點的值全爲1(0xff)，否則爲0(0x00).
                inRange(hsv, Scalar(0, smin, MIN(_vmin,_vmax)),
                        Scalar(180, 256, MAX(_vmin, _vmax)), mask);
                int ch[] = {0, 0};
                hue.create(hsv.size(), hsv.depth());//hue初始化爲與hsv大小深度一樣的矩陣，色調的度量是用角度表示的，紅綠藍之間相差120度，反色相差180度
                mixChannels(&hsv, 1, &hue, 1, ch, 1);//將hsv第一個通道(也就是色調)的數複製到hue中，0索引數組

                if( trackObject < 0 )//鼠標選擇區域鬆開後，該函數內部又將其賦值1
                {
                    //此處的構造函數roi用的是Mat hue的矩陣頭，且roi的數據指針指向hue，即共用相同的數據，select爲其感興趣的區域
                    Mat roi(hue, selection), maskroi(mask, selection);//設置hue圖選擇框爲ROI，設置掩膜版爲選擇框

//calcHist()函數第一個參數爲輸入矩陣序列，第2個參數表示輸入的矩陣數目，第3個參數表示將被計算直方圖維數通道的列表，第4個參數表示可選的掩碼函數
//第5個參數表示輸出直方圖，第6個參數表示直方圖的維數，第7個參數爲每一維直方圖數組的大小，第8個參數爲每一維直方圖bin的邊界
                    calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges);//將roi的0通道計算直方圖並通過mask放入hist中，hsize爲每一維直方圖的大小
                    normalize(hist, hist, 0, 255, CV_MINMAX);//將hist矩陣進行數組範圍歸一化，都歸一化到0~255
                    
                    trackWindow = selection;
                    trackObject = 1;//只要鼠標選完區域鬆開後，且沒有按鍵盤清0鍵'c'，則trackObject一直保持爲1，因此該if函數只能執行一次，除非重新選擇跟蹤區域

                    histimg = Scalar::all(0);//與按下'c'鍵是一樣的，這裏的all(0)表示的是標量全部清0
                    int binW = histimg.cols / hsize;  //histing是一個200*300的矩陣，hsize應該是每一個bin的寬度，也就是histing矩陣能分出幾個bin出來
                    Mat buf(1, hsize, CV_8UC3);//定義一個緩衝單bin矩陣
                    for( int i = 0; i < hsize; i++ )//saturate_case函數爲從一個初始類型準確變換到另一個初始類型
                        buf.at<Vec3b>(i) = Vec3b(saturate_cast<uchar>(i*180./hsize), 255, 255);//Vec3b爲3個char值的向量
                    cvtColor(buf, buf, CV_HSV2BGR);//將hsv又轉換成bgr
                        
                    for( int i = 0; i < hsize; i++ )//繪製直方圖
                    {
					 int val = saturate_cast<int>(hist.at<float>(i)*histimg.rows/255);//
                        rectangle( histimg, Point(i*binW,histimg.rows),    //在一幅輸入圖像上畫一個簡單的矩形，指定左上角和右下角，並定義顏色，大小，線型等
                                   Point((i+1)*binW,histimg.rows - val),	//特別注意此處畫圖由於參考系的原因，導致在畫圖時縱座標用了減法
                                   Scalar(buf.at<Vec3b>(i)), -1, 8 );
                    }
                }

                calcBackProject(&hue, 1, 0, hist, backproj, &phranges);//計算直方圖的反向投影，計算hue圖像0通道直方圖hist的反向投影，並存入backproj中
                backproj &= mask;//得到掩膜內的反向投影
                RotatedRect trackBox = CamShift(backproj, trackWindow,               //TermCriteria爲確定迭代終止的準則
                                    TermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ));//CV_TERMCRIT_EPS是通過forest_accuracy,CV_TERMCRIT_ITER
				send_pos_error(fd, trackBox.center.x, trackBox.center.y);

	
                if( trackWindow.area() <= 1 )                                                  //是通過max_num_of_trees_in_the_forest  
                {
                    int cols = backproj.cols, rows = backproj.rows, r = (MIN(cols, rows) + 5)/6;
                    trackWindow = Rect(trackWindow.x - r, trackWindow.y - r,
                                       trackWindow.x + r, trackWindow.y + r) &
                                  Rect(0, 0, cols, rows);//Rect函數爲矩陣的偏移和大小，即第一二個參數爲矩陣的左上角點座標，第三四個參數爲矩陣的寬和高
                }

                if( backprojMode )
                    cvtColor( backproj, image, CV_GRAY2BGR );//顯示模式翻轉 從灰度圖轉成RGB彩圖
                ellipse( image, trackBox, Scalar(0,0,255), 3, CV_AA );//跟蹤的時候以橢圓爲代表目標
            }
        }

        //後面的代碼是不管pause爲真還是爲假都要執行的
        else if( trackObject < 0 )
            paused = false;

        if( selectObject && selection.width > 0 && selection.height > 0 )//如果正處於物體選擇，畫出選擇框
        {
            Mat roi(image, selection);
            bitwise_not(roi, roi);//bitwise_not爲將每一個bit位取反
        }

        imshow( "CamShift Demo", image );
        //imshow( "Histogram", histimg );//close temporarily

        char c = (char)waitKey(10);
        if( c == 27 )              //退出鍵
            break;
        switch(c)
        {
        case 'b':             //反向投影模型交替
            backprojMode = !backprojMode;
            break;
        case 'c':            //清零跟蹤目標對象
            trackObject = 0;
            histimg = Scalar::all(0);
            break;
        case 'h':          //顯示直方圖交替
            showHist = !showHist;
            if( !showHist )
                destroyWindow( "Histogram" );
            else
                namedWindow( "Histogram", 1 );
            break;
        case 'p':       //暫停跟蹤交替
            paused = !paused;
            break;
        case 'w':       //最大跟蹤速度提高
            speed_max+=2;
			cout<<"Speed_Max now is: "<< speed_max <<endl;
            break;
        case 's':       //最大跟蹤速度降低
            speed_max-=2;
			cout<<"Speed_Max now is: "<< speed_max <<endl;
            break;
        case 'a':       //跟蹤速度降低
            P+=2;
			cout<<"Speed_Down!!\t\tSpeed now is: "<< P <<endl;
            break;
        case 'd':       //跟蹤速度提高
            P-=2;
			cout<<"Speed_UP!!\t\t Speed now is: "<< P <<endl;
            break;
		
        default:
            ;
        }
		}
		catch(cv::Exception& e)
		{
		const char* err_msg= e.what();
		std::cout << "exception caught: " << err_msg << std::endl;
		cout<<"please initialize the track windows again"<<endl;
		cout<<"Servos move to middle!!"<<endl;
	
		xc=1560;xc_last=1560;
		yc=1590;yc_last=1560;
	    x_tune=0;
	    y_tune=0;
		usleep(100000);
		cnt_write=serialport_write(fd, "1560+1590\n");
    	if(cnt_write==-1) error("error writing");
		sleep(1);
		cnt_write=serialport_write(fd, "1560+1590\n");
		trackObject = 0;
		}

    }
     exit(EXIT_SUCCESS);
}

int serialport_init(const char* serialport, int baud)
{
    struct termios toptions;
   //fd = open(serialport, O_RDWR | O_NOCTTY | O_NDELAY);
    fd = open(serialport, O_RDWR | O_NONBLOCK );
    
    if (fd == -1)  {
        perror("serialport_init: Unable to open port ");
        return -1;
    }
    
    //int iflags = TIOCM_DTR;
    //ioctl(fd, TIOCMBIS, &iflags);     // turn on DTR
    //ioctl(fd, TIOCMBIC, &iflags);    // turn off DTR

    if (tcgetattr(fd, &toptions) < 0) {
        perror("serialport_init: Couldn't get term attributes");
        return -1;
    }
    speed_t brate = baud; // let you override switch below if needed
    switch(baud) {
    case 4800:   brate=B4800;   break;
    case 9600:   brate=B9600;   break;
#ifdef B14400
    case 14400:  brate=B14400;  break;
#endif
    case 19200:  brate=B19200;  break;
#ifdef B28800
    case 28800:  brate=B28800;  break;
#endif
    case 38400:  brate=B38400;  break;
    case 57600:  brate=B57600;  break;
    case 115200: brate=B115200; break;
    }
    cfsetispeed(&toptions, brate);
    cfsetospeed(&toptions, brate);

    // 8N1
    toptions.c_cflag &= ~PARENB;
    toptions.c_cflag &= ~CSTOPB;
    toptions.c_cflag &= ~CSIZE;
    toptions.c_cflag |= CS8;
    // no flow control
    toptions.c_cflag &= ~CRTSCTS;

    //toptions.c_cflag &= ~HUPCL; // disable hang-up-on-close to avoid reset

    toptions.c_cflag |= CREAD | CLOCAL;  // turn on READ & ignore ctrl lines
    toptions.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl

    toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
    toptions.c_oflag &= ~OPOST; // make raw

    // see: http://unixwiz.net/techtips/termios-vmin-vtime.html
    toptions.c_cc[VMIN]  = 0;
    toptions.c_cc[VTIME] = 0;
    //toptions.c_cc[VTIME] = 20;
    
    tcsetattr(fd, TCSANOW, &toptions);
    if( tcsetattr(fd, TCSAFLUSH, &toptions) < 0) {
        perror("init_serialport: Couldn't set term attributes");
        return -1;
    }

    return fd;
}


int serialport_close( int fd )
{
    return close( fd );
}

int serialport_write(int fd, const char* str)
{
    int len = strlen(str);
    int n = write(fd, str, len);
    if( n!=len ) {
        perror("serialport_write: couldn't write whole string\n");
        return -1;
    }
    return 0;
}

int serialport_flush(int fd)
{
    sleep(1); //required to make flush work, for some reason
    return tcflush(fd, TCIOFLUSH);
}

void error(const char* msg)
{
    fprintf(stderr, "%s\n",msg);
    exit(EXIT_FAILURE);
}
void onMouse( int event, int x, int y, int, void* )
{
    if( selectObject )//只有當鼠標左鍵按下去時纔有效，然後通過if裏面代碼就可以確定所選擇的矩形區域selection了
    {
        selection.x = MIN(x, origin.x);//矩形左上角頂點座標
        selection.y = MIN(y, origin.y);
        selection.width = std::fabs(x - origin.x);//矩形寬
        selection.height = std::fabs(y - origin.y);//矩形高

        selection &= Rect(0, 0, image.cols, image.rows);//用於確保所選的矩形區域在圖片範圍內
    }

    switch( event )
    {
    case CV_EVENT_LBUTTONDOWN:
        origin = Point(x,y);
        selection = Rect(x,y,0,0);//鼠標剛按下去時初始化了一個矩形區域
        selectObject = true;
        break;
    case CV_EVENT_LBUTTONUP:
        selectObject = false;
        if( selection.width > 0 && selection.height > 0 )
            trackObject = -1;
        break;
    }
}


void help()
{
    cout << "\nThis is a demo that shows mean-shift based tracking\n"
            "You select a color objects such as your face and it tracks it.\n"
            "This reads from video camera (0 by default, or the camera number the user enters\n"
            "Usage: \n"
            "    ./camshiftdemo [camera number]\n";

    cout << "\n\nHot keys: \n"
            "\tESC - quit the program\n"
            "\tc - stop the tracking\n"
            "\tb - switch to/from backprojection view\n"
            "\th - show/hide object histogram\n"
            "\tp - pause video\n"
            "To initialize tracking, select the object with mouse\n";
}
void send_pos_error(int fd, float x, float y){


	float error_x,error_y;
	int xx,yy;


	const int buf_max = 256;
	char buf[buf_max];
	int rc;

	usleep(1000);//1ms*70+30=100ms
  
    if((send_delay_cnt++)%3== 0){
		//printf("send_delay_cnt:%d",send_delay_cnt);
		error_x=x-320;
		error_y=y-240;
		x_tune=(error_x/P);
		y_tune=(error_y/P);
		xx=(int)x_tune;
		yy=(int)y_tune;

		if(fabs(xx)<1 ) 	xc=xc;
		else if(fabs(xx<640)) xc=xc-xx;
		else xc=xc_last;

		if (fabs(yy)<1) yc=yc;
		else if(fabs(yy)<480)yc=yc-yy;

		if(abs(xc)>10000 ||abs(yc)>10000) 
		{xc=xc_last;yc=yc_last;printf("data error!!\r\n");}

		if(fabs(xc-xc_last)>speed_max){
			printf("XC_last:%d XC:%d\r\n",xc_last,xc);
			printf("x direction changing sharply!!!\r\n");  
			if (error_x >0) xc=xc_last-speed_max;
			else xc=xc_last+speed_max; 
		}
		if(fabs(yc-yc_last)>speed_max){
			printf("YC_last:%d YC:%d\r\n",yc_last,yc);
			printf("y direction changing sharply!!!\r\n");
			if (error_y >0) yc=yc_last-speed_max;
			else yc=yc_last+speed_max; 
		}
    
		if(xc<800)xc=800;
		if(xc>2200)xc=2200;
		if(yc<1100)yc=1100;
		if(yc>1900)yc=1900;


		xc_last=xc;
		yc_last=yc;

	//	printf("xx:%d yy:%d\r\n",xx,yy);
		printf("x_tune:%lf y_tune:%lf\r\n",x_tune,y_tune);
	 	sprintf(buf,"%d+%d\n",xc,yc);
		rc = serialport_write(fd, buf);//send 10次/s
   		if(rc==-1) error("error writing");
   		printf("send string:%s\n", buf);
	}	
}

void open_serial(){
	strcpy(serialport,"/dev/ttyUSB0");
	fd = serialport_init(serialport, baudrate);
	if( fd==-1 ){
		serialport_close(fd);
		printf("Couldn't open default serialport\r\n");
		printf("Trying open ttyUSB1...\r\n");
		strcpy(serialport,"/dev/ttyUSB1");
		fd = serialport_init(serialport, baudrate);
		//serialport_close(fd);
		if(fd==-1)
			error("couldn't open port,please check the hardware connect!");		  
	}
	printf("opened port %s\n",serialport);
	serialport_flush(fd);
}