脚步追踪备份

from sklearn.cluster import DBSCAN
from KF import KalmanFilter
from fuc.PeiZhun import *
import open3d as o3d
from fuc.getThreePoints import get

np.warnings.filterwarnings('ignore', category=np.VisibleDeprecationWarning)

lastr = []
lastl = []
lf = 0


def isLeft(lMean, llMean, lrMean):
    if ((lMean[0] - llMean[0]) ** 2 + (lMean[1] - llMean[1]) ** 2) < (
            (lMean[0] - lrMean[0]) ** 2 + (lMean[1] - lrMean[1]) ** 2):
        return True
    else:
        return False


def leftRight(pointList, s1, s2):
    global lastl
    global lastr
    global lf
    change = False
    leftFootList = []
    rightFootList = []
    f0 = open(s1, "w+")
    f1 = open(s2, "w+")
    n = 0
    db = DBSCAN(eps=5, min_samples=7)
    db.fit(pointList)
    # 模型拟合与聚类预测
    # 检索唯一群集
    label = db.labels_
    ll = len(leftFootList)
    lr = len(rightFootList)
    leftFootList = np.array(leftFootList).reshape(ll, 3)
    rightFootList = np.array(rightFootList).reshape(lr, 3)
    if ll == 0:
        lMean = np.array([0, 0, 0])
    else:
        lMean = leftFootList.mean(axis=0)
    if lr == 0:
        rMean = np.array([0, 0, 0])
    else:
        rMean = rightFootList.mean(axis=0)
    if ll != 0 and lr != 0 and lf == 0:
        lastl = lMean
        lastr = rMean
        lf = 1
    if ll != 0:
        if isLeft(lMean, lastl, lastr) and lf == 1:
            # 这次的左脚就是上次的左脚
            if lr != 0:
                lastl = lMean
                lastr = rMean
        else:
            change = True
            if lr != 0:
                lastl = rMean
                lastr = lMean
    if change:
        for point in pointList:
            s = str(point[0]) + " " + str(point[1]) + " " + str(0) + "\n"
            if label[n] == 1:
                leftFootList.append(point)
                f0.write(s)
            if label[n] == 0:
                rightFootList.append(point)
                f1.write(s)
            n = n + 1
    return lMean, rMean


def remove_mapPoint(cloud):
    cl, ind = cloud.remove_radius_outlier(nb_points=2, 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


if __name__ == "__main__":
    # 旋转矩阵
    tp = [[-9.77053866e-01, 2.12992352e-01, 0.00000000e+00, 1.56157731e+02],
          [-2.12992352e-01, -9.77053866e-01, 0.00000000e+00, 2.22208412e+02],
          [0.00000000e+00, 0.00000000e+00, 1.00000000e+00, 0.00000000e+00],
          [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]

    T = np.eye(4)
    ifChange = False
    iftxt = True
    n = 1
    s1 = '../data/smapp.txt'
    s2 = '../data/sfoott.txt'
    s3 = '../data/cloud1.txt'
    s4 = '../data/cloud2.txt'
    s5 = '../data/cloud3.txt'
    x1 = '../data/xmapp.txt'
    x2 = '../data/xfoott.txt'
    x3 = '../data/xcloud1.txt'
    x4 = '../data/xcloud2.txt'
    x5 = '../data/xcloud3.txt'
    fl = open('../data/leftFootCloud.txt', "w+")
    fr = open('../data/rightFootCloud.txt', "w+")
    fxr = open('../data/xleftFootCloud.txt', 'w+')
    fxl = open('../data/xrightFootCloud.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')
    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])  # 点云着色
    kfLeft = KalmanFilter.KalmanFilter()
    kfRight = KalmanFilter.KalmanFilter()
    # 初始化
    kfLeft.initModel()
    kfRight.initModel()
    # 预测
    kfLeft.predict()
    kfRight.predict()
    f = 0
    # 动态展示
    while True:
        tl0 = kfLeft.predict()
        tr0 = kfRight.predict()
        vis.remove_geometry(leftFootCloud)
        vis.remove_geometry(rightFootCloud)
        vis.remove_geometry(xleftFootCloud)
        vis.remove_geometry(xrightFootCloud)
        vis.remove_geometry(mapCloud)
        get_footPoints(s2, s4, n)
        get_footPoints(x2, x4, n)
        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)
        # 当前左右脚的中心点
        lMean, rMean = leftRight(footArray, '../data/leftFootCloud.txt', '../data/rightFootCloud.txt')
        # 两个脚都有
        xlMean, xrMean = leftRight(xfootArray, '../data/xleftFootCloud.txt', '../data/xrightFootCloud.txt')
        if f == 0:
            f = 1
            llMean = np.array(lMean)
            rrMean = np.array(rMean)
            xllMean = np.array(xlMean)
            xrrMean = np.array(xrMean)
        else:
            if isLeft(lMean, llMean, rrMean):
                # 当前左脚集合就是左脚
                # 把当前左脚的代表点集合送入左脚的卡尔曼滤波进行预测
                xMin, xMax, yMin, yMax, xA, yA = get('../data/leftFootCloud.txt')
                tl = kfLeft.correct(np.array([xA, yA]).reshape(2, 1))
                lMean = [tl[0], tl[2]]
                xMin, xMax, yMin, yMax, xA, yA = get('../data/rightFootCloud.txt')
                tr = kfRight.correct(np.array([xA, yA]).reshape(2, 1))
                rMean = [tr[0], tr[2]]
            else:
                # 当前左脚集合是右脚
                # 把当前左脚集合的代表点送入右脚的卡尔曼滤波进行预测
                xMin, xMax, yMin, yMax, xA, yA = get('../data/leftFootCloud.txt')
                tr = kfRight.correct(np.array([xA, yA]).reshape(2, 1))
                rMean = [tr[0], tr[2]]
                xMin, xMax, yMin, yMax, xA, yA = get('../data/rightFootCloud.txt')
                tl = kfLeft.correct(np.array([xA, yA]).reshape(2, 1))
                lMean = [tl[0], tl[2]]
                t = lMean
                lMean = rMean
                rMean = t
        llMean = np.array(lMean)
        rrMean = np.array(rMean)

        if iftxt:
            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')
        xleftFootCloud.transform(tp)
        xrightFootCloud = o3d.io.read_point_cloud('../data/xrightFootCloud.txt', format='xyz')
        xrightFootCloud.transform(tp)
        leftFootCloud.paint_uniform_color([1, 0, 0])
        rightFootCloud.paint_uniform_color([1, 0, 0])
        xleftFootCloud.paint_uniform_color([0, 0, 0])
        xrightFootCloud.paint_uniform_color([0, 0, 0])
        f0.write(str(np.array(leftFootCloud.points).shape[0]) + "\n")
        f1.write(str(np.array(rightFootCloud.points).shape[0]) + "\n")
        vis.add_geometry(xleftFootCloud)
        vis.add_geometry(xrightFootCloud)
        vis.add_geometry(leftFootCloud)
        vis.add_geometry(rightFootCloud)
        vis.add_geometry(mapCloud)
        vis.poll_events()  # 注册更新操作
        vis.update_renderer()  # 渲染图窗
        n = n + 1
        iftxt = True
    vis.run()
    vis.destroy_window()  # 关闭显示窗口