3D定位

简述

3D定位是指通过设置图像目标坐标，旋转球机PTZ，使目标在球机视野中心；
球机坐标系有三个轴分别控制pan(水平) tilt(垂直) zoom(倍率）；

3D定位原理分析

3D定位原理见下图（C是镜头中心，O是图像中心点，P是目标点在图上位置，A是图像边界点）

由上图可得

tan(∠ACO)tan(∠PCO)=AOPO\frac{tan(∠ACO)}{tan(∠PCO)} = \frac{AO}{PO} tan(∠PCO)tan(∠ACO)=POAO

可得图像上的目标点P到图像视野中心的水平和垂直角度差
{αV:1−>o=arctan(Y−H/2H/2∗tan(FOVv/2))αH:1−>o=arctan(X−W/2W/2∗tan(FOVh/2))\begin{cases} \alpha _{V:1->o}=arctan(\frac{Y-H/2}{H/2}*tan(FOV_v/2)) \\ \alpha_{H:1->o}=arctan(\frac{X-W/2}{W/2}*tan(FOV_h/2)) \end{cases} {αV:1−>o=arctan(H/2Y−H/2∗tan(FOVv/2))αH:1−>o=arctan(W/2X−W/2∗tan(FOVh/2))
通过水平和垂直视野角度差，转动云台指定角度即可。

海康SDK实现

目前在海康球机看到有相关功能实现，点击3D，然后在图上选择想定位的目标点。
海康球机有相关SDK实现，SDK下载
相关实现函数
- pStruPointFrame图像上的点归一化到255x255的坐标系(具体原因可以参考海康专利《一种利用快球跟踪系统实现监控的方法及装置》)
- NET_DVR_PTZSelZoomIn和NET_DVR_PTZSelZoomIn_EX功能一致，实现略微不同
- SDK调用可以查看《设备网络SDK使用手册》

NET_DVR_PTZSelZoomIn(LONG lRealHandle, LPNET_DVR_POINT_FRAME pStruPointFrame);
NET_DVR_PTZSelZoomIn_EX(LONG lUserID, LONG lChannel, LPNET_DVR_POINT_FRAME pStruPointFrame);

strcpy(cam_ip, "192.168.1.x");
cam_port = 8000;
strcpy(user_name, "admin");
strcpy(pwd, "xxxxx");NET_DVR_DEVICEINFO_V30 DeviceInfoTmp;
memset(&DeviceInfoTmp, 0, sizeof(NET_DVR_DEVICEINFO_V30));LONG lLoginID = NET_DVR_Login_V30(cam_ip, cam_port, user_name, pwd, &DeviceInfoTmp); // 注册int std_cols = 2560;   // 图像长宽
int std_rows = 1440;
NET_DVR_POINT_FRAME posdata;posdata.xTop = (int)(target_x * 255 / std_cols);   // 坐标归一化到(255, 255)
posdata.xBottom = posdata.xTop;
posdata.yTop = (int)(target_y * 255 / std_rows);
posdata.yBottom = posdata.yTop;
posdata.bCounter = 3;   // 没用
if (!NET_DVR_PTZSelZoomIn_EX(0, 1, &posdata)){
printf("3D定位失败, %d\n", NET_DVR_GetLastError());return -1;
}

ONVIF实现

既然原理已知，考虑以下，研究一下3D定位通用方法，
- 海康SDK有Linux和Windows的实现方式，但是在arm上没找到相关SDK？
- 如果有别的品牌球机，没有海康SDK，需要如何实现？
- 如果不是球机，只有一个云台和一个摄像头又要如何实现？
ONVIF协议作为标准通用的公有协议，可以实现摄像头PTZ控制
需要获取球机的PTZ和视场角与ONVIF的PTZ对应关系，参考《球机的PTZ和视场角与ONVIF的PTZ对应关系》
以海康球机DS-2DC22041W-D3/W为例

球机的PTZ与ONVIF的PTZ的对应关系

球机的PTZ与onvif的ptz是线性关系
ONVIF与SDK的PTZ对应关系

ONVIF_P	ONVIF_T	ONVIF_Z	SDK_P	SDK_T	SDK_Z
-1	-1	0	0	0	1
0	0	0.5	180	45	2.5
1	1	1	350	90	4

拟合线性关系

{p′=min(180+180∗p,360)t′=45+45∗tz′=1+3∗z\begin{cases} p' = min(180 + 180 * p, 360)\\ t'= 45 + 45*t \\ z'= 1 + 3*z\\ \end{cases} ⎩⎪⎨⎪⎧p′=min(180+180∗p,360)t′=45+45∗tz′=1+3∗z

球机的视场角与ONVIF的Zoom的对应关系

此版本不支持SDK直接读取视场角值
参照《视场角相关计算》，通过相机标定，获取像素焦距

{fx=2807.89fy=2804.10\begin{cases} f_x=2807.89\\ f_y=2804.10 \end{cases} {fx=2807.89fy=2804.10

计算出水平和垂直视场角

{FOVH=37.75FOVV=21.80\begin{cases} FOV_H = 37.75\\ FOV_V=21.80 \end{cases} {FOVH=37.75FOVV=21.80

3D 定位

主要分为ONVIF控制、3D控制和UI交互三部分
app.py负责UI交互

def onMouse(event, x, y, flags, param):if event == cv2.EVENT_LBUTTONDBLCLK:# print(x, y, flags, param)coor = (x,y)param.put(coor)if param.qsize() > 1:param.get()else:time.sleep(0.01)def show_image(wnd_name, img, delay=0):cv2.namedWindow(wnd_name, cv2.WINDOW_NORMAL)cv2.resizeWindow(wnd_name, 1280, 720)cv2.imshow(wnd_name, img)ret = cv2.waitKey(delay)return retdef img_put(queue, status):wnd_name = "frame"cap = cv2.VideoCapture(rtsp)if not cap.isOpened():print('error open cam')returnnum = 0while True:num += 1frame = cv2.flip(cap.read()[1], 0)status.value = show_image(wnd_name, frame, 1)if status.value == ord('q'):print("image put exit!")returncv2.setMouseCallback(wnd_name, onMouse, param=queue)def img_get(queue, status):my_onvif = onvif_control(ip, usr, pwd)my_locate = locate(Size[0], Size[1], FOV[0], FOV[1])num = 0while True:num += 1if status.value == ord('q'):print("image get exit!")returnif queue.qsize() > 0:coor = queue.get()pos_cur = my_onvif.get_status()pos_target = my_locate.locate(coor, pos_cur)my_onvif.abs_move(pos_target)def app():multiprocessing.set_start_method(method='spawn')queue = multiprocessing.Queue(maxsize=5)processes = []status = Value('i', -1)processes.append(multiprocessing.Process(target=img_put, args=(queue, status)))processes.append(multiprocessing.Process(target=img_get, args=(queue, status)))for process in processes:process.daemon = Trueprocess.start()for process in processes:process.join()print("Done")

onvif_control.py负责ONVIF控制

class onvif_control:def __init__(self, ip, usr, pwd, hom_pos=None):self.usr = usrself.pwd = pwdself.hom_pos = hom_posself.cam = ONVIFCamera(ip, 80, usr, pwd)self.ptz = self.cam.create_ptz_service()self.media = self.cam.create_media_service()self.imaging = self.cam.create_imaging_service()self.profile = self.cam.media.GetProfiles()[0]def get_status(self):request = self.cam.ptz.create_type('GetStatus')request.ProfileToken = self.profile.tokenstatus = self.cam.ptz.GetStatus(request)return {'pan': status.Position.PanTilt.x, 'tilt': status.Position.PanTilt.y, 'zoom': status.Position.Zoom.x}def move_home(self, speed={'pan':1, 'tilt':1, 'zoom':1}):request = self.ptz.create_type('AbsoluteMove')request.ProfileToken = self.profile.tokenrequest.Position = {'PanTilt':{'x':self.hom_pos['pan'], 'y':self.hom_pos['tilt']}, 'Zoom':self.hom_pos['zoom']}request.Speed = {'PanTilt':{'x':speed['pan'], 'y':speed['tilt']}, 'Zoom':speed['zoom']}self.ptz.AbsoluteMove(request)def abs_move(self, pos, speed={'pan':1, 'tilt':1, 'zoom':1}):request = self.ptz.create_type('AbsoluteMove')request.ProfileToken = self.profile.tokenrequest.Position = {'PanTilt':{'x':pos['pan'], 'y':pos['tilt']}, 'Zoom':pos['zoom']}request.Speed = {'PanTilt':{'x':speed['pan'], 'y':speed['tilt']}, 'Zoom':speed['zoom']}self.ptz.AbsoluteMove(request)def continuous_move(self, speed):request = self.ptz.create_type("ContinuousMove")request.ProfileToken =  self.profile.tokenrequest.Velocity = {'PanTilt':{'x':speed['pan'], 'y':speed['tilt']}, 'Zoom':speed['zoom']}self.ptz.ContinuousMove(request)def relative_move(self, pos, speed={'pan':1, 'tilt':1, 'zoom':1}):request = self.cam.ptz.create_type('RelativeMove')request.ProfileToken = self.profile.tokenrequest.Translation = {'PanTilt':{'x':pos['pan'], 'y':pos['tilt']}, 'Zoom':pos['zoom']}request.Speed = {'PanTilt':{'x':speed['pan'], 'y':speed['tilt']}, 'Zoom':speed['zoom']}self.ptz.RelativeMove(request)def stop(self):self.ptz.Stop({'ProfileToken': self.profile.token})def snap_image(self, path):import requests, osres = self.media.GetSnapshotUri({'ProfileToken': self.profile.token})auth = requests.auth.HTTPDigestAuth(self.usr, self.pwd)response = requests.get(url=res.Uri, auth=auth)path_out = os.path.join(path, 'tmp.jpg')with open(path_out, 'wb') as fp:fp.write(response.content)def get_rtsp(self):obj = self.media.create_type('GetStreamUri')obj.StreamSetup = {'Stream': 'RTP-Unicast', 'Transport': {'Protocol': 'RTSP'}}obj.ProfileToken = self.profile.tokenres_uri = self.media.GetStreamUri(obj)['Uri']return res_uri

locate_3d.py负责3D定位计算

class locate:def __init__(self, width, height, fov_h, fov_v):self.width = widthself.height = heightself.fov_h = fov_hself.fov_v = fov_vdef ptz_to_sdk(self, pos_onvif):p_ = min(180 * pos_onvif['pan'] + 180, 360)t_ = 45 * pos_onvif['tilt'] + 45z_ = 3 * pos_onvif['zoom'] + 1return {'pan' :p_, 'tilt' :t_, 'zoom' :z_}def ptz_to_onvif(self, pos_sdk):p_ = (pos_sdk['pan'] - 180) / 180t_ = (pos_sdk['tilt'] - 45) / 45z_ = (pos_sdk['zoom'] - 1) / 3return {'pan' :p_, 'tilt' :t_, 'zoom' :z_}def locate(self, target, pos_onvif):# 图像目标点，移动到视野中心# pos转到sdkpos_sdk = self.ptz_to_sdk(pos_onvif)print("[pos_sdk]:", pos_sdk)x, y = target# 水平方向if x > (self.width / 2):delt_x = np.rad2deg(   np.arctan((x - self.width / 2) / (self.width / 2) * np.tan( np.deg2rad(self.fov_h /2)))    )else:delt_x = -np.rad2deg(  np.arctan((self.width / 2 - x) / (self.width / 2) * np.tan( np.deg2rad(self.fov_h /2)))    )# 垂直方向if y > (self.height / 2):delt_y = np.rad2deg(   np.arctan((y - self.height / 2) / (self.height / 2) * np.tan( np.deg2rad(self.fov_v / 2)))  )else:delt_y = -np.rad2deg(  np.arctan((self.height / 2 - y) / (self.height / 2) * np.tan( np.deg2rad(self.fov_v / 2)))  )# print("delt:[%.3f, %.3f]" % (delt_x, delt_y))pos_sdk['pan'] += delt_x        # 得到转换后的ptpos_sdk['tilt'] -= delt_y       # 相机是倒置放置的。。。pos_onvif = self.ptz_to_onvif(pos_sdk)        # pos转到onvifreturn pos_onvif

参考链接

一种利用快球跟踪系统实现监控的方法及装置
海康球机控制函数VC (PTZ控制+对准具体坐标点)
海康球机SDK下载

《球机3D定位解析及ONVIF实现》相关推荐

ComeFuture英伽学院——2020年全国大学生英语竞赛【C类初赛真题解析】(持续更新)
视频:ComeFuture英伽学院--2019年全国大学生英语竞赛[C类初赛真题解析]大小作文--详细解析课件:[课件]2019年大学生英语竞赛C类初赛.pdf 视频:2020年全国大学生英语竞赛 ...
ComeFuture英伽学院——2019年全国大学生英语竞赛【C类初赛真题解析】大小作文——详细解析
视频:ComeFuture英伽学院--2019年全国大学生英语竞赛[C类初赛真题解析]大小作文--详细解析课件:[课件]2019年大学生英语竞赛C类初赛.pdf 视频:2020年全国大学生英语竞赛 ...
信息学奥赛真题解析（玩具谜题）
玩具谜题(2016年信息学奥赛提高组真题) 题目描述小南有一套可爱的玩具小人, 它们各有不同的职业.有一天, 这些玩具小人把小南的眼镜藏了起来.小南发现玩具小人们围成了一个圈,它们有的面朝圈内,有的 ...
信息学奥赛之初赛第1轮讲解（01-08课）
信息学奥赛之初赛讲解 01 计算机概述系统基本结构信息学奥赛之初赛讲解 01 计算机概述系统基本结构_哔哩哔哩_bilibili 信息学奥赛之初赛讲解 02 软件系统计算机语言进制转换信息 ...
信息学奥赛一本通习题答案（五）
最近在给小学生做C++的入门培训,用的教程是信息学奥赛一本通,刷题网址 http://ybt.ssoier.cn:8088/index.php 现将部分习题的答案放在博客上,希望能给其他有需要的人带来 ...
信息学奥赛一本通习题答案（三）
最近在给小学生做C++的入门培训,用的教程是信息学奥赛一本通,刷题网址 http://ybt.ssoier.cn:8088/index.php 现将部分习题的答案放在博客上,希望能给其他有需要的人带来 ...
信息学奥赛一本通提高篇第六部分数学基础相关的真题
第1章快速幂 1875:[13NOIP提高组]转圈游戏信息学奥赛一本通(C++版)在线评测系统第2 章素数第 3 章约数第 4 章同余问题第 5 章矩阵乘法第 6 章 ...
信息学奥赛一本通题目代码（非题库）
为了完善自己学c++,很多人都去读相关文献,就比如<信息学奥赛一本通>,可又对题目无从下手,从今天开始,我将把书上的题目一一的解析下来,可以做参考,如果有错,可以告诉我,将在下次解析里重 ...
信息学奥赛一本通（C++版）刷题记录
总目录详见:https://blog.csdn.net/mrcrack/article/details/86501716 信息学奥赛一本通(C++版) 刷题记录 http://ybt.ssoier. ...
最近公共祖先三种算法详解 + 模板题建议新手收藏例题：信息学奥赛一本通祖孙询问距离
首先什么是最近公共祖先?? 如图:红色节点的祖先为红色的1, 2, 3. 绿色节点的祖先为绿色的1, 2, 3, 4. 他们的最近公共祖先即他们最先相交的地方,如在上图中黄色的点就是他们的最近公共祖先 ...

《球机3D定位解析及ONVIF实现》

文章目录