#!/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, 0, 222 ],
[ 0, 0, 225 ],
[ 0, 0, 228 ],
[ 0, 0, 231 ],
[ 0, 0, 234 ],
[ 0, 0, 237 ],
[ 0, 0, 240 ],
[ 0, 0, 243 ],
[ 0, 0, 246 ],
[ 0, 0, 249 ],
[ 0, 0, 252 ],
[ 0, 0, 255 ],
[ 0, 3, 252 ],
[ 0, 6, 249 ],
[ 0, 9, 246 ],
[ 0, 12, 243 ],
[ 0, 15, 240 ],
[ 0, 18, 237 ],
[ 0, 21, 234 ],
[ 0, 24, 231 ],
[ 0, 27, 228 ],
[ 0, 30, 225 ],
[ 0, 33, 222 ],
[ 0, 36, 219 ],
[ 0, 39, 216 ],
[ 0, 42, 213 ],
[ 0, 45, 210 ],
[ 0, 48, 207 ],
[ 0, 51, 204 ],
[ 0, 54, 201 ],
[ 0, 57, 198 ],
[ 0, 60, 195 ],
[ 0, 63, 192 ],
[ 0, 66, 189 ],
[ 0, 69, 186 ],
[ 0, 72, 183 ],
[ 0, 75, 180 ],
[ 0, 78, 177 ],
[ 0, 81, 174 ],
[ 0, 84, 171 ],
[ 0, 87, 168 ],
[ 0, 90, 165 ],
[ 0, 93, 162 ],
[ 0, 96, 159 ],
[ 0, 99, 156 ],
[ 0, 102, 153 ],
[ 0, 105, 150 ],
[ 0, 108, 147 ],
[ 0, 111, 144 ],
[ 0, 114, 141 ],
[ 0, 117, 138 ],
[ 0, 120, 135 ],
[ 0, 123, 132 ],
[ 0, 126, 129 ],
[ 0, 129, 126 ],
[ 0, 132, 123 ],
[ 0, 135, 120 ],
[ 0, 138, 117 ],
[ 0, 141, 114 ],
[ 0, 144, 111 ],
[ 0, 147, 108 ],
[ 0, 150, 105 ],
[ 0, 153, 102 ],
[ 0, 156, 99 ],
[ 0, 159, 96 ],
[ 0, 162, 93 ],
[ 0, 165, 90 ],
[ 0, 168, 87 ],
[ 0, 171, 84 ],
[ 0, 174, 81 ],
[ 0, 177, 78 ],
[ 0, 180, 75 ],
[ 0, 183, 72 ],
[ 0, 186, 69 ],
[ 0, 189, 66 ],
[ 0, 192, 63 ],
[ 0, 195, 60 ],
[ 0, 198, 57 ],
[ 0, 201, 54 ],
[ 0, 204, 51 ],
[ 0, 207, 48 ],
[ 0, 210, 45 ],
[ 0, 213, 42 ],
[ 0, 216, 39 ],
[ 0, 219, 36 ],
[ 0, 222, 33 ],
[ 0, 225, 30 ],
[ 0, 228, 27 ],
[ 0, 231, 24 ],
[ 0, 234, 21 ],
[ 0, 237, 18 ],
[ 0, 240, 15 ],
[ 0, 243, 12 ],
[ 0, 246, 9 ],
[ 0, 249, 6 ],
[ 0, 252, 3 ],
[ 0, 255, 0 ],
[ 3, 252, 0 ],
[ 6, 249, 0 ],
[ 9, 246, 0 ],
[ 12, 243, 0 ],
[ 15, 240, 0 ],
[ 18, 237, 0 ],
[ 21, 234, 0 ],
[ 24, 231, 0 ],
[ 27, 228, 0 ],
[ 30, 225, 0 ],
[ 33, 222, 0 ],
[ 36, 219, 0 ],
[ 39, 216, 0 ],
[ 42, 213, 0 ],
[ 45, 210, 0 ],
[ 48, 207, 0 ],
[ 51, 204, 0 ],
[ 54, 201, 0 ],
[ 57, 198, 0 ],
[ 60, 195, 0 ],
[ 63, 192, 0 ],
[ 66, 189, 0 ],
[ 69, 186, 0 ],
[ 72, 183, 0 ],
[ 75, 180, 0 ],
[ 78, 177, 0 ],
[ 81, 174, 0 ],
[ 84, 171, 0 ],
[ 87, 168, 0 ],
[ 90, 165, 0 ],
[ 93, 162, 0 ],
[ 96, 159, 0 ],
[ 99, 156, 0 ],
[ 102, 153, 0 ],
[ 105, 150, 0 ],
[ 108, 147, 0 ],
[ 111, 144, 0 ],
[ 114, 141, 0 ],
[ 117, 138, 0 ],
[ 120, 135, 0 ],
[ 123, 132, 0 ],
[ 126, 129, 0 ],
[ 129, 126, 0 ],
[ 132, 123, 0 ],
[ 135, 120, 0 ],
[ 138, 117, 0 ],
[ 141, 114, 0 ],
[ 144, 111, 0 ],
[ 147, 108, 0 ],
[ 150, 105, 0 ],
[ 153, 102, 0 ],
[ 156, 99, 0 ],
[ 159, 96, 0 ],
[ 162, 93, 0 ],
[ 165, 90, 0 ],
[ 168, 87, 0 ],
[ 171, 84, 0 ],
[ 174, 81, 0 ],
[ 177, 78, 0 ],
[ 180, 75, 0 ],
[ 183, 72, 0 ],
[ 186, 69, 0 ],
[ 189, 66, 0 ],
[ 192, 63, 0 ],
[ 195, 60, 0 ],
[ 198, 57, 0 ],
[ 201, 54, 0 ],
[ 204, 51, 0 ],
[ 207, 48, 0 ],
[ 210, 45, 0 ],
[ 213, 42, 0 ],
[ 216, 39, 0 ],
[ 219, 36, 0 ],
[ 222, 33, 0 ],
[ 225, 30, 0 ],
[ 228, 27, 0 ],
[ 231, 24, 0 ],
[ 234, 21, 0 ],
[ 237, 18, 0 ],
[ 240, 15, 0 ],
[ 243, 12, 0 ],
[ 246, 9, 0 ],
[ 249, 6, 0 ],
[ 252, 3, 0 ],
[ 255, 0, 0 ]]
def main():
global running
running = True
try:
sock1 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock1.bind(SOCK_ADDR)
sock1.listen(5)
while running:
(clientsocket, address) = sock1.accept()
print (" Accept client: ", address)
ct = ClientThread(SocketClientObject(clientsocket, address))
ct.start()
except:
print ("#! EXC: ", sys.exc_info())
sock1.close()
print ("THE END! Goodbye!")
if __name__ == "__main__":
main()
# Configuration file for ReSpeaker USB 4 Mic Array (ReSpeaker USB Mic Array v2.0)
# 配置文件代码
version = "2.1";
# Raw
raw:
{
fS = 16000;
hopSize = 128;
nBits = 16;
nChannels = 6;
# Input with raw signal from microphones
interface: {
type = "soundcard";
card = 1;
device = 0;
}
}
# Mapping
mapping:
{
map: (2, 3, 4, 5);
}
# General
general:
{
epsilon = 1E-20;
size:
{
hopSize = 128;
frameSize = 256;
};
samplerate:
{
mu = 16000;
sigma2 = 0.01;
};
speedofsound:
{
mu = 343.0;
sigma2 = 25.0;
};
mics = (
# Microphone 2
{
mu = ( -0.032, +0.000, +0.000 );
sigma2 = ( +0.000, +0.000, +0.000, +0.000, +0.000, +0.000, +0.000, +0.000, +0.000 );
direction = ( +0.000, +0.000, +1.000 );
angle = ( 80.0, 100.0 );
},
# Microphone 3
{
mu = ( +0.000, -0.032, +0.000 );
sigma2 = ( +0.000, +0.000, +0.000, +0.000, +0.000, +0.000, +0.000, +0.000, +0.000 );
direction = ( +0.000, +0.000, +1.000 );
angle = ( 80.0, 100.0 );
},
# Microphone 4
{
mu = ( +0.032, +0.000, +0.000 );
sigma2 = ( +0.000, +0.000, +0.000, +0.000, +0.000, +0.000, +0.000, +0.000, +0.000 );
direction = ( +0.000, +0.000, +1.000 );
angle = ( 80.0, 100.0 );
},
# Microphone 5
{
mu = ( +0.000, +0.032, +0.000 );
sigma2 = ( +0.000, +0.000, +0.000, +0.000, +0.000, +0.000, +0.000, +0.000, +0.000 );
direction = ( +0.000, +0.000, +1.000 );
angle = ( 80.0, 100.0 );
}
);
# Spatial filter to include only a range of direction if required
# (may be useful to remove false detections from the floor)
spatialfilters = (
{
direction = ( +0.000, +0.000, +1.000 );
angle = (80.0, 100.0);
}
);
nThetas = 181;
gainMin = 0.25;
};
# Stationnary noise estimation
sne:
{
b = 3;
alphaS = 0.1;
L = 150;
delta = 3.0;
alphaD = 0.1;
}
# Sound Source Localization
ssl:
{
nPots = 4;
nMatches = 10;
probMin = 0.5;
nRefinedLevels = 1;
interpRate = 4;
# Number of scans: level is the resolution of the sphere
# and delta is the size of the maximum sliding window
# (delta = -1 means the size is automatically computed)
scans = (
{ level = 2; delta = -1; },
{ level = 4; delta = -1; }
);
# Output to export potential sources
potential: {
# format = "undefined";
format = "json";
interface: {
#type = "blackhole";
type = "socket"; ip = "127.0.0.1"; port = 9000;
#type = "terminal";
};
};
};
# Sound Source Tracking
sst:
{
# Mode is either "kalman" or "particle"
mode = "kalman";
# Add is either "static" or "dynamic"
add = "dynamic";
# Parameters used by both the Kalman and particle filter
active = (
{ weight = 1.0; mu = 0.4; sigma2 = 0.0025 }
);
inactive = (
{ weight = 1.0; mu = 0.25; sigma2 = 0.0025 }
);
sigmaR2_prob = 0.0025;
sigmaR2_active = 0.0225;
sigmaR2_target = 0.0025;
Pfalse = 0.1;
Pnew = 0.1;
Ptrack = 0.8;
theta_new = 0.9;
N_prob = 5;
theta_prob = 0.8;
N_inactive = ( 250, 250, 250, 250 );
theta_inactive = 0.9;
# Parameters used by the Kalman filter only
kalman: {
sigmaQ = 0.001;
};
# Parameters used by the particle filter only
particle: {
nParticles = 1000;
st_alpha = 2.0;
st_beta = 0.04;
st_ratio = 0.5;
ve_alpha = 0.05;
ve_beta = 0.2;
ve_ratio = 0.3;
ac_alpha = 0.5;
ac_beta = 0.2;
ac_ratio = 0.2;
Nmin = 0.7;
};
target: ();
# Output to export tracked sources
tracked: {
format = "json";
interface: {
type = "file"; path = "tracks.txt";
#type = "socket"; ip = "127.0.0.1"; port = 9001;
#type = "terminal";
};
};
}
sss:
{
# Mode is either "dds", "dgss" or "dmvdr"
mode_sep = "dds";
mode_pf = "ms";
gain_sep = 1.0;
gain_pf = 10.0;
dds: {
};
dgss: {
mu = 0.01;
lambda = 0.5;
};
dmvdr: {
};
ms: {
alphaPmin = 0.07;
eta = 0.5;
alphaZ = 0.8;
thetaWin = 0.3;
alphaWin = 0.3;
maxAbsenceProb = 0.9;
Gmin = 0.01;
winSizeLocal = 3;
winSizeGlobal = 23;
winSizeFrame = 256;
};
ss: {
Gmin = 0.01;
Gmid = 0.9;
Gslope = 10.0;
};
separated: {
fS = 16000;
hopSize = 128;
nBits = 16;
interface: {
type = "file";
path = "separated.raw";
};
};
postfiltered: {
fS = 16000;
hopSize = 128;
nBits = 16;
gain = 10.0;
interface: {
type = "file";
path = "postfiltered.raw";
};
};
};
classify:
{
frameSize = 4096;
winSize = 3;
tauMin = 88;
tauMax = 551;
deltaTauMax = 20;
alpha = 0.3;
gamma = 0.05;
phiMin = 0.5;
r0 = 0.2;
category: {
format = "undefined";
interface: {
type = "blackhole";
}
}
}
# usb相机1080p的校正，参考以校正声像仪画面
import cv2 as cv
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)
# 打开摄像头
cap = cv.VideoCapture(0) # 参数0表示第一个摄像头，如果有多个摄像头可以更改参数
# 检查摄像头是否成功打开
if not cap.isOpened():
print("无法打开摄像头。")
exit()
while True:
ret, frame = cap.read() # 读取帧
if ret:
cv.imshow("origin", frame) # 显示实时画面
dst = cv.undistort(frame, matrix, dist, new_camera_matrix)
# x, y, w, h = roi
# dst = dst[y:y + h, x:x + w]
dst = cv.resize(dst, (1920, 1080))
cv.imshow('rectified', dst)
# 按下"q"键退出循环
if cv.waitKey(1) & 0xFF == ord('q'):
break
# 释放资源
cap.release()
cv.destroyAllWindows()
#!/usr/bin/env python
#改进后加入摄像头画面，实现声像融合 只实现了基础的图像映射
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
cmd ='build/bin/odaslive -c config/usb4.cfg'
os.system(cmd)
stop = False
HOST = "0.0.0.0"
PORT = 9000
SOCK_ADDR = (HOST, PORT)
# PORT2 = 9001
# SOCK_ADDR2 = (HOST, PORT2)
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, 0, 222 ],
[ 0, 0, 225 ],
[ 0, 0, 228 ],
[ 0, 0, 231 ],
[ 0, 0, 234 ],
[ 0, 0, 237 ],
[ 0, 0, 240 ],
[ 0, 0, 243 ],
[ 0, 0, 246 ],
[ 0, 0, 249 ],
[ 0, 0, 252 ],
[ 0, 0, 255 ],
[ 0, 3, 252 ],
[ 0, 6, 249 ],
[ 0, 9, 246 ],
[ 0, 12, 243 ],
[ 0, 15, 240 ],
[ 0, 18, 237 ],
[ 0, 21, 234 ],
[ 0, 24, 231 ],
[ 0, 27, 228 ],
[ 0, 30, 225 ],
[ 0, 33, 222 ],
[ 0, 36, 219 ],
[ 0, 39, 216 ],
[ 0, 42, 213 ],
[ 0, 45, 210 ],
[ 0, 48, 207 ],
[ 0, 51, 204 ],
[ 0, 54, 201 ],
[ 0, 57, 198 ],
[ 0, 60, 195 ],
[ 0, 63, 192 ],
[ 0, 66, 189 ],
[ 0, 69, 186 ],
[ 0, 72, 183 ],
[ 0, 75, 180 ],
[ 0, 78, 177 ],
[ 0, 81, 174 ],
[ 0, 84, 171 ],
[ 0, 87, 168 ],
[ 0, 90, 165 ],
[ 0, 93, 162 ],
[ 0, 96, 159 ],
[ 0, 99, 156 ],
[ 0, 102, 153 ],
[ 0, 105, 150 ],
[ 0, 108, 147 ],
[ 0, 111, 144 ],
[ 0, 114, 141 ],
[ 0, 117, 138 ],
[ 0, 120, 135 ],
[ 0, 123, 132 ],
[ 0, 126, 129 ],
[ 0, 129, 126 ],
[ 0, 132, 123 ],
[ 0, 135, 120 ],
[ 0, 138, 117 ],
[ 0, 141, 114 ],
[ 0, 144, 111 ],
[ 0, 147, 108 ],
[ 0, 150, 105 ],
[ 0, 153, 102 ],
[ 0, 156, 99 ],
[ 0, 159, 96 ],
[ 0, 162, 93 ],
[ 0, 165, 90 ],
[ 0, 168, 87 ],
[ 0, 171, 84 ],
[ 0, 174, 81 ],
[ 0, 177, 78 ],
[ 0, 180, 75 ],
[ 0, 183, 72 ],
[ 0, 186, 69 ],
[ 0, 189, 66 ],
[ 0, 192, 63 ],
[ 0, 195, 60 ],
[ 0, 198, 57 ],
[ 0, 201, 54 ],
[ 0, 204, 51 ],
[ 0, 207, 48 ],
[ 0, 210, 45 ],
[ 0, 213, 42 ],
[ 0, 216, 39 ],
[ 0, 219, 36 ],
[ 0, 222, 33 ],
[ 0, 225, 30 ],
[ 0, 228, 27 ],
[ 0, 231, 24 ],
[ 0, 234, 21 ],
[ 0, 237, 18 ],
[ 0, 240, 15 ],
[ 0, 243, 12 ],
[ 0, 246, 9 ],
[ 0, 249, 6 ],
[ 0, 252, 3 ],
[ 0, 255, 0 ],
[ 3, 252, 0 ],
[ 6, 249, 0 ],
[ 9, 246, 0 ],
[ 12, 243, 0 ],
[ 15, 240, 0 ],
[ 18, 237, 0 ],
[ 21, 234, 0 ],
[ 24, 231, 0 ],
[ 27, 228, 0 ],
[ 30, 225, 0 ],
[ 33, 222, 0 ],
[ 36, 219, 0 ],
[ 39, 216, 0 ],
[ 42, 213, 0 ],
[ 45, 210, 0 ],
[ 48, 207, 0 ],
[ 51, 204, 0 ],
[ 54, 201, 0 ],
[ 57, 198, 0 ],
[ 60, 195, 0 ],
[ 63, 192, 0 ],
[ 66, 189, 0 ],
[ 69, 186, 0 ],
[ 72, 183, 0 ],
[ 75, 180, 0 ],
[ 78, 177, 0 ],
[ 81, 174, 0 ],
[ 84, 171, 0 ],
[ 87, 168, 0 ],
[ 90, 165, 0 ],
[ 93, 162, 0 ],
[ 96, 159, 0 ],
[ 99, 156, 0 ],
[ 102, 153, 0 ],
[ 105, 150, 0 ],
[ 108, 147, 0 ],
[ 111, 144, 0 ],
[ 114, 141, 0 ],
[ 117, 138, 0 ],
[ 120, 135, 0 ],
[ 123, 132, 0 ],
[ 126, 129, 0 ],
[ 129, 126, 0 ],
[ 132, 123, 0 ],
[ 135, 120, 0 ],
[ 138, 117, 0 ],
[ 141, 114, 0 ],
[ 144, 111, 0 ],
[ 147, 108, 0 ],
[ 150, 105, 0 ],
[ 153, 102, 0 ],
[ 156, 99, 0 ],
[ 159, 96, 0 ],
[ 162, 93, 0 ],
[ 165, 90, 0 ],
[ 168, 87, 0 ],
[ 171, 84, 0 ],
[ 174, 81, 0 ],
[ 177, 78, 0 ],
[ 180, 75, 0 ],
[ 183, 72, 0 ],
[ 186, 69, 0 ],
[ 189, 66, 0 ],
[ 192, 63, 0 ],
[ 195, 60, 0 ],
[ 198, 57, 0 ],
[ 201, 54, 0 ],
[ 204, 51, 0 ],
[ 207, 48, 0 ],
[ 210, 45, 0 ],
[ 213, 42, 0 ],
[ 216, 39, 0 ],
[ 219, 36, 0 ],
[ 222, 33, 0 ],
[ 225, 30, 0 ],
[ 228, 27, 0 ],
[ 231, 24, 0 ],
[ 234, 21, 0 ],
[ 237, 18, 0 ],
[ 240, 15, 0 ],
[ 243, 12, 0 ],
[ 246, 9, 0 ],
[ 249, 6, 0 ],
[ 252, 3, 0 ],
[ 255, 0, 0 ]]
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()
# 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), 30, (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()
# class VideoThread(threading.Thread):
# def __init__(self,dest_object):
# threading.Thread.__init__(self)
# self.dest_object=dest_object
#
# def run(self):
# global running
# while running == True:
# #img = np.zeros((800,800,3),np.uint8)
# data = self.dest_object.socket.recv(1024)
# data = data.decode("utf-8")
# data = data.replace("\n", "")
# try:
# src = (data.split('[')[1]).split(']')[0]
# items = src.split(", ")
# for item in items:
# target = json.loads(item)
# x = int(float(target["x"]) * 400) + 400
# y = int(-float(target["y"]) * 400) + 400
# activity = int(float(target["activity"]) * 255)
# #if (activity > 100):
# # cv2.circle(img, (x, y), 30, (0,255,0), -1)
#
# #cv2.imshow('pu2', img)
# #if cv2.waitKey(1) & 0xFF == ord('q'):
# #break
#
# except:
# print ("problem2")
#
# #cv2.destroyAllWindows()
# self.dest_object.socket.close()
def main():
global running
running = True
try:
sock1 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock1.bind(SOCK_ADDR)
sock1.listen(5)
# sock2 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# sock2.bind(SOCK_ADDR2)
# sock2.listen(2)
while running:
(clientsocket, address) = sock1.accept()
print (" Accept client: ", address)
ct = ClientThread(SocketClientObject(clientsocket, address))
ct.start()
# (dst,dst_addr) = sock2.accept()
# print ("Destination Connected by", dst_addr)
# vt = VideoThread(SocketClientObject(dst,dst_addr))
# vt.start()
except:
print ("#! EXC: ", sys.exc_info())
sock1.close()
# sock2.close()
print ("THE END! Goodbye!")
if __name__ == "__main__":
main()
248
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
