2019电赛 H题-电磁炮 视觉部分
2019电赛-H题-电磁炮-视觉自瞄部分;
硬件平台: Jetson Nano, Realsense D435
实现功能：识别到引导标志反馈坐标及距离，串口输出
#include
using namespace std;
#include
#include
#include
#include
#include
#include
#include
#include
using namespace cv;
#include
#include
#include "stdio.h"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
/**
* open port
* @param fd
* @param comport
* @return
*/
int open_port(int fd,int comport)
{
//char *dev[]={L"/dev/ttyUSB0",L"/dev/ttyS1",L"/dev/ttyS2"};
if (comport==1)
{
fd = open("/dev/ttyTHS1", O_RDWR|O_NOCTTY|O_NDELAY);
if (-1 == fd)
{
perror("Can't Open Serial Port");
return(-1);
}
}
else if(comport==2)
{
fd = open( "/dev/ttyTHS2", O_RDWR|O_NOCTTY|O_NDELAY);
if (-1 == fd)
{
perror("Can't Open Serial Port");
return(-1);
}
}
else if (comport==3)
{
fd = open( "/dev/ttyS2", O_RDWR|O_NOCTTY|O_NDELAY);
if (-1 == fd)
{
perror("Can't Open Serial Port");
return(-1);
}
}
if(fcntl(fd, F_SETFL, 0)<0)
printf("fcntl failed!\n");
else
printf("fcntl=%d\n",fcntl(fd, F_SETFL,0));
if(isatty(STDIN_FILENO)==0)
printf("standard input is not a terminal device\n");
else
printf("isatty success!\n");
printf("fd-open=%d\n",fd);
return fd;
}
int set_opt(int fd,int nSpeed, int nBits, char nEvent, int nStop)
{
struct termios newtio,oldtio;
if ( tcgetattr( fd,&oldtio) != 0) {
perror("SetupSerial 1");
printf("tcgetattr( fd,&oldtio) -> %d\n",tcgetattr( fd,&oldtio));
return -1;
}
bzero( &newtio, sizeof( newtio ) );
newtio.c_cflag |= CLOCAL | CREAD;
newtio.c_cflag &= ~CSIZE;
switch( nBits )
{
case 7:
newtio.c_cflag |= CS7;
break;
case 8:
newtio.c_cflag |= CS8;
break;
}
switch( nEvent )
{
case 'o':
case 'O':
newtio.c_cflag |= PARENB;
newtio.c_cflag |= PARODD;
newtio.c_iflag |= (INPCK | ISTRIP);
break;
case 'e':
case 'E':
newtio.c_iflag |= (INPCK | ISTRIP);
newtio.c_cflag |= PARENB;
newtio.c_cflag &= ~PARODD;
break;
case 'n':
case 'N':
newtio.c_cflag &= ~PARENB;
break;
default:
break;
}
switch( nSpeed )
{
case 2400:
cfsetispeed(&newtio, B2400);
cfsetospeed(&newtio, B2400);
break;
case 4800:
cfsetispeed(&newtio, B4800);
cfsetospeed(&newtio, B4800);
break;
case 9600:
cfsetispeed(&newtio, B9600);
cfsetospeed(&newtio, B9600);
break;
case 115200:
cfsetispeed(&newtio, B115200);
cfsetospeed(&newtio, B115200);
break;
case 460800:
cfsetispeed(&newtio, B460800);
cfsetospeed(&newtio, B460800);
break;
default:
cfsetispeed(&newtio, B9600);
cfsetospeed(&newtio, B9600);
break;
}
if( nStop == 1 )
newtio.c_cflag &= ~CSTOPB;
else if ( nStop == 2 )
newtio.c_cflag |= CSTOPB;
newtio.c_cc[VTIME] = 0;
newtio.c_cc[VMIN] = 0;
tcflush(fd,TCIFLUSH);
if((tcsetattr(fd,TCSANOW,&newtio))!=0)
{
perror("com set error");
return -1;
}
printf("set done!\n");
return 0;
}
// char buf[10]={0};
// int n=0;
// perror("璇诲彇涓插彛鏁版嵁\n");
// while( n >= 0)
// {
// buf[0]=0;
// n = read(fd,buf,10);
// if(n<0)
// {
// perror("read STDIN_FILENO\n");
// exit(1);
// break;
// }
// if( n > 0)
// {
// printf("璇诲埌鏁版嵁=%d,%s\n",n,buf);
// write(fd,buf,n);
// if(buf[0] == 'Q')
// {
// break;
// }
// memset(buf,10,0);
// }
// }
// perror("绋嬪簭鎵ц缁撴潫\n");
// return 0;
//获取深度像素对应长度单位（米）的换算比例
float get_depth_scale(rs2::device dev)
{
// Go over the device's sensors
for (rs2::sensor& sensor : dev.query_sensors())
{
// Check if the sensor if a depth sensor
if (rs2::depth_sensor dpt = sensor.as())
{
return dpt.get_depth_scale();
}
}
throw std::runtime_error("Device does not have a depth sensor");
}
//深度图对齐到彩色图函数
Mat align_Depth2Color(Mat depth,Mat color,rs2::pipeline_profile profile){
//声明数据流
auto depth_stream=profile.get_stream(RS2_STREAM_DEPTH).as();
auto color_stream=profile.get_stream(RS2_STREAM_COLOR).as();
//获取内参
const auto intrinDepth=depth_stream.get_intrinsics();
const auto intrinColor=color_stream.get_intrinsics();
//直接获取从深度摄像头坐标系到彩色摄像头坐标系的欧式变换矩阵
//auto extrinDepth2Color=depth_stream.get_extrinsics_to(color_stream);
rs2_extrinsics extrinDepth2Color;
rs2_error *error;
rs2_get_extrinsics(depth_stream,color_stream,&extrinDepth2Color,&error);
//平面点定义
float pd_uv[2],pc_uv[2];
//空间点定义
float Pdc3[3],Pcc3[3];
//获取深度像素与现实单位比例（D435默认1毫米）
float depth_scale = get_depth_scale(profile.get_device());
int y=0,x=0;
//初始化结果
//Mat result=Mat(color.rows,color.cols,CV_8UC3,Scalar(0,0,0));
Mat result=Mat(color.rows,color.cols,CV_16U,Scalar(0));
//对深度图像遍历
for(int row=0;row(row,col);
//换算到米
float depth_m=depth_value*depth_scale;
//将深度图的像素点根据内参转换到深度摄像头坐标系下的三维点
rs2_deproject_pixel_to_point(Pdc3,&intrinDepth,pd_uv,depth_m);
//将深度摄像头坐标系的三维点转化到彩色摄像头坐标系下
rs2_transform_point_to_point(Pcc3,&extrinDepth2Color,Pdc3);
//将彩色摄像头坐标系下的深度三维点映射到二维平面上
rs2_project_point_to_pixel(pc_uv,&intrinColor,Pcc3);
//取得映射后的（u,v)
x=(int)pc_uv[0];
y=(int)pc_uv[1];
// if(x<0||x>color.cols)
// continue;
// if(y<0||y>color.rows)
// continue;
//最值限定
x=x<0? 0:x;
x=x>depth.cols-1 ? depth.cols-1:x;
y=y<0? 0:y;
y=y>depth.rows-1 ? depth.rows-1:y;
result.at(y,x)=depth_value;
}
}
//返回一个与彩色图对齐了的深度信息图像
return result;
}
void measure_distance(Mat &color,Mat depth,cv::Size range,rs2::pipeline_profile profile,cv::Point point,float *distance)
{
//获取深度像素与现实单位比例（D435默认1毫米）
float depth_scale = get_depth_scale(profile.get_device());
//定义图像中心点
//定义计算距离的范围
cv::Rect RectRange(point.x-range.width/2,point.y-range.height/2,range.width,range.height);
//遍历该范围
float distance_sum=0;
int effective_pixel=0;
for(int y=RectRange.y;y(y,x)){
distance_sum+=depth_scale*depth.at(y,x);
effective_pixel++;
}
}
}
//cout<<"遍历完成，有效像素点:"<(p)[0]);
printf("g=%d\t", rgb.at(p)[1]);
printf("r=%d\n", rgb.at(p)[2]);
printf("H=%d\t", hsv.at(p)[0]);
printf("S=%d\t", hsv.at(p)[1]);
printf("V=%d\n", hsv.at(p)[2]);
circle(rgb, p, 2, Scalar(255), 3);
}
break;
}
}
int main()
{
const char* depth_win="depth_Image";
namedWindow(depth_win,WINDOW_AUTOSIZE);
const char* color_win="color_Image";
namedWindow(color_win,WINDOW_AUTOSIZE);
int fd;
uint8_t buf[8];
fd = open_port(0,1);
if(fd == -1)
{
perror("cannot open port!\n");
return 0;
}
int cshjg;
cshjg = set_opt(fd,115200,8,'N',1);
fd = open_port(0,1);
if(cshjg == -1)
{
perror("cannot open port!\n");
return 0;
}
//深度图像颜色map
rs2::colorizer c; // Helper to colorize depth images
//创建数据管道
rs2::pipeline pipe;
rs2::config pipe_config;
pipe_config.enable_stream(RS2_STREAM_DEPTH,640,480,RS2_FORMAT_Z16,30);
pipe_config.enable_stream(RS2_STREAM_COLOR,640,480,RS2_FORMAT_BGR8,30);
//start()函数返回数据管道的profile
rs2::pipeline_profile profile = pipe.start(pipe_config);
//定义一个变量去转换深度到距离
float depth_clipping_distance = 1.f;
//声明数据流
auto depth_stream=profile.get_stream(RS2_STREAM_DEPTH).as();
auto color_stream=profile.get_stream(RS2_STREAM_COLOR).as();
//获取内参
auto intrinDepth=depth_stream.get_intrinsics();
auto intrinColor=color_stream.get_intrinsics();
//直接获取从深度摄像头坐标系到彩色摄像头坐标系的欧式变换矩阵
auto extrinDepth2Color=depth_stream.get_extrinsics_to(color_stream);
while (cvGetWindowHandle(depth_win)&&cvGetWindowHandle(color_win)) // Application still alive?
{
//堵塞程序直到新的一帧捕获
rs2::frameset frameset = pipe.wait_for_frames();
//取深度图和彩色图
rs2::frame color_frame = frameset.get_color_frame();//processed.first(align_to);
rs2::frame depth_frame = frameset.get_depth_frame();
rs2::frame depth_frame_4_show = frameset.get_depth_frame().apply_filter(c);
//获取宽高
const int depth_w=depth_frame.as().get_width();
const int depth_h=depth_frame.as().get_height();
const int color_w=color_frame.as().get_width();
const int color_h=color_frame.as().get_height();
//创建OPENCV类型 并传入数据
Mat depth_image(Size(depth_w,depth_h),
CV_16U,(void*)depth_frame.get_data(),Mat::AUTO_STEP);
Mat depth_image_4_show(Size(depth_w,depth_h),
CV_8UC3,(void*)depth_frame_4_show.get_data(),Mat::AUTO_STEP);
Mat color_image(Size(color_w,color_h),
CV_8UC3,(void*)color_frame.get_data(),Mat::AUTO_STEP);
int low_H =160,low_S = 180,low_V = 20;
int High_H = 180,High_S = 255,High_V = 150;
int x,y;
Mat src,rrr;
vector circles;
cvtColor(color_image, src, COLOR_BGR2HSV);//从BGR->HSV
inRange(src, Scalar(low_H, low_S, low_V), Scalar(High_H, High_S, High_V), src);//二值化
GaussianBlur(src, src, Size(5, 3), 2, 2);
HoughCircles(src, circles, CV_HOUGH_GRADIENT, 2,10,200,100,10,30);//找圆
cv::Point cc1;
for(size_t i = 0;i < circles.size();++i)
{
Point center(cvRound(circles[i][0]),cvRound(circles[i][1]));
int radius = cvRound(circles[i][2]);
circle(src,center,3,Scalar(0,255,0),-1,8,0);
circle(src, center, radius, Scalar(155,50,255), 3, 8, 0 );
cc1 = center;
}
//实现深度图对齐到彩色图
Mat result=align_Depth2Color(depth_image,color_image,profile);
float distance ;
measure_distance(color_image,result,cv::Size(20,20),profile,cc1,&distance);
printf("x = %d\t,y = %d\t,d = %f\n ",cc1.x,cc1.y,distance);
buf[0] = 0x5a;
buf[1] = 0xa5;
buf[2] = cc1.x;
buf[3] = cc1.y;
buf[4] = distance;
buf[5] =((uint16_t) (cc1.x+cc1.y+distance))&0xff;
buf[6] = 0xff;
buf[7] = 0xfe;
write(fd,buf,8);
//显示
//imshow(depth_win,depth_image_4_show);
imshow(color_win,color_image);
//imshow("result",src);
waitKey(1);
}
// write(fd,buf,8);
return 0;
}
