RoboMaster视觉教程(5)目标位置解算(通过像素点获取转角)
RoboMaster视觉教程(5)目标位置解算(通过像素点获取转角)
- 概览
- 直接使用像素坐标的缺陷
- 摄像头标定
- 根据小孔成像原理得到需要的转角
- 角度测量验证
概览
在识别到目标后,有一个很重要的问题:我们的最终目的是瞄准、跟踪、打击,怎样利用识别到目标后得到的目标在图像中的像素坐标来确定在真实世界中目标的位置呢?更清楚点说就是我识别得到的是图像中点的坐标,而我要输出告诉下位机的是它应该旋转或者移动到的目的地。
直接使用像素坐标的缺陷
在RoboMaster视觉辅助瞄准中我们需要让枪管一直瞄准装甲板,由于摄像头是装在枪管上的,一种显而易见的方法就是直接利用目标所在像素的坐标与图像中心坐标对比,然后利用一套控制算法移动摄像头使目标在摄像头的中心区域(东北林业大学ARES战队的开源代码是这种方式 GitHub:https://github.com/moxiaochong/NEFU-ARES-Vison-Robomaster2018)。
直接这样做的话PID调节会很麻烦,可以看到东林的代码中PID部分根据像素差的不同使用了不同的PID值。
像素差如果直接喂给PID,那PID只能知道我有偏差了需要照着这个方向转减小偏差,如果偏差大了就给大点偏差小了就少给点。
而发给战车云台的数据是旋转的角度,所以像素差与角度之间的关系需要不断调节PID来控制。并且因为像素差与角度并不是线性关系,导致在图像的不同地方需要不同的PID值。如下图:
可以看到如果角度在30度以下的时候角度与像素差的关系还可以近似线性,当大于30度后就无法近似了。
那怎样将像素差转化成角度呢?根据上面的图可以看出如果能够得到OF的距离就可以通过反正切函数来得到角度值了,而OF其实就是焦距,当然上图是理论情况,在实际应用中我们需要通过相机标定来得到需要的相机内参。
摄像头标定
因为我们需要从图像中得到关于物理世界的信息,所以需要标定摄像头来得到摄像头内参。
摄像头标定的工具有很多,OpenCV自带例子中就有标定工具,也可以用MATLAB来进行标定,标定方法可以参考:https://blog.csdn.net/Loser__Wang/article/details/51811347
根据小孔成像原理得到需要的转角
在标定完摄像头后我们会得到摄像头的内参矩阵和畸变参数:
cameraMatrix=[fx0cx0fycy001]distortionCoefficients=(k1k2p1p2k3)cameraMatrix=\left[ \begin{matrix} f_x & 0 & c_x\\ 0 & f_y & c_y\\ 0 & 0 &1 \end{matrix} \right] distortionCoefficients=(k_1 \ k_2 \ p_1 \ p_2 \ k_3) cameraMatrix=⎣⎡fx000fy0cxcy1⎦⎤distortionCoefficients=(k1 k2 p1 p2 k3)
首先根据摄像头内参矩阵和畸变参数矫正x和y的像素值,之后通过计算可以得到转角,原理在《Learning OpenCV》的第11章「摄像头模型与标定」中有。
像素点与物理世界坐标的关系:其中X、Y、Z为物理世界中点的坐标
xscreen=fx(XZ)+cxyscreen=fy(YZ)+cyx_{screen}=f_x(\frac{X}{Z})+c_x\\ \\ y_{screen}=f_y(\frac{Y}{Z})+c_y xscreen=fx(ZX)+cxyscreen=fy(ZY)+cy
我们要想得到转角只要得到X/Z和Y/Z,然后通过反三角函数就可以得到需要的角度了。
tanθx=XZ=xscreen−cxfxtanθy=YZ=yscreen−cyfyθx=arctan(tanθx)θy=arctan(tanθy)\tan\theta_x=\frac{X}{Z}=\frac{x_{screen}-c_x}{f_x}\\ \tan\theta_y=\frac{Y}{Z}=\frac{y_{screen}-c_y}{f_y}\\ \theta_x=\arctan(\tan\theta_x)\\ \theta_y=\arctan(\tan\theta_y) tanθx=ZX=fxxscreen−cxtanθy=ZY=fyyscreen−cyθx=arctan(tanθx)θy=arctan(tanθy)
在东南大学的开源代码中的位姿解算部分中的单点解算角度部分就是利用这个原理。
if(angle_solver_algorithm == 0)//One Point
{double x1, x2, y1, y2, r2, k1, k2, p1, p2, y_ture;x1 = (centerPoint.x - _cam_instant_matrix.at<double>(0, 2)) / _cam_instant_matrix.at<double>(0, 0);y1 = (centerPoint.y - _cam_instant_matrix.at<double>(1, 2)) / _cam_instant_matrix.at<double>(1, 1);r2 = x1 * x1 + y1 * y1;k1 = _params.DISTORTION_COEFF.at<double>(0, 0);k2 = _params.DISTORTION_COEFF.at<double>(1, 0);p1 = _params.DISTORTION_COEFF.at<double>(2, 0);p2 = _params.DISTORTION_COEFF.at<double>(3, 0);x2 = x1 * (1 + k1 * r2 + k2 * r2*r2) + 2 * p1*x1*y1 + p2 * (r2 + 2 * x1*x1);y2 = y1 * (1 + k1 * r2 + k2 * r2*r2) + 2 * p2*x1*y1 + p1 * (r2 + 2 * y1*y1);y_ture = y2 - _params.Y_DISTANCE_BETWEEN_GUN_AND_CAM / 1000;_xErr = atan(x2) / 2 / CV_PI * 360;_yErr = atan(y_ture) / 2 / CV_PI * 360;if(is_shooting_rune) _yErr -= _rune_compensated_angle;return ONLY_ANGLES;
}
不过这段代码在我测试的时候感觉有问题,它矫正畸变的过程可能有误。
我在实现的时候直接使用OpenCV的矫正函数对点进行矫正,得到的结果是正确的
void calAngle(Mat cam,Mat dis,int x,int y)
{double fx=cam.at<double>(0,0);double fy=cam.at<double>(1,1);double cx=cam.at<double>(0,2);double cy=cam.at<double>(1,2);Point2f pnt;vector<cv::Point2f> in;vector<cv::Point2f> out;in.push_back(Point2f(x,y));//对像素点去畸变undistortPoints(in,out,cam,dis,noArray(),cam);pnt=out.front();//没有去畸变时的比值double rx=(x-cx)/fx;double ry=(y-cy)/fy;//去畸变后的比值double rxNew=(pnt.x-cx)/fx;double ryNew=(pnt.y-cy)/fy;//输出原来点的坐标和去畸变后点的坐标cout<< "x: "<<x<<" xNew:"<<pnt.x<<endl;cout<< "y: "<<y<<" yNew:"<<pnt.y<<endl;//输出未去畸变时测得的角度和去畸变后测得的角度cout<< "angx: "<<atan(rx)/CV_PI*180<<" angleNew:"<<atan(rxNew)/CV_PI*180<<endl;cout<< "angy: "<<atan(ry)/CV_PI*180<<" angleNew:"<<atan(ryNew)/CV_PI*180<<endl;
}
角度测量验证
如何知道我们测出的角度是正确的呢?可以通过一把三角尺来进行测量。
将三角尺的30度尖对准摄像头,不断调整位置,使尖对准cx的像素值位置,之后不断调整摄像头,使三角尺的两条边在图像中平行于y轴,此时就可以选取三角尺边上的像素点来测量角度了,如下图:
拍摄出的图像如图:
这个时候选择左边那条边测得的角度就是30度,选择右边的那条边测得的角度就是0度。
角度测量的代码我放在GitHub上了 https://github.com/hejiangda/angleMeasure
申请了一个自己的公众号江达小记,打算将自己的学习研究的经验总结下来帮助他人也方便自己。感兴趣的朋友可以关注一下。
RoboMaster视觉教程(5)目标位置解算(通过像素点获取转角)相关推荐
- RoboMaster视觉教程(6)目标位置解算(PnP求解目标与摄像头间的相对位置)
RoboMaster视觉教程(6)目标位置解算(PnP求解目标与摄像头间的相对位置) 概览 算法原理 solvePnP的使用流程 实验:测量二维码相对于摄像头的位置 RoboMaster视觉程序中的位 ...
- RoboMaster视觉教程(4)装甲板识别算法
RoboMaster视觉教程(4)装甲板识别算法 概览 下面是一些资料链接,篇篇经典! 装甲板识别 test_sentry.cpp 分析一下装甲板 识别函数 int ArmorDetector::de ...
- RoboMaster视觉教程(9)风车能量机关识别2
RoboMaster视觉教程(9)风车能量机关识别2 之前说能量机关的教程有很多了打算不写了,但是总有同学来问,想了想还是写一下吧. 风车能量机关我只做了识别,因为准备分区赛的时候没有实物可以测试就一 ...
- RoboMaster视觉教程(3)视觉识别程序整体框架
RoboMaster 视觉教程(3)视觉识别程序框架 概览 多线程 除了多线程,还可使用多进程 接下来以东南大学的开源程序为例讲一下他们的整体架构 下面进入正题 项目配置文件概览 ImgProdCon ...
- RoboMaster视觉教程(8)串口通讯
RoboMaster视觉教程(8)串口通讯 概览 DJI开源代码串口部分 东南大学开源代码串口部分 Qt编写串口助手 概览 这几天一直在做一个小车打算做好了再往下写的,但是由于我两年没写stm32的程 ...
- RoboMaster视觉教程(2)妙算(Nvidia Tegra K1)系统配置
RoboMaster视觉教程(2)妙算(Nvidia Tegra K1)系统配置 概览 妙算资料链接汇总 妙算系统重置/克隆/恢复 妙算安装系统后要做的事 妙算通过网线直连电脑并共享电脑网络 妙算远程 ...
- GPS从入门到放弃(七) --- GPS卫星位置解算
GPS从入门到放弃(七) - GPS卫星位置解算 上一篇讲了开普勒轨道参数,根据这些参数就可以确定卫星的位置,这一篇我们来实际计算一下. WGS-84基本参数 首先给出几个WGS-84坐标系中的基本参 ...
- RoboMaster视觉教程(7)风车能量机关识别
RoboMaster视觉教程(7)风车能量机关识别 今年的能量机关在识别的难度上降低了,难在怎么打中.能量机关我只写了识别部分,因为没有道具可以做测试,焊灯条的同学焊的千辛万苦也没焊出可以用的灯条.当 ...
- RoboMaster视觉教程(1)摄像头
观文有感 之 RoboMaster视觉教程(1)摄像头 闲来垂钓碧溪上.今天钓到一篇RM视觉摄像头的好文,记录一下笔记: 文章目录 观文有感 之 RoboMaster视觉教程(1)摄像头 一.摄像头参 ...
最新文章
- 《Xcode实战开发》——2.8节调试区域
- 无线通信 -- 跳频技术
- Spring Ioc注解式开发中注解的作用
- HashiCorp Nomad中的高级节点排干
- android数据持久化存储(2)
- react学习(16)---getFieldDecorator赋值
- 安装open3d python
- 企业微信_新建自建H5小程序应用及主页与菜单设置
- EasyUI--权限管理(二)显示左侧菜单
- 6.Jenkins 权威指南 --- 高级构建
- 网站SEO实践之 - 网站关键词库扩展的几种方法
- ES内存溢出,报错:java.lang.OutOfMemoryError: Java heap space
- html网页抓取建一个网站前端,创建网页的方法以及生成HTML骨架
- 纺织erp有哪些,纺织企业为什么要上ERP
- 告白气球,这个情人节你值得拥有!
- APISpace 中文分词API
- 暨南大学计算机技术复试名单复试,关于2020年暨南大学硕士生复试资格线及复试名单的通知来...
- Gitlab CI/CD教程以及实战docker自动部署前端项目(全)
- C# 如何捕获键盘按钮和组合键以及KeyPress/KeyDown事件之间的区别 (附KeyChar/KeyCode值)
- GB/T 33582-2017机械产品结构有限元力学分析通用规则
热门文章
- idea里面解决jsp,html,xml黄色背景的方法
- 使用u启动为苹果笔记本重装win7系统教程
- echarts 时间曲线图_web前端显示设备实时温度,ECharts实现温度折线图,实时动态温度曲线图生成...
- 3D目标检测框架综述(OpenPCDet、mmdet3d、Det3D、Paddle3D)
- 一份小白前端可视化学习指南——附思维导图
- 【水果识别】柑橘质量检测及分级系统【含GUI Matlab源码 738期】
- 解决克隆虚拟机后无法上网问题(亲测有效)
- 基于PHP+MySQL的大学生求职招聘网站
- Exchange Server 2013 运维系列——EMS实用命令收集(持续更新中...)
- java统计有多少个单词_Java不同单词个数统计