import time
from math import sqrt
from sklearn.cluster import DBSCAN
from KF import KalmanFilter
import numpy as np
import open3d as o3d
from pynput import keyboard
import threading
from fuc.getPoints import *
from fuc.track import *
np.warnings.filterwarnings('ignore', category=np.VisibleDeprecationWarning)
k1 = -1.0787464
b1 = 164.38382
k2 = -0.97042704
b2 = 173.88885
kk1 = -0.5019615
bb1 = 159.25883
kk2 = -0.54667634
bb2 = 179.50114
xl = 0
xr = 0
sl = 0
sr = 0
xlShow = False
xrShow = False
srShow = False
slShow = False
leftc = 0
rightc = 0
p = 1
def get_length(ll, lr, l, r):
# 左脚位置发生了变化
# if (ll[0] - l[0]) * (ll[0] - l[0]) + (ll[1] - l[1]) * (ll[1] - l[1]) > 10000:
# print("左脚位置发生了变化", (ll[0] - l[0]) * (ll[0] - l[0]) + (ll[1] - l[1]) * (ll[1] - l[1]))
if (lr[0] - r[0])*(lr[0] - r[0])+(lr[1] - r[1])*(lr[1] - r[1])>1000:
print("右脚位置发生了变化",(lr[0] - r[0])*(lr[0] - r[0])+(lr[1] - r[1])*(lr[1] - r[1]))
def if_null():
la = 0 # 浅蓝色脚质心
ra = 0 # 蓝色脚质心
global xl, xr, sl, sr, xlShow, xrShow, srShow, slShow, leftc, rightc
if xl == 0:
if xr == 1:
if sr == 1:
srShow = False
if sl == 1:
slShow = False
leftc = 0
if xr == 2:
if sr == 2:
srShow = False
if sl == 2:
slShow = False
rightc = 0
if xr == 0:
if xl == 1:
if sr == 1:
srShow = False
if sl == 1:
slShow = False
leftc = 0
if xl == 2:
if sr == 2:
srShow = False
if sl == 2:
slShow = False
rightc = 0
if sl == 0:
if sr == 1:
if xr == 1:
xrShow = False
if xl == 1:
xlShow = False
rightc = 0
if sr == 2:
if xr == 2:
xrShow = False
if xl == 2:
xlShow = False
leftc = 0
if sr == 0:
if sl == 1:
if xr == 1:
xrShow = False
if xl == 1:
xlShow = False
rightc = 0
if sl == 2:
if xr == 2:
xrShow = False
if xl == 2:
xlShow = False
leftc = 0
return la, ra
def leftRight(pointList, s1, s2):
# 点云聚类，从非地图点里区分人脚
f0 = open(s1, "w+")
f1 = open(s2, "w+")
leftFootList = []
rightFootList = []
n = 0
db = DBSCAN(eps=5, min_samples=7)
db.fit(pointList)
label = db.labels_
for point in pointList:
s = str(point[0]) + " " + str(point[1]) + " " + str(0) + "\n"
if label[n] == 0:
leftFootList.append(point)
f0.write(s)
if label[n] == 1:
rightFootList.append(point)
f1.write(s)
n = n + 1
def remove_mapPoint(cloud):
# 去除地图点云中的离群点
cl, ind = cloud.remove_radius_outlier(nb_points=3, radius=6)
inlier_source = cloud.select_by_index(ind)
return inlier_source
def remove_point(cloud):
# 去除脚步点云中的离群点
cl, ind = cloud.remove_radius_outlier(nb_points=4, radius=20)
inlier_source = cloud.select_by_index(ind)
# print(inlier_source)
return inlier_source
def fliter(mapTxt, footTxt, newFootTxt):
# 地图点点云，脚步点点云，要写的点云文件
# 分离包含脚步点云的点云中的脚步
n = 0
f0 = open(newFootTxt, "w+")
fMap = open(mapTxt, "r")
fFoot = open(footTxt, "r")
mapLine = fMap.readlines()
footLine = fFoot.readlines()
sumLine = len(footLine)
while n < sumLine:
mapPoint = mapLine[n].split(' ')
footPoint = footLine[n].split(' ')
mapX = float(mapPoint[0])
mapY = float(mapPoint[1])
footX = float(footPoint[0])
footY = float(footPoint[1])
# 去除0,0点
if footX == 0 and footY == 0:
n = n + 1
continue
if ((mapX - footX) ** 2 + (mapY - footY) ** 2) >= 100:
# 判断为脚步点
s = footPoint[0] + " " + footPoint[1] + " " + str(0) + "\n"
f0.write(s)
n = n + 1
def disxLR(pcd):
points = np.array(pcd.points)
if points.shape[0] == 0:
return 0
[x, y, z] = np.mean(points, axis=0)
d1 = round(abs(k1 * x - y + b1) / np.sqrt(k1 ** 2 + 1), 2)
d2 = round(abs(k2 * x - y + b2) / np.sqrt(k2 ** 2 + 1), 2)
if d1 < d2:
return 1
else:
return 2
def dissLR(pcd):
points = np.array(pcd.points)
if points.shape[0] == 0:
return 0
[x, y, z] = np.mean(points, axis=0)
d1 = round(abs(kk1 * x - y + bb1) / np.sqrt(kk1 ** 2 + 1), 2)
d2 = round(abs(kk2 * x - y + bb2) / np.sqrt(kk2 ** 2 + 1), 2)
if d1 < d2:
return 1
else:
return 2
def on_press(key):
global p
try:
if key.char == 'p':
p = -p
except AttributeError:
print('special key {0} pressed'.format(key))
def run():
while (True):
with keyboard.Listener(on_press=on_press) as listener:
listener.join()
if __name__ == "__main__":
t = threading.Thread(target=run)
t.setDaemon(True)
t.start()
# 旋转矩阵 ICP配准后获得
tp = [[-0.97054084, 0.24093666, 0., 142.35725171],
[-0.24093666, -0.97054084, 0., 232.05485009],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]
T = np.eye(4)
ifChange = False
iftxt = True
n = 1
s1 = '../data/smapp.txt' # S雷达测得的地图
s2 = '../data/sfoott.txt' # S雷达测得的包含脚步的地图
s3 = '../data/cloud1.txt' # S雷达地图txt点云
s4 = '../data/cloud2.txt' # S雷达包含脚步的地图txt点云
s5 = '../data/cloud3.txt' # S雷达分离出的人脚txt点云
x1 = '../data/xmapp.txt' # X雷达同S
x2 = '../data/xfoott.txt'
x3 = '../data/xcloud1.txt'
x4 = '../data/xcloud2.txt'
x5 = '../data/xcloud3.txt'
fc = open('../data/focusCloud.txt', 'w+')
f0 = open('../data/dara.txt', 'w+')
f1 = open('../data/data.txt', 'w+')
# 以s雷达地图为基准 x雷达地图进行配准 获取选旋转矩阵后旋转x雷达中的脚步 画到s地图中
get_mapPoints(s1, s3)
get_mapPoints(x1, x3)
vis = o3d.visualization.Visualizer()
vis.create_window(window_name="脚步分离",
width=1700, height=900,
left=50, top=50)
mapCloud = o3d.io.read_point_cloud(s3, format='xyz')
focusCloud = o3d.io.read_point_cloud('../data/focusCloud.txt', format='xyz')
mapCloud = remove_mapPoint(mapCloud)
xleftFootCloud = o3d.io.read_point_cloud('../data/xleftFootCloud.txt', format='xyz')
xrightFootCloud = o3d.io.read_point_cloud('../data/xrightFootCloud.txt', format='xyz')
leftFootCloud = o3d.io.read_point_cloud('../data/leftFootCloud.txt', format='xyz')
rightFootCloud = o3d.io.read_point_cloud('../data/rightFootCloud.txt', format='xyz')
# mapCloud.paint_uniform_color([0, 1, 0]) # 点云着色
newCloud = o3d.io.read_point_cloud('../data/xleftFootCloud.txt', format='xyz')
xkfLeft = KalmanFilter.KalmanFilter()
xkfRight = KalmanFilter.KalmanFilter()
skfLeft = KalmanFilter.KalmanFilter()
skfRight = KalmanFilter.KalmanFilter()
# 初始化
xkfLeft.initModel()
xkfRight.initModel()
skfLeft.initModel()
skfRight.initModel()
# 动态展示
llav = [0, 0]
lrav = [0, 0]
nL = 0
while True:
nL = nL + 1
# 左右脚重心
leftAv = [0, 0]
rightAv = [0, 0]
if p == -1:
vis.run()
while (True):
print(p)
if p == 1:
vis.destroy_window()
vis = o3d.visualization.Visualizer()
vis.create_window(window_name="脚步分离",
width=1700, height=900,
left=50, top=50)
break
slShow = True
srShow = True
xlShow = True
xrShow = True
leftc = 0
rightc = 0
change = False
vis.remove_geometry(focusCloud)
vis.remove_geometry(mapCloud)
vis.remove_geometry(newCloud)
vis.remove_geometry(xleftFootCloud)
vis.remove_geometry(xrightFootCloud)
vis.remove_geometry(leftFootCloud)
vis.remove_geometry(rightFootCloud)
newCloud = o3d.io.read_point_cloud('../data/smappoints.txt', format='xyz')
focusCloud = o3d.io.read_point_cloud('../data/focusCloud.txt', format='xyz')
focusCloud.paint_uniform_color([0, 1, 0])
fla = get_footPoints(s2, s4, n)
if fla == 1:
break
fla = get_footPoints(x2, x4, n)
if fla == 1:
break
fliter(s3, s4, s5)
fliter(x3, x4, x5)
footCloud = o3d.io.read_point_cloud(s5, format='xyz')
xfootCloud = o3d.io.read_point_cloud(x5, format='xyz')
footArray = np.array(footCloud.points)
xfootArray = np.array(xfootCloud.points)
leftRight(footArray, '../data/leftFootCloud.txt', '../data/rightFootCloud.txt')
leftRight(xfootArray, '../data/xleftFootCloud.txt', '../data/xrightFootCloud.txt')
leftFootCloud = o3d.io.read_point_cloud('../data/leftFootCloud.txt', format='xyz')
rightFootCloud = o3d.io.read_point_cloud('../data/rightFootCloud.txt', format='xyz')
xleftFootCloud = o3d.io.read_point_cloud('../data/xleftFootCloud.txt', format='xyz')
xrightFootCloud = o3d.io.read_point_cloud('../data/xrightFootCloud.txt', format='xyz')
# 区分左右脚，并修正卡尔曼滤波--------------------------------------------------------
xl = disxLR(xleftFootCloud)
xr = disxLR(xrightFootCloud)
xleftFootCloud.transform(tp)
xrightFootCloud.transform(tp)
# 1:right
# 2:left
if xl == 1:
rightc = rightc + np.array(xleftFootCloud.points).shape[0]
xleftFootCloud.paint_uniform_color([0, 0, 1])
xkfRight.predict()
xkfRight.correct(np.mean(np.array(xleftFootCloud.points)[:, :2], axis=0).reshape(2, 1))
rightAv = np.mean(np.array(xleftFootCloud.points)[:, :2], axis=0)
if xl == 2:
leftc = leftc + np.array(xleftFootCloud.points).shape[0]
xleftFootCloud.paint_uniform_color([0, 1, 1])
xkfLeft.predict()
xkfLeft.correct(np.mean(np.array(xleftFootCloud.points)[:, :2], axis=0).reshape(2, 1))
leftAv = np.mean(np.array(xleftFootCloud.points)[:, :2], axis=0)
if xr == 1:
rightc = rightc + np.array(xrightFootCloud.points).shape[0]
xrightFootCloud.paint_uniform_color([0, 0, 1])
xkfRight.predict()
xkfRight.correct(np.mean(np.array(xrightFootCloud.points)[:, :2], axis=0).reshape(2, 1))
rightAv = np.mean(np.array(xrightFootCloud.points)[:, :2], axis=0)
if xr == 2:
leftc = leftc + np.array(xrightFootCloud.points).shape[0]
xrightFootCloud.paint_uniform_color([0, 1, 1])
xkfLeft.predict()
xkfLeft.correct(np.mean(np.array(xrightFootCloud.points)[:, :2], axis=0).reshape(2, 1))
leftAv = np.mean(np.array(xrightFootCloud.points)[:, :2], axis=0)
sl = dissLR(leftFootCloud)
sr = dissLR(rightFootCloud)
# 1:left
# 2:right
if sl == 1:
leftc = leftc + np.array(leftFootCloud.points).shape[0]
leftFootCloud.paint_uniform_color([0, 1, 1])
skfLeft.predict()
skfLeft.correct(np.mean(np.array(leftFootCloud.points)[:, :2], axis=0).reshape(2, 1))
leftAv = (leftAv + np.mean(np.array(leftFootCloud.points)[:, :2], axis=0)) / 2
if sl == 2:
rightc = rightc + np.array(leftFootCloud.points).shape[0]
leftFootCloud.paint_uniform_color([0, 0, 1])
skfRight.predict()
skfRight.correct(np.mean(np.array(leftFootCloud.points)[:, :2], axis=0).reshape(2, 1))
rightAv = (rightAv + np.mean(np.array(leftFootCloud.points)[:, :2], axis=0)) / 2
if sr == 1:
leftc = leftc + np.array(rightFootCloud.points).shape[0]
rightFootCloud.paint_uniform_color([0, 1, 1])
skfLeft.predict()
skfLeft.correct(np.mean(np.array(rightFootCloud.points)[:, :2], axis=0).reshape(2, 1))
leftAv = (leftAv + np.mean(np.array(rightFootCloud.points)[:, :2], axis=0)) / 2
if sr == 2:
rightc = rightc + np.array(rightFootCloud.points).shape[0]
rightFootCloud.paint_uniform_color([0, 0, 1])
skfRight.predict()
skfRight.correct(np.mean(np.array(rightFootCloud.points)[:, :2], axis=0).reshape(2, 1))
rightAv = (rightAv + np.mean(np.array(rightFootCloud.points)[:, :2], axis=0)) / 2
# -----------------------------------------------------------
# 有缺失的情况
# x雷达left丢了
if (xl == 0 and xr == 1) or (xr == 0 and xl == 1):
if sr == 1:
xkfLeft.predict()
txl = xkfLeft.correct(np.mean(np.array(rightFootCloud.points)[:, :2], axis=0).reshape(2, 1))
rmean = np.mean(np.array(rightFootCloud.points)[:, :2], axis=0).reshape(2, 1)
t = [txl[0] - rmean[0], txl[2] - rmean[1], 0]
T[:3, 3] = t
newCloud = copy.deepcopy(rightFootCloud).transform(T)
if sl == 1:
xkfLeft.predict()
txl = xkfLeft.correct(np.mean(np.array(leftFootCloud.points)[:, :2], axis=0).reshape(2, 1))
lmean = np.mean(np.array(leftFootCloud.points)[:, :2], axis=0).reshape(2, 1)
t = [txl[0] - lmean[0], txl[2] - lmean[1], 0]
T[:3, 3] = t
newCloud = copy.deepcopy(leftFootCloud).transform(T)
# x雷达right丢了
if (xl == 0 and xr == 2) or (xr == 0 and xl == 2):
if sr == 2:
xkfRight.predict()
txr = xkfRight.correct(np.mean(np.array(rightFootCloud.points)[:, :2], axis=0).reshape(2, 1))
rmean = np.mean(np.array(rightFootCloud.points)[:, :2], axis=0).reshape(2, 1)
t = [txr[0] - rmean[0], txr[2] - rmean[1], 0]
T[:3, 3] = t
newCloud = copy.deepcopy(rightFootCloud).transform(T)
if sl == 2:
xkfRight.predict()
txr = xkfRight.correct(np.mean(np.array(leftFootCloud.points)[:, :2], axis=0).reshape(2, 1))
lmean = np.mean(np.array(leftFootCloud.points)[:, :2], axis=0).reshape(2, 1)
t = [txr[0] - lmean[0], txr[2] - lmean[1], 0]
T[:3, 3] = t
newCloud = copy.deepcopy(leftFootCloud).transform(T)
# s雷达left丢了
if (sl == 0 and sr == 2) or (sr == 0 and sl == 2):
if xl == 2:
skfLeft.predict()
tsl = skfLeft.correct(np.mean(np.array(xleftFootCloud.points)[:, :2], axis=0).reshape(2, 1))
xlmean = np.mean(np.array(xleftFootCloud.points)[:, :2], axis=0).reshape(2, 1)
t = [tsl[0] - xlmean[0], tsl[2] - xlmean[1], 0]
T[:3, 3] = t
newCloud = copy.deepcopy(xleftFootCloud).transform(T)
if xr == 2:
skfLeft.predict()
tsl = skfLeft.correct(np.mean(np.array(xrightFootCloud.points)[:, :2], axis=0).reshape(2, 1))
xrmean = np.mean(np.array(xrightFootCloud.points)[:, :2], axis=0).reshape(2, 1)
t = [tsl[0] - xrmean[0], tsl[2] - xrmean[1], 0]
T[:3, 3] = t
newCloud = copy.deepcopy(xrightFootCloud).transform(T)
# s雷达right丢了
if (sl == 0 and sr == 1) or (sr == 0 and sl == 1):
if xl == 1:
skfRight.predict()
tsr = skfRight.correct(np.mean(np.array(xleftFootCloud.points)[:, :2], axis=0).reshape(2, 1))
xlmean = np.mean(np.array(xleftFootCloud.points)[:, :2], axis=0).reshape(2, 1)
t = [tsr[0] - xlmean[0], tsr[2] - xlmean[1], 0]
T[:3, 3] = t
newCloud = copy.deepcopy(xleftFootCloud).transform(T)
if xr == 1:
skfRight.predict()
tsr = skfRight.correct(np.mean(np.array(xrightFootCloud.points)[:, :2], axis=0).reshape(2, 1))
xrmean = np.mean(np.array(xrightFootCloud.points)[:, :2], axis=0).reshape(2, 1)
t = [tsr[0] - xrmean[0], tsr[2] - xrmean[1], 0]
T[:3, 3] = t
newCloud = copy.deepcopy(xrightFootCloud).transform(T)
# 过滤小点
if_null()
# 计算特征
if leftAv[0] != 0 and rightAv[0] != 0:
focus = (leftAv + rightAv) / 2
elif leftAv[0] == 0:
focus = rightAv
else:
focus = leftAv
s = str(focus[0]) + " " + str(focus[1]) + " " + str(1) + "\n"
fc.write(s)
# print("the center of feet:", leftAv, rightAv)
# print("the focus:", focus)
# print("-----------------------------------------------------")
# if nL == 4:
# if leftAv[0] == 0:
# llav = llav
# if rightAv[0] == 0:
# lrav = lrav
# else:
# get_length(llav, lrav, leftAv, rightAv)
# llav = leftAv
# lrav = rightAv
# nL = 0
newCloud.paint_uniform_color([1, 0, 0])
f0.write(str(leftc))
f0.write('\n')
f1.write(str(rightc))
f1.write('\n')
get_length(llav, lrav, leftAv, rightAv)
# print(xlShow)
if xlShow and slShow:
# 有左脚)
vis.add_geometry(xleftFootCloud)
vis.add_geometry(leftFootCloud)
llav = leftAv
if xrShow and srShow:
# 有右脚
vis.add_geometry(xrightFootCloud)
vis.add_geometry(rightFootCloud)
lrav = rightAv
else:
print(11111)
# vis.add_geometry(newCloud)
vis.add_geometry(focusCloud)
vis.add_geometry(mapCloud)
vis.poll_events() # 注册更新操作
vis.update_renderer() # 渲染图窗
n = n + 1
iftxt = True
vis.destroy_window() # 关闭显示窗口
0
点赞
收藏
分享
bei1111111
相关推荐
0 条评论
