开放式视觉伺服平台源代码_上位机（初步版本，未对细节做修订）

/***********************************************************************
 * 开放式视觉伺服平台测试例程
 * 作者:李智超(西北工业大学)
 * 如需转载，请注明出处
 * 测试环境为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);
}