bei1111111

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
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值