from sklearn.cluster import DBSCAN
from KF import KalmanFilter
from test.PeiZhun import *
import open3d as o3d
np.warnings.filterwarnings('ignore', category=np.VisibleDeprecationWarning)
lastr = []
lastl = []
lf = 0
lastp = 0
change = False
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 xleftRight(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
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 leftRight(pointList, s1, s2):
global lastl
global lastr
global lf
global lastp
global change
leftFootList = []
rightFootList = []
f0 = open(s1, "w+")
f1 = open(s2, "w+")
n = 0
db = DBSCAN(eps=5, min_samples=6)
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
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)
# print(lMean)
# print(rMean)
# print("------------------------------")
if ll != 0 and lr != 0 and lf == 0:
lastl = lMean
lastr = rMean
lf = 1
if ll != 0 and lf == 1:
if isLeft(lMean, lastl, lastr):
# 这次的左脚就是上次的左脚
if lr != 0:
lastl = lMean
lastr = rMean
else:
change = True
if lr != 0:
lastl = rMean
lastr = lMean
if lr != 0 and lastp == 0:
change = True
lastp = lr
return
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:
change = False
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)
xleftRight(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')
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])
if change:
xleftFootCloud.paint_uniform_color([0, 0, 0])
xrightFootCloud.paint_uniform_color([0, 0, 0])
f0.write(str(np.array(xrightFootCloud.points).shape[0]) + "\n")
f1.write(str(np.array(xleftFootCloud.points).shape[0]) + "\n")
else:
xleftFootCloud.paint_uniform_color([0, 0, 0])
xrightFootCloud.paint_uniform_color([0, 0, 0])
f0.write(str(np.array(xleftFootCloud.points).shape[0]) + "\n")
f1.write(str(np.array(xrightFootCloud.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() # 关闭显示窗口
备份11111
最新推荐文章于 2024-09-01 16:57:39 发布