整体思路
提取雷达地图点数据,绘制地图。读取包含地图点和脚步点的雷达数据,做脚步提取,脚步区分,脚步跟踪。
地图点提取
读取所有帧,当前角度出现最多次数的点为地图点
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() # 关闭显示窗口