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)
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])
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+')
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()