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=10)
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')
# print(footCloud)
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)
# 右脚点集缺失
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() # 关闭显示窗口
脚步追踪备份
最新推荐文章于 2024-09-15 22:31:42 发布