声像仪（实心圆版）

#!/usr/bin/env python
#改进后加入摄像头画面，实现声像融合，校正声像仪画面，不加热力图的完整版，配合usb4.cfg使用
import math 
import socket
import sys
import threading 
import random
import os 
import time
import struct
import cv2 
import signal 
import json 
import ast 
import numpy as np

matrix = np.array([[1150.4796065932892, 0.0, 866.7906072916296],
                   [0.0, 1154.5867399421018, 498.44391507650516],
                   [0.0, 0.0, 1.0]], dtype=float)





new_camera_matrix = np.array([[561.1795654296875, 0.0, 854.4371857272781],
                             [0.0, 523.6474609375, 371.79368114244426],
                             [0.0, 0.0, 1.0]], dtype=float)



dist = np.array([[-0.4210920702510668, 0.2214943232573482, 0.0012200103644413377, 0.001434741235348292, -0.06668565568366154]], dtype=float)

cmd ='build/bin/odaslive -c config/usb4.cfg' 
os.system(cmd)

stop = False
HOST = "0.0.0.0" 
PORT = 9000 
SOCK_ADDR = (HOST, PORT)



class SocketClientObject(object): 
    def __init__(self, socket, address ):
        self.socket = socket
        self.address = address
 
 
 
class ClientThread(threading.Thread):
    def __init__(self, client_object):
        threading.Thread.__init__(self)
        self.client_object = client_object


    def run(self):
        global running
        while running == True:
            cap = cv2.VideoCapture(0)
            cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
            cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)
 
            while True:
                ret, frame = cap.read()
                frame = cv2.undistort(frame, matrix, dist, new_camera_matrix)
                frame = cv2.resize(frame, (1920, 1080))
                # cv2.imshow('fff', frame)
                # img = np.zeros((800,800,3),np.uint8)
                data = self.client_object.socket.recv(20480)
                data = data.decode("utf-8")
                data = data.replace("\n", "")
                try:
                    src = (data.split('[')[1]).split(']')[0]
                    items = src.split(",        ")
                    target = json.loads(items[0])
                    # print(f'data={data}')
                    # print(f'target={target}')
                    # energy = int(float(target["E"]) * 255)
                    #if (energy > 80):
                    x = float(target["x"])
                    y = -float(target["y"])
                    z = float(target["z"])
                    # print(f'x={x}，y={y},z={z}')
                    camera_focal_length = 806        # 以像素为单位计算的虚拟焦距
                    az = math.atan2(y, x)                                    # 方位角 以x轴正方向为起始边逆时针旋转的角度
                    inc = math.acos(z)    # 俯仰角 与z轴的夹角
                    # print(f'{math.sqrt(x * x + y * y + z*z)}')
                    print(f'方位角={az}，俯仰角={math.degrees(inc)}')
                    s = int(camera_focal_length * np.tan(inc))  # 计算目标映射位置偏离中心距离 单位为像素
                    m = int(s * np.cos(az))  # 目标映射位置在x轴上偏离中心的距离
                    n = int(s * np.sin(az))  # 目标映射位置在y轴上偏离中心的距离
                    # print(f'x轴上偏离中心的距离={m}，y轴上偏离中心的距离={n}')
                    center_x = 960 + m                  # 目标映射位置的x轴坐标
                    center_y = 540 - n                  # 目标映射位置的y轴坐标
                    energy = int(float(target["E"]) * 255)
                    if (energy > 80):
                        cv2.circle(frame,  (center_x, center_y),  60,  (spectrum_rgb3_lut[255- energy][0], spectrum_rgb3_lut[255- energy][1], spectrum_rgb3_lut[255- energy][2]), -1)
                    cv2.imshow('pu', frame)
                    if cv2.waitKey(1) & 0xFF == ord('q'):
                        break
                except:
                    print ("problem1")
                if cv2.waitKey(1) & 0xFF == ord('q'):
                    break
            cap.release()
            cv2.destroyAllWindows()
        cv2.destroyAllWindows()
        self.client_object.socket.close()



def stop_handler(signum, frame):
    global running
    running = False

signal.signal(signal.SIGINT, stop_handler) 


spectrum_rgb3_lut = [ 
	[   0,   0,   0 ], 
	[   0,   0,   3 ], 
	[   0,   0,   6 ], 
	[   0,   0,   9 ], 
	[   0,   0,  12 ], 
	[   0,   0,  15 ],
	[   0,   0,  18 ],
	[   0,   0,  21 ],
	[   0,   0,  24 ], 
	[   0,   0,  27 ],
	[   0,   0,  30 ], 
	[   0,   0,  33 ],
	[   0,   0,  36 ],
	[   0,   0,  39 ],
	[   0,   0,  42 ],
	[   0,   0,  45 ],
	[   0,   0,  48 ],
	[   0,   0,  51 ],
	[   0,   0,  54 ],
	[   0,   0,  57 ],
	[   0,   0,  60 ],
	[   0,   0,  63 ],
	[   0,   0,  66 ],
	[   0,   0,  69 ],
	[   0,   0,  72 ],
	[   0,   0,  75 ],
	[   0,   0,  78 ],
	[   0,   0,  81 ],
	[   0,   0,  84 ],
	[   0,   0,  87 ],
	[   0,   0,  90 ],
	[   0,   0,  93 ],
	[   0,   0,  96 ],
	[   0,   0,  99 ],
	[   0,   0, 102 ],
	[   0,   0, 105 ],
	[   0,   0, 108 ],
	[   0,   0, 111 ],
	[   0,   0, 114 ],
	[   0,   0, 117 ],
	[   0,   0, 120 ],
	[   0,   0, 123 ],
	[   0,   0, 126 ],
	[   0,   0, 129 ],
	[   0,   0, 132 ],
	[   0,   0, 135 ],
	[   0,   0, 138 ],
	[   0,   0, 141 ],
	[   0,   0, 144 ],
	[   0,   0, 147 ],
	[   0,   0, 150 ],
	[   0,   0, 153 ],
	[   0,   0, 156 ],
	[   0,   0, 159 ],
	[   0,   0, 162 ],
	[   0,   0, 165 ],
	[   0,   0, 168 ],
	[   0,   0, 171 ],
	[   0,   0, 174 ],
	[   0,   0, 177 ],
	[   0,   0, 180 ],
	[   0,   0, 183 ],
	[   0,   0, 186 ],
	[   0,   0, 189 ],
	[   0,   0, 192 ], 
	[   0,   0, 195 ], 
	[   0,   0, 198 ], 
	[   0,   0, 201 ],
	[   0,   0, 204 ],
	[   0,   0, 207 ],
	[   0,   0, 210 ],
	[   0,   0, 213 ],
	[   0,   0, 216 ],
	[   0,   0, 219 ],
	[   0,