雷达脚步跟踪备份

整体思路

提取雷达地图点数据，绘制地图。读取包含地图点和脚步点的雷达数据，做脚步提取，脚步区分，脚步跟踪。

地图点提取

读取所有帧，当前角度出现最多次数的点为地图点

def clear(filename):
    lidar_all = []
    n = 0
    while n < 768:
        i = 1
        lidar_lists = []
        with open(filename, "r")as f:
            lines = f.readlines()
            while i < len(lines):
                s = lines[i][13:]
                s_new = s.split(',')
                lidar_lists.append(int(round(float(s_new[n]), 2) * 100))
                i = i + 2
        lidar_lists = np.array(lidar_lists)
        lidar_all.append(np.argmax(np.bincount(lidar_lists)))
        n = n + 1
    return lidar_all

确定地图点的角度和距离后，转换为x，y坐标，用于绘制点云

def get_mapPoints(s1, s2):
    with open(s2, "w+")as f0:
        lidar_all = clearData.clear(s1)
        angle = 0.0
        angle_increment = 0.006135923
        for r in lidar_all:
            x = math.trunc(r * math.cos(angle + (-90.0 * 3.1416 / 180.0)))
            y = math.trunc(r * math.sin(angle + (-90.0 * 3.1416 / 180.0)))
            angle = angle + angle_increment
            s = str(x) + " " + str(y) + " " + str(0) + "\n"
            f0.write(s)
        return 0

去除地图离群点

def remove_mapPoint(cloud):
    cl, ind = cloud.remove_radius_outlier(nb_points=2, radius=14)
    inlier_source = cloud.select_by_index(ind)
    print(inlier_source)
    return inlier_source

脚步点提取

由于脚步点不用考虑所有帧出现次数最多的点，所以需要再写一个提取雷达数据的函数

def get_footPoints(s1, s2, n):
    f1 = open(s1, "r+")
    with open(s2, "w+")as f0:
        lines = f1.readlines()
        angle = 0.0
        angle_increment = 0.006135923
        s = lines[2*n-1][13:]
        s_new = s.split(',')
        for r in s_new:
            if "\n" in r:
                continue
            r = float(r)
            x = math.trunc((float(r) * 100.0) * math.cos(angle + (-90.0 * 3.1416 / 180.0)))
            y = math.trunc((float(r) * 100.0) * math.sin(angle + (-90.0 * 3.1416 / 180.0)))
            angle = angle + angle_increment
            s = str(x) + " " + str(y) + " " + str(1) + "\n"
            f0.write(s)
        return 0

提取到包含地图点和脚步点的雷达数据后，再进行脚步点的提取

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:
        # print(mapLine[n])
        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 leftRight(pointList):
    print(pointList)
    leftFootList = []
    rightFootList = []
    f0 = open('../data/leftFootCloud.txt', "w+")
    f1 = open('../data/rightFootCloud.txt', "w+")
    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 and label[n] != 1:
        #     print(label[n])
        if label[n] == 0:
            leftFootList.append(point)
            f0.write(s)
        if label[n] == 1:
            rightFootList.append(point)
            f1.write(s)
        n = n + 1
    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)
    return lMean, rMean

移除脚步离群点

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

脚步跟踪

利用卡尔曼滤波进行脚步跟踪

import numpy as np


class KalmanFilter(object):
    # stateVariance 状态方差
    # measurementVariance 测量方差
    def __init__(self, dt=1 / 30, stateVariance=1, measurementVariance=0.1,
                 method="Accerelation"):
        super(KalmanFilter, self).__init__()
        self.method = method
        self.stateVariance = stateVariance
        self.measurementVariance = measurementVariance
        self.dt = dt
        self.initModel()

    """init function to initialise the model"""

    def initModel(self):
        # [x,x_v,y,y_v]x,x方向的速度，y，y方向的速度
        # 有加速度的情况 A，B都有用
        if self.method == "Accerelation":
            # 加速度值
            self.U = 1
        # 匀速直线运动的时候，B没有用
        else:
            self.U = 1
        # 状态转移矩阵
        self.A = np.matrix([[1, self.dt, 0, 0], [0, 1, 0, 0],
                            [0, 0, 1, self.dt], [0, 0, 0, 1]])
        # 控制增益
        self.B = np.matrix([[self.dt ** 2 / 2], [self.dt], [self.dt ** 2 / 2],
                            [self.dt]])
        # 量测矩阵
        self.H = np.matrix([[1, 0, 0, 0], [0, 0, 1, 0]])
        # 初始预测协方差矩阵
        self.P = np.matrix(self.stateVariance * np.identity(self.A.shape[0]))  # np.identity:输出对角线为1，其余为0的n*n矩阵
        # 测量噪声协方差
        self.R = np.matrix(self.measurementVariance * np.identity(  # 2*2
            self.H.shape[0]))
        # 过程激励噪声协方差（系统噪声）
        self.Q = np.matrix([[self.dt ** 4 / 4, self.dt ** 3 / 2, 0, 0],
                            [self.dt ** 3 / 2, self.dt ** 2, 0, 0],
                            [0, 0, self.dt ** 4 / 4, self.dt ** 3 / 2],
                            [0, 0, self.dt ** 3 / 2, self.dt ** 2]])
        # print(self.A.shape)

        # 初始值设置
        self.erroCov = self.P
        self.state = np.array([[100], [3], [0], [3]])

    """Predict function which predicst next state based on previous state"""

    def predict(self):
        # 第一步，第二步，估计当前状态和当前协方差
        self.predictedState = self.A * self.state + self.U * self.B  # U为控制量，因为是匀速直线运动，所以U为0，U和B都没有用
        self.predictedErrorCov = self.A * self.erroCov * self.A.T + self.Q
        temp = np.asarray(self.predictedState)
        return temp

    """Correct function which correct the states based on measurements"""

    def correct(self, currentMeasurement):
        # 三四五步，计算卡尔曼增益，预测最优当前状态和当前协方差
        self.kalmanGain = self.predictedErrorCov * self.H.T * np.linalg.pinv(
            self.H * self.predictedErrorCov * self.H.T + self.R)
        self.state = self.predictedState + self.kalmanGain * (currentMeasurement
                                                              - (self.H * self.predictedState))

        self.erroCov = (np.identity(self.P.shape[0]) -
                        self.kalmanGain * self.H) * self.predictedErrorCov
        temp = np.asarray(self.state)
        return temp

完整代码

import time

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)


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):
    print(pointList)
    leftFootList = []
    rightFootList = []
    f0 = open('../data/leftFootCloud.txt', "w+")
    f1 = open('../data/rightFootCloud.txt', "w+")
    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 and label[n] != 1:
        #     print(label[n])
        if label[n] == 0:
            leftFootList.append(point)
            f0.write(s)
        if label[n] == 1:
            rightFootList.append(point)
            f1.write(s)
        n = n + 1
    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)
    return lMean, rMean


def remove_mapPoint(cloud):
    cl, ind = cloud.remove_radius_outlier(nb_points=2, radius=14)
    inlier_source = cloud.select_by_index(ind)
    print(inlier_source)
    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:
        # print(mapLine[n])
        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__":
    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'
    fl = open('../data/leftFootCloud.txt', "w+")
    fr = open('../data/rightFootCloud.txt', "w+")
    get_mapPoints(s1, s3)
    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)
    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()
        # print('预测', t0)
        vis.remove_geometry(leftFootCloud)
        vis.remove_geometry(rightFootCloud)
        vis.remove_geometry(mapCloud)
        # vis.remove_geometry(footCloud)
        get_footPoints(s2, s4, n)
        fliter(s3, s4, s5)
        footCloud = o3d.io.read_point_cloud(s5, format='xyz')
        footCloud = remove_point(footCloud)
        footArray = np.array(footCloud.points)
        # 当前左右脚的中心点
        lMean, rMean = leftRight(footArray)
        # print("左：", tl0[0],tl0[1], lMean)
        # print("右：", tr0[0],tl0[1], rMean)
        if f == 0:
            f = 1
            llMean = np.array(lMean)
            rrMean = np.array(rMean)
        # 脚缺失
        if rMean[0] == 0:
            # print("有缺失")
            # 两个脚的点云都用预测的值
            # tr = kfRight.correct((np.array([rrMean[0], rrMean[1]]).reshape(2, 1)))
            rMean = np.array([tr0[0], tr0[2], 0])
            # rMean = rrMean
            # tl = kfLeft.correct((np.array([llMean[0], llMean[1]]).reshape(2, 1)))
            lMean = np.array([tl0[0], tl0[2], 0])
            # print(rMean,lMean)
            # lMean = llMean
            t = [tr0[0] - rrMean[0], tr0[2] - rrMean[1], 0]
            # print(t)
            T[:3, 3] = t
            rightFootCloud = copy.deepcopy(rightFootCloud).transform(T)
            t = [tl0[0] - llMean[0], tl0[2] - llMean[1], 0]
            # print(t)
            T[:3, 3] = t
            leftFootCloud = copy.deepcopy(leftFootCloud).transform(T)
            iftxt = False
            # print("左预测的：", tr0[0], tr0[2], "上一个：", rrMean)
            # print("右预测的：", tl0[4], tl0[5], "上一个：", llMean)
            # 右脚点集缺失
        #     if isLeft(lMean, llMean, rrMean):
        #         # 右脚丢失，右脚点云没有数据
        #         # t = [tr0[0] - rrMean[0], tr0[2] - rrMean[1], 0]
        #         t = [1, 1, 1]
        #         # print("右预测的：", tr0[0], tr0[2], "上一个：", rrMean)
        #         T[:3, 3] = t
        #         # print(t)
        #         rightFootCloud = copy.deepcopy(rightFootCloud).transform(T)
        #         leftFootCloud = o3d.io.read_point_cloud('../data/leftFootCloud.txt', format='xyz')
        #         # 用上一次的右脚点修改这次的预测值
        #         tr = kfRight.correct((np.array([rrMean[0], rrMean[1]]).reshape(2, 1)))
        #         rMean = np.array([tr[0], tr[2], 0])
        #         # rMean = rrMean
        #         # 左脚没丢失
        #         kfLeft.correct((np.array([lMean[0], lMean[1]]).reshape(2, 1)))
        #         iftxt = False
        #     else:
        #         # 左脚丢失，右脚点云没有数据
        #         # t = [tl0[0] - llMean[0], tl0[2] - llMean[1], 0]
        #         t = [1, 1, 1]
        #         # print("左预测的：", tl0[0], tl0[2], "上一个：", llMean)
        #         # print(t)
        #         T[:3, 3] = t
        #         leftFootCloud = copy.deepcopy(leftFootCloud).transform(T)
        #         rightFootCloud = o3d.io.read_point_cloud('../data/leftFootCloud.txt', format='xyz')
        #         rMean = lMean
        #         # 用上一次的左脚点修改这次的预测值
        #         tl = kfLeft.correct((np.array([llMean[0], llMean[1]]).reshape(2, 1)))
        #         lMean = np.array([tl[0], tl[2], 0])
        #         # lMean = llMean
        #         # 右脚没丢失
        #         kfLeft.correct((np.array([rMean[0], rMean[1]]).reshape(2, 1)))
        #         iftxt = False
        #         ifChange = True
        # elif lMean[0] == 0 and lMean[1] == 0 and rMean[0] == 0 and rMean[1] == 0:
        #     # 两个脚都缺失
        #     tr = kfRight.correct((np.array([rrMean[0], rrMean[1]]).reshape(2, 1)))
        #     rMean = np.array([tr[0], tr[2], 0])
        #     # rMean = rrMean
        #     tl = kfLeft.correct((np.array([llMean[0], llMean[1]]).reshape(2, 1)))
        #     lMean = np.array([tl[0], tl[2], 0])
        #     # lMean = llMean
        #     # t = [tr0[0] - rrMean[0], tr0[2] - rrMean[1], 0]
        #     t = [1, 1, 1]
        #     # print(t)
        #     T[:3, 3] = t
        #     rightFootCloud = copy.deepcopy(rightFootCloud).transform(T)
        #     # t = [tl0[0] - llMean[0], tl0[2] - llMean[1], 0]
        #     t = [1, 1, 1]
        #     # print(t)
        #     T[:3, 3] = t
        #     leftFootCloud = copy.deepcopy(leftFootCloud).transform(T)
        #     iftxt = False
        #     # print("左预测的：", tr0[0], tr0[2], "上一个：", rrMean)
        #     # print("右预测的：", tl0[4], tl0[5], "上一个：", llMean)
        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))
                # print("左脚修改后的预测值：", xA - tl[0], yA - tl[2])
                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))
                # print("右脚修改后的预测值：", xA - tr[0], yA - tr[2])
                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]]
                # print("右脚修改后的预测值：", xA - tr[0], yA - tr[2])
                xMin, xMax, yMin, yMax, xA, yA = get('../data/rightFootCloud.txt')
                tl = kfLeft.correct(np.array([xA, yA]).reshape(2, 1))
                # print("左脚修改后的预测值：", xA - tl[0], yA - tl[2])
                lMean = [tl[0], tl[2]]
                t = lMean
                lMean = rMean
                rMean = t
        # print(llMean - lMean)
        # print(rrMean - rMean)
        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')


        leftFootCloud.paint_uniform_color([1, 0, 0])
        rightFootCloud.paint_uniform_color([1, 0, 0])
        vis.add_geometry(leftFootCloud)
        vis.add_geometry(rightFootCloud)
        vis.add_geometry(mapCloud)
        vis.poll_events()  # 注册更新操作
        vis.update_renderer()  # 渲染图窗
        # time.sleep(0.3)
        n = n + 1
        iftxt = True
    vis.run()
    vis.destroy_window()  # 关闭显示窗口