您的位置：首页 > 运维架构 > Linux

基于Linux的UAV图像处理与数据通信Project剖析

2012-09-18 17:38 417 查看
对于一些简单的单片机程序，很多人可能不会感到陌生，无非是一条道走到黑，不会涉及到复杂的多线程与各种机制设计。但是，再深入一点，添加了算法,控制，通信等module的软件就不那么好理解了。计算机语言同自然语言一样，也是在传达一种情感，只是这种情感不只是湿漉漉的情愫，更是在完成某一项程序员赋予的使命。很多人觉得某某会C，C++，计算机过了3级，以为很厉害，在为看来，会语言只是第一步，关键在于你要用语言来表达点什么，机器按照你的想法来做事的时候，也完成了人与机器精神上的融合。

此处特别开源一段笔者写的UAV Project中的算法与通信程序，主要完成无人机视觉跟踪，数据交互等工作，核心部分已经屏蔽，程序经过多重性能分析，无Bug,提供给各位看官参考(软件基于linux3.0 kernel,采用eclipse IDE开发)：

（By the way:本无人机工程是笔者带领团队前后5个多月，花费近４万元的研究成果，期间受到了王越院士的亲临指导，笔者正致力于开发国内第一个OpenSource UAV Project，之后会将研究过程中碰到的诸多细节一一在ＣＳＤＮ博客中转述）

code content:

/*

* file name: uav_algorithm.cpp

* notes: ESDC2012 测试通过，只依赖opencv库，可以打开摄像头(解决方法见日志记录)

* Created on: 2012-4-28

* Author: CSU Lx-Tech team software designer 王龙

* Version: v2.0 Release

* Last Modify:

* 2012-5-18调试日志：目前算法的处理速度很慢，在没有开gnome的情况下只有1fps，需要优化函数，采用Release版本可以提速30%

* 2012-6-7:添加了图像的发送以及bounding box的接收功能，但未经过无线传输测试

* 2012-6-14：存在的问题：多线程对一个socket同时发送接收的处理尚未解决，所以bounding box无法发回给UAV

* 2012-6-15:采用多线程实现并发传输，客户端创建多个端口使用connect()发起连接请求，主线程响应连接请求，一旦建立连接

* 即创建一个新线程执行thread_handle();

* 2012-6-17：采用Release版本并裁剪了linux server版本后，算法运行速度提高1.5倍

* 2012-6-20: 师兄提供思路：发送4帧左右，处理一帧，提高视觉效果

* 设计规划：cap>>frame需要采用读写锁,threadA threadB采集图像不能同时进行。TCP_Send需要互斥锁，TCP不能在两个线程中同时使用

* 将各个模块所用的时间算出来，看采用多线程并行执行是否有助于加速跟踪效果

* 采用Intel C++编译器

* 2012-6-22:将各个部分执行所消耗的时间计算出来，为程序优化作准备

* 分析结果：capture:15ms encode_send:15ms TLD process:200-300ms不等所以单就此处多线程似乎帮助不大

* intel性能分析指出：

* FerNNClassifier::getFeatures函数占用资源排第二

* hog算子直接采集发送回地面站显示没有时滞，所以可认定是算法导致的延时6s

* 传输如果不采用多线程的话，在网络状况不好的情况下会阻塞算法处理 速度提升了10%

*/

/*OpenCV including files*/

#include <opencv2/opencv.hpp>

#include <unistd.h>

#include <sys/types.h>

#include "tld_utils.h"

#include "TLD.h"

#include <iostream>

#include <sstream>

#include <stdio.h>

/*UART including files*/

#include "serial.h"

/*TCP including files*/

#include "tcp.h"

/*sem and mutil-thread including files*/

#include <semaphore.h>

#include <sys/ipc.h>

#include <pthread.h>

#include <time.h>

/*Global variables*/

int serial_fd;

Rect box;

bool drawing_box = false;

bool gotBB = false;

bool tl = true;

bool rep = false;

bool fromfile=false;

string video;

int i;

Mat ugs_img; //等待发送到地面站的图像

/*extern variables*/

extern pthread_t id1,id2,id3,id4;

/*cond and mutex variables*/

pthread_mutex_t lock;

pthread_cond_t jpg_enable; //jpg可以发送了

int jpg_sock; //定义成图像发送线程的全局端口

Point uwb; //定位系统坐标

bool PTZ_EN=0; //云台开关

/*串口及显示功能使能开关位 defines*/

//#define UART_EN //串口使能位

//#define UI_EN //显示使能

#define UART_CMD

/*static functions*/

/*条件变量跟互斥量初始化*/

void cond_init(void)

{

int ret;

ret=pthread_mutex_init(&lock,NULL);

if(ret!=0)

{

printf("mutex initialization failed!\n");

exit(0);

}

ret=pthread_cond_init(&jpg_enable,NULL);

if(ret!=0)

{

printf("cond init failed!\n");

exit(0);

}

}

/*自动读取paraments.yml文件的函数*/

void auto_read(FileStorage &fs)

{

fs.open("/opt/arthurv-OpenTLD-1e3cd0b/parameters.yml",FileStorage::READ);

}

void *SendCoordinate2UGS(void *id)

{

cout<<"this is the UWB Coordinate sending thread!"<<endl;

/*将socket传入*/

int sock;

int *ir=(int *)id;

sock=*ir;

/*此时串口已经打开*/

// char buf[1]={0,0};

char buf; //对应的发送端程序：Uart3_tx(0xff);Uart3_tx((unsigned char)Point.x);Uart3_tx((unsigned char)Point.y);

/*发送端是unsigned char*/

unsigned char bufX=0; //用来存放x坐标

unsigned char bufY=0; //用来存放y坐标

while(1)

{

/*接收标志位*/

usleep(10000);

int res1=read(serial_fd,&buf,1);

/*分析：因为本处需要的是阻塞读取串口，串口没有数据的时候read()函数不能返回*/

/*如果是起始位，则开始接收X坐标*/

if(buf==125)

{

int res2=read(serial_fd,&bufX,1);

printf("res2:%d\n",res2);

printf("X:%d\n",bufX);

/*接收Y坐标*/

int res3=read(serial_fd,&bufY,1);

printf("res3:%d\n",res3);

printf("Y:%d\n",bufY);

/*通过TCP发送*/

uwb.x=(int)bufX;

uwb.y=(int)bufY;

/*判断发送的总数据量为6时才发送*/

if(res1+res2+res3==3)

{

send(sock,&uwb,sizeof(Point),0);

printf("TCP send one UWB coordinate\n");

}

else

{

printf("there is no UWB Coordinate to send!\n ");

}

/*对应接收端的处理*/

}

else

{

printf("start flag recv error!\n");

}

}

close(sock);

close(serial_fd);

pthread_exit(0);

}

/*接收地面站发送来的控制命令，并通过串口发送*/

void *RecvCmd_Thread(void *id)

{

cout<<"this is the cmd recv thread!"<<endl;

/*将socket传入*/

int sock;

int *ir=(int *)id;

sock=*ir;

int uav_cmd;

/*初始化串口*/

#ifdef UART_CMD

char* port="/dev/ttyUSB0";

serial_fd=open_port(port);

if(serial_fd==-1)

{

printf("serial port open failure!\n");

exit(1);

}

set_ports(serial_fd); //设置串口

fcntl(serial_fd,F_SETFL,0); //设置为阻塞模式，已经过测试

#endif

/*main loop*/

while(1)

{

int len=recv(sock,&uav_cmd,sizeof(int),0);

if(len!=sizeof(int))

{

printf("uav cmd receive error!\n");

}

/*如果接收正确*/

else

{

printf("UAV CMD:%d\n",uav_cmd);

/*开启云台标志位*/

if(uav_cmd==64)

{

PTZ_EN=1;

}

else if(uav_cmd==62)

{

PTZ_EN=0;

}

else

{

/*将接收到的控制指令通过串口发送给Nios*/

#ifdef UART_CMD

/*控制指令标识位*/

unsigned char cmd = (unsigned char)uav_cmd;

pthread_mutex_lock(&lock); //锁定互斥量

Send2_Nios(serial_fd,&cmd,Cmd_Type);

pthread_mutex_unlock(&lock);

}

#endif

}

}

close(sock);

pthread_exit(NULL);

}

/*图像处理线程*/

void *ImgProc_Thread(void *id)

{

printf("This is the imgProc thread!\n");

/*video variables*/

VideoCapture capture;

/*将socket传入*/

int *ir=(int *)id;

jpg_sock=*ir;

/*Open camera*/

capture.open(0); //打开/dev/video0

FileStorage fs; //定义一个文件标志符

auto_read(fs); //读取参数

if (!capture.isOpened()) //如果没有成功打开摄像头设备，打印错误并退出

{

cout << "capture device failed to open!" << endl;

return 0;

}

TLD tld; //定义TLD类 （在TLD.h中声明）

//Read parameters file

tld.read(fs.getFirstTopLevelNode()); //取出paremeters.yml中的参数

Mat frame;

Mat last_gray;

Mat first;

/*设置帧的尺寸*/

capture.set(CV_CAP_PROP_FRAME_WIDTH,320);

capture.set(CV_CAP_PROP_FRAME_HEIGHT,240);

/*用了一个goto语句*/

GETBOUNDINGBOX:

while(!gotBB) //只有在gotBB=true才会退出

{

capture>>frame; //抓取一帧

cvtColor(frame, last_gray, CV_RGB2GRAY);//转换到GRAY空间

/*发送第一帧*/

Encode_Send(frame,jpg_sock);

}

/*如果圈定的大小小于设置的最小窗口，提醒用户重新圈定*/

if (min(box.width,box.height)<(int)fs.getFirstTopLevelNode()["min_win"])

{

cout << "Bounding box too small, try again.you can modify parament.yml"<< endl;

gotBB = false;

goto GETBOUNDINGBOX;

}

printf("Initial Bounding Box = x:%d y:%d h:%d w:%d\n",box.x,box.y,box.width,box.height);

//以txt文件的形式输出

FILE *bb_file = fopen("bounding_boxes.txt","w");

//TLD初始化，这是TLD.cpp提供的API

tld.init(last_gray,box,bb_file); //这里调用了buildGrid()这个函数不知是不是用GTK

///Run-time

Mat current_gray;

BoundingBox pbox;

vector<Point2f> pts1; //在一些情况下，vector可以被描述为灵活的c风格数组，它对单个任意排序的元素进行高效访问，减小用户内存分配的麻烦

vector<Point2f> pts2;

bool status=true;

int frames = 1;

int detections = 1;

REPEAT:

while(capture.read(frame))

{

//get frame

cvtColor(frame, current_gray, CV_RGB2GRAY);

//Process Frame

tld.processFrame(last_gray,current_gray,pts1,pts2,pbox,status,tl,bb_file);

//Draw Points

if (status)

{

drawPoints(frame,pts1);

drawPoints(frame,pts2,Scalar(0,255,0));

drawBox(frame,pbox);

/*pbox是为可以使用的final数据，在此处通过串口发送*/

int x1=pbox.x; //起始点横坐标

int y1=pbox.y; //起始点纵坐标

int w=pbox.width;//bounding box宽度

int h=pbox.height;//bounding box高度

int x=x1+w/2;

int y=y1+h/2;

/*PTZ云台调整算法*/

unsigned char DeviationX; //坐标与最优区域的偏差

unsigned char DeviationY;

/*x在最优区域的左边*/

if(x>=0&&x<=120)

{

DeviationX=120-x;

DeviationX=(unsigned char)cvRound(DeviationX/6);

DeviationX |= 0x80;

if(PTZ_EN)

{

pthread_mutex_lock(&lock);

Send2_Nios(serial_fd,&DeviationX,Co_Type);

pthread_mutex_unlock(&lock);

}

}

/*x在最优区域右边*/

else if(x>=200&&x<=320)

{

DeviationX=x-200;

DeviationX=(unsigned char)cvRound(DeviationX/6);

DeviationX |= 0xc0;

if(PTZ_EN)

{

pthread_mutex_lock(&lock);

Send2_Nios(serial_fd,&DeviationX,Co_Type);

pthread_mutex_unlock(&lock);

}

}

else

{

//do nothing

}

/*y坐标*/

if(y>=150&&y<=240)

{

DeviationY=y-150;

DeviationY=(unsigned char)cvRound(DeviationY/6);

DeviationY |= 0x00;

if(PTZ_EN)

{

pthread_mutex_lock(&lock);

Send2_Nios(serial_fd,&DeviationY,Co_Type);

pthread_mutex_unlock(&lock);

}

}

else if(y>=0&&y<=90)

{

DeviationY=90-y;

DeviationY=(unsigned char)cvRound(DeviationY/6);

DeviationY |= 0x40;

if(PTZ_EN)

{

pthread_mutex_lock(&lock);

Send2_Nios(serial_fd,&DeviationY,Co_Type);

pthread_mutex_unlock(&lock);

}

}

detections++;

}

frames++;

/*已经将目标圈出来，发送回地面站*/

char text[15];

char text1[15];

char locationx[15];

char locationy[15];

char uwbx[10];

char uwby[10];

char track1[10]="T:tracked"; //显示是否跟踪到

char track2[15]="T:not tracking";

sprintf(text,"detections:%d",detections);

sprintf(text1,"frames:%d",frames);

sprintf(locationx,"x1:%d",pbox.x);

sprintf(locationy,"Y1:%d",pbox.y);

sprintf(uwbx,"x2:%d",uwb.x);

sprintf(uwby,"y2:%d",uwb.y);

if(status)

{

putText(frame,track1,Point(5,230),CV_FONT_HERSHEY_COMPLEX,0.5,Scalar(255,255,255),1,8,false);

}

else

{

putText(frame,track2,Point(5,230),CV_FONT_HERSHEY_COMPLEX,0.5,Scalar(0,255,255),1,8,false);

}

putText(frame,text,Point(5,20),CV_FONT_HERSHEY_COMPLEX,0.5,Scalar(255,255,255),1,8,false);

putText(frame,text1,Point(5,40),CV_FONT_HERSHEY_COMPLEX,0.5,Scalar(255,255,255),1,8,false);

putText(frame,locationx,Point(5,60),CV_FONT_HERSHEY_COMPLEX,0.5,Scalar(255,255,255),1,8,false);

putText(frame,locationy,Point(5,80),CV_FONT_HERSHEY_COMPLEX,0.5,Scalar(255,255,255),1,8,false);

putText(frame,uwbx,Point(5,100),CV_FONT_HERSHEY_COMPLEX,0.5,Scalar(0,255,255),1,8,false);

putText(frame,uwby,Point(5,120),CV_FONT_HERSHEY_COMPLEX,0.5,Scalar(0,255,255),1,8,false);

rectangle(frame, cvPoint(1,1),cvPoint(319,239),Scalar(255,255,255),1);

/***********************************************************************************/

/*算法处理后的发送，采用多线程操作，此处发送信号量*/

ugs_img=frame.clone(); //复制一份用于多线程发送

pthread_cond_signal(&jpg_enable);//发送信号给发送线程

/***************************************************************************/

//swap points and images 交换点和图像，将current_gray copy到last_gray

swap(last_gray,current_gray);

pts1.clear();

pts2.clear();

//frames++; //帧计数变量加1

//printf("Detection rate: %d/%d\n",detections,frames);

if (cvWaitKey(33) == 'q') //'q'是什么意思

break;

}

if (rep){

rep = false;

tl = false;

fclose(bb_file);

bb_file = fopen("final_detector.txt","w");

capture.release();

capture.open(video);

goto REPEAT;

}

fclose(bb_file);

pthread_exit(NULL);

}

void *RecvRect_Thread(void *id)

{

cout<<"this is the recv thread"<<endl;

/*将socket传入*/

int sock;

int *ir=(int *)id;

sock=*ir;

/*子线程阻塞等待地面站发回Rect*/

box=Recv_BoundingBox(sock);

gotBB=true;

printf("bounding box got done\n");

close(sock);

pthread_exit(NULL);

}

/*jpg图像发送线程*/

void *Sendjpg_Thread(void *)

{

printf("this is the jpg send thread!\n");

while(1)

{

/*不能上锁，否则会卡死*/

pthread_cond_wait(&jpg_enable,&lock);

Encode_Send(ugs_img,jpg_sock);

}

close(jpg_sock);

pthread_exit(NULL);

}

/*主函数*/

int main(void)

{

/*主线程*/

pthread_t id; //线程id

/*条件变量初始化*/

cond_init();

pthread_create(&id,NULL,Sendjpg_Thread,NULL);

Server_Init(); //此处创建了另外4个子线程

/*等待thread线程结束，回收线程资源*/

pthread_join(id1,NULL);

pthread_join(id2,NULL);

pthread_join(id3,NULL);

pthread_join(id4,NULL);

pthread_join(id,NULL);

return 0;

}

内容来自用户分享和网络整理，不保证内容的准确性，如有侵权内容，可联系管理员处理 点击这里给我发消息
标签： 
相关文章推荐
新的分享
章节导航