robomaster视觉教程,robomaster基础知识
Robmaster视觉教程(6)物体位置求解(PnP求解物体与摄像机的相对位置)概述算法原理使用solvePnP的过程实验：测量二维码相对于Robmaster视觉程序中摄像机位置求解扩展的位置
概述
上一篇文章提到，图像中某一点相对于相机的角度可以通过针孔成像原理得到，利用针孔成像原理可以测量出需要的角度。
但是这种方法有很大的局限性。只能拿到相对于摄像头中心的转角。但在实际应用中，摄像机肯定不会在云台的轴上，每次相对于摄像机中心的角度指令云台都会有误差。
如果摄像头放在枪管上，这个误差问题不大。一般采用闭环控制算法控制云台跟踪目标，这样如果目标静止，摄像机的中心点正对稳态目标，枪管略低。可以根据实际情况调整摄像头的中心位置，使枪管对准目标。
如果摄像头放在车里，那么这种测角的方法就不能用了。上篇方法最大的缺陷是缺失了深度信息，不能改变坐标系。这里还有一个算法求解目标位置。两者之间的坐标通过求解Perspective N-Point (PNP) Problem得到。
与上面的方法相比，这种方法可以得到三维坐标。缺点是需要四个点来计算，而上面的算法只需要一个点，所以目标距离太远适合用单点解算角度，目标距离合适。
算法原理关于算法原理和一些用例的解释，请参考以下文章：
「使用OpenCV和Dlib进行头部姿态估计」https://www . learnopencv . com/Head-Pose-Estimation-using-OpenCV-and-Dlib/
「纹理物体的实时姿态估计」https://docs . opencv . org/3 . 4 . 6/DC/d2c/tutorial _ Real _ Time _ pose . html
“《视觉SLAM十四讲》研究笔记-3D-https://blog.csdn.net/luohuiwu/article/details/80722542 2D:PNP问题的起源”
要使用这种算法，首先要对摄像机进行标定，得到摄像机的内部参考矩阵和畸变参数。之后需要PnP解算角度才能得到物体在世界坐标系中的坐标。
这个坐标是测量物体的尺寸。由于装甲板可视为平板，其Z轴坐标可设为0，X轴和Y轴坐标可设为长度和宽度的正负二分之一，使装甲板的中心为原点。OpenCV中的坐标系见下图：
(来源：“OpenCV文档”https://docs.opencv.org/3.4.6/d9/d0c/group__calib3d.html)
检测程序检测到装甲板后，可以得到装甲板在图像中的坐标，然后通过solvePnP函数可以得到平移向量和旋转向量。
旋转矢量可以忽略，因为我们只需要得到位置信息而不是姿态信息。当当前相机中心是原点时，获得的平移向量是对象原点的位置。
因为我们的目的是把枪管对准装甲板的中心，而得到的坐标是以摄像头的中心为基准的，所以需要在这个坐标上做自定义的，把原点平移到平移才能得到正确的角。
一般来说，X坐标是不需要移动的，因为相机一般放在桶的上方，这样X方向就不会有偏差。y和z方向需要校正。校正后，可以通过反三角函数得到角度值。
实验：测量二维码相对于摄像头的位置。为什么用二维码代替钢板？因为没有没有比赛可以用的装甲板。有了二维码，就可以直接使用现成的库来完成识别，而不用自己编写识别函数。
要识别二维码，请参考“使用ZBar和OpenCV的条形码和二维码扫描仪”3359 www . lear OpenCV . com/barcode-and-QR-code-scanner-using-ZBar-and-OpenCV/using ZBar library。如果使用的是OpenCV的较新版本，也可以使用OpenCV自带的二维码识别功能。
参考代码：https://github . com/SPMAllick/LearnOpenCV/Tree/Master/Barcode-QR Code Scanner
我只是添加了代码来读取相机的内部参考，并用solvePnP获取二维码的位置。
其中，求解位置只需要以下几行：
#定义HALF_LENGTH 29.5//mmvectorpoint 3f obj=矢量点3f {cv: point3f (-half _ length，-half _ length，0)，//tl cv: point3f (half _ length，-half _ length，0)，//tr cv:Point3f(HALF_LENGTH，half _ length，0)，//br cv:Point3f(-HALF_LENGTH，HALF _ LENGTH，0)//bl }；cv:Mat rVec=cv:Mat:zeros(3，1，CV _ 64fc 1)；//init RVE ccv:Mat tVec=CV:Mat:zeros(3，1，CV _ 64fc 1)；//init tvec//Solve求解PNP的位置(obj，pnts，cam，dis，RVEC，tvec，false，Solve PNP _ iterative)；//输出翻译向量cout tvec: tVecendl；HALF_LENGTH是我手机上显示的二维码长度的一半。点坐标的定义是从左上角开始顺时针定义。求解PnP后得到的平移向量由cout输出。
这是我的实验环境。摄像头与手机平面平行(当然还是有点歪)。手机夹着一本书，可以在卡槽内移动改变位置。
将手机放在右侧，如下图所示：
此时，在摄像头拍摄的图片中，可以得到二维码中心的坐标为(53.04，43.74，266.77)。
将手机放在左侧，如下图所示：
此时，在摄像头拍摄的画面中，可以得到二维码中心的坐标为(-113.49，39.58，266.07)。
可以看到二维码的Z坐标基本不变，而X坐标变化较大。Y坐标按照理想情况应该是不变的，但是因为摄像头还有点歪，所以稍微变了一下。
接下来用直尺测量摄像头与二维码的距离，看看我们测量的摄像头与二维码平面的距离是否正确：
靠近点看。
可以看出，26.6cm左右刚好接近相机镜头的中心，说明测量结果是准确的。
我把测试代码放在GitHub:https://github.com/hejiangda/pnpTest上
RoboMaster可视化程序中的定位解在东南大学开源代码中，定位解部分位于Pose文件夹中。
在AngleSolver.cpp文件中，定义了大小装甲板的世界坐标。这里需要注意的是，通过识别功能得到的装甲板四个点的坐标是云台的轴的坐标，也就是靠近装甲板中间部分的两个光条的坐标。因此，在定义装甲板的世界坐标时，应以中间部分的长度和宽度为准，否则算法将无法正常工作，测量的数据也将是错误的。
由于不同摄像机识别的钢板位置和预处理参数不同，需要根据实际情况确定钢板的世界坐标。我的做法是每隔一个地砖(一块地砖的长宽都是1.15m)在这个距离上检查程序测得的装甲板的距离。如果不同距离的钢板测量距离误差很小，则采用这套世界坐标，否则进行调整。
另外，我觉得函数void angle solver:set resolution(constcv:size 2 I image _ resolution)有问题。好像是根据图像的分辨率修改了相机的内参，改变了内参中中心点的位置和焦距。那样改肯定有问题。它的中心点是不按比例变化的，也就是除非用19201080的分辨率，否则中心点会偏很多。实际使用中，我屏蔽了几句改它内参的话。在不同分辨率下再校准一次就好了，而且反正coup只能运行640480分辨率的图像，在这个分辨率下校准内参就行了。
相机内部参数和失真系数放在angle_solver_params.xml文件中，您可以按原样添加您的测量参数。
偏移和重力补偿可以参考无效角度解算器：补偿偏移()和无效角度解算器：补偿重力()。
此外，在RoboMaster官方开源的RoboRTS代码中还有重力补偿部分可供参考。
https://github . com/robo master/RoboRTS/blob/ROS/RoboRTS _ detection/armor _ detection/gimbal _ control . CPP
除了pnp，相似三角形原理也可以用来扩展单目距离，可以参考《使用python和opencv查找相机到物体/标记的距离》3359 www . pyimagesearch . com/2015/01/19/find-distance-camera-object marker-using-python-opencv/。但相似三角形只适用于被测物体与相机平行的情况，有很大的局限性。
双目测距的精度会更好，因为我的水平和精力有限，没有做过研究。有兴趣的话可以看看大连交通大学的开源代码“装甲板中间图案”https://github.com/Ponkux/RM2018-DJTU-VisionOpenSource。
我申请了自己的微信官方账号RM2018-DJTU-VisionOpenSource，打算总结一下学习研究的经验，帮助别人，方便自己。感兴趣的朋友可以关注一下。
郑重声明：本文由网友发布，不代表盛行IT的观点，版权归原作者所有，仅为传播更多信息之目的，如有侵权请联系，我们将第一时间修改或删除，多谢。