def SocialNavigationGaussian_getPassOnRight(self, persons, h, dibujar):
plt.close("all")
lx_inf = -6
lx_sup = 8
ly_inf = -6
ly_sup = 8
normals = []
for p in persons:
pn = Person(p.x / 1000, p.z / 1000, p.angle, p.vel)
pn.draw(2., 1, 4. / 3., pi / 2 - pn.th, drawPersonalSpace=dibujar)
pn.draw(2., 1, 4. / 3., pi - pn.th, drawPersonalSpace=dibujar)
normals.append(Normal(mu=[[pn.x], [pn.y]], sigma=[-pn.th - pi / 2., 2, 1, 4. / 3], elliptical=True))
normals.append(Normal(mu=[[pn.x], [pn.y]], sigma=[-pn.th, 2, 0.75, 4. / 3], elliptical=True))
# h = 0.4
# h = prox / 100
resolution = 0.1
limits = [[lx_inf, lx_sup], [ly_inf, ly_sup]]
_, z = Normal.makeGrid(normals, h, 2, limits=limits,
resolution=resolution) # Las posiciones de las personas tienen que estar en metros
grid = GM.filterEdges(z, h)
if (dibujar):
plt.figure()
plt.imshow(grid, extent=[lx_inf, lx_sup, ly_inf, ly_sup], shape=grid.shape, interpolation='none',
aspect='equal', origin='lower', cmap='Greys', vmin=0, vmax=2)
plt.xlabel('X')
plt.ylabel('Y')
plt.axis('equal')
np.savetxt('log.txt', grid, fmt='%i')
polylines = []
totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf)
for pol in totalpuntosorden:
polyline = []
for pnt in pol:
punto = SNGPoint2D()
punto.x = pnt[0] * 1000
punto.z = pnt[1] * 1000
polyline.append(punto)
polylines.append(polyline)
plt.show()
return polylines
def SocialNavigationGaussian_getSocialSpace(self, persons, h, draw):
plt.close('all')
##Limites de la representacion
lx_inf = -6
lx_sup = 10
ly_inf = -6
ly_sup = 10
##########################################CLUSTERING##################################################
normals = []
for p in persons:
pn = Person(p.x / 1000, p.z / 1000, p.angle)
pn.draw(3, 1, 1.3, pi / 2 - pn.th,
drawPersonalSpace=draw) # Valores originales
normals.append(
Normal(mu=[[pn.x], [pn.y]],
sigma=[-pn.th - pi / 2., 3, 1, 1.3],
elliptical=True))
# print ("numero de gaussianas",len(normals))
resolution = 0.1
limits = [[lx_inf, lx_sup], [ly_inf, ly_sup]]
_, z = Normal.makeGrid(normals,
h,
2,
limits=limits,
resolution=resolution)
grid = GM.filterEdges(z, h)
###########################LEO EL GRID Y SEPARO LAS POLILINEAS, DESPUES SE HACE CONVEXHULL####################################
polylines = []
totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf)
for pol in totalpuntosorden:
polyline = []
for pnt in pol:
punto = SNGPoint2D()
punto.x = pnt[0] * 1000
punto.z = pnt[1] * 1000
polyline.append(punto)
polylines.append(polyline)
if (draw):
##DIBUJO ZONA Social
_, z = Normal.makeGrid(normals,
0.1,
2,
limits=limits,
resolution=resolution)
grid = GM.filterEdges(z, 0.1)
polylines = []
totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf)
for pol in totalpuntosorden:
polyline = []
for pnt in pol:
punto = SNGPoint2D()
punto.x = pnt[0]
punto.z = pnt[1]
polyline.append(punto)
polylines.append(polyline)
for ps in polylines:
# plt.figure()
for p in ps:
plt.plot(p.x, p.z, "oc-")
plt.axis('equal')
plt.xlabel('X')
plt.ylabel('Y')
plt.show()
return polylines
def getAllPersonalSpaces(self, persons, represent):
personal_spaces = ["intimate", "personal", "social"]
# sigma_h, sigma_r, sigma_s, h
dict_space_param = {
"intimate": [1.3, 1., 1.3, 0.8],
"personal": [1.3, 1., 1.3, 0.4],
"social": [3., 1., 1.3, 0.1],
}
dict_space_polylines = {
"intimate": [],
"personal": [],
"social": [],
}
dict_spaces_to_plot = {
"intimate": [],
"personal": [],
"social": [],
}
##Limites de la representacion
lx_inf = -6
lx_sup = 10
ly_inf = -6
ly_sup = 10
for space in personal_spaces:
normals = []
for p in persons:
person = Person(p.x, p.z, p.angle)
# print('Pose x', person.x, 'Pose z', person.y, 'Rotacion', person.th)
# person.draw(2,1, 4./3.,pi/2 - person.th, drawPersonalSpace=dibujar) #Valores originales
person.draw(dict_space_param[space][0],
dict_space_param[space][1],
dict_space_param[space][2],
pi / 2 - person.th,
drawPersonalSpace=represent)
normals.append(
Normal(mu=[[person.x], [person.y]],
sigma=[
-person.th - pi / 2.,
dict_space_param[space][0],
dict_space_param[space][1],
dict_space_param[space][2]
],
elliptical=True))
# print ("numero de gaussianas",len(normals))
resolution = 0.1
limits = [[lx_inf, lx_sup], [ly_inf, ly_sup]]
_, z = Normal.makeGrid(normals,
dict_space_param[space][3],
2,
limits=limits,
resolution=resolution)
grid = GM.filterEdges(z, dict_space_param[space][3])
totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf)
for pol in totalpuntosorden:
polyline = []
polyline_to_plt = []
for pnt in pol:
punto = SNGPoint2D()
punto.x = pnt[0] * 1000
punto.z = pnt[1] * 1000
polyline.append(punto)
polyline_to_plt.append([pnt[0], pnt[1]])
dict_space_polylines[space].append(polyline_to_plt)
if len(polyline_to_plt) != 0:
dict_spaces_to_plot[space].append(polyline_to_plt)
if represent:
for soc in dict_spaces_to_plot["social"]:
x, y = zip(*soc)
plt.plot(x, y, color='c', marker='.')
for per in dict_spaces_to_plot["personal"]:
x, y = zip(*per)
plt.plot(x, y, color='m', marker='.')
for inti in dict_spaces_to_plot["intimate"]:
x, y = zip(*inti)
plt.plot(x, y, color='r', marker='.')
plt.axis('equal')
plt.xlabel('X')
plt.ylabel('Y')
plt.show()
return dict_space_polylines['intimate'], dict_space_polylines[
'personal'], dict_space_polylines['social']
sigma=[360 * 3.1415 / 180, 2.0, 1.0, 1.3333333333333333],
elliptical=True),
Normal(mu=[[1.88], [1.88]],
sigma=[290 * 3.1415 / 180, 2.0, 1.0, 1.3333333333333333],
elliptical=True),
Normal(mu=[[2.48], [1.67]],
sigma=[225 * 3.1415 / 180, 2.0, 1.0, 1.3333333333333333],
elliptical=True),
Normal(mu=[[-1.22], [-2.12]],
sigma=[360 * 3.1415 / 180, 2.0, 1.0, 1.3333333333333333],
elliptical=True),
Normal(mu=[[-2.12], [-2.12]],
sigma=[290 * 3.1415 / 180, 2.0, 1.0, 1.3333333333333333],
elliptical=True)
]
_, z = Normal.makeGrid(normals, h, 2, limits=limits, resolution=resolution)
grid = filterEdges(z, h)
np.savetxt('log.txt', grid, fmt='%.4e')
plt.figure()
plt.imshow(z,
shape=grid.shape,
interpolation='none',
aspect='equal',
origin='lower',
cmap='Greys',
vmin=0,
vmax=2)
plt.figure()
def getPersonalSpace(self, persons, h, dibujar):
plt.close('all')
##DESCOMENTAR EL FIGUREEE
# plt.figure()
#ax = fig.add_subplot(111, projection='3d')
#fig, ax = plt.subplots()
#ax.grid(True)
# x = y = np.arange(-3.0, 3.0, 0.05)
# X, Y = np.meshgrid(x, y)
##Limites de la representacion
lx_inf = -6
lx_sup = 10
ly_inf = -6
ly_sup = 10
"""""
##cambio los limites para los otros valores de sigma
lx_inf = 0
lx_sup = 10
ly_inf = 0
ly_sup = 10
""" ""
# zs = np.array([fun(x,y) for x,y in zip(np.ravel(X), np.ravel(Y))])
# Z = zs.reshape(X.shape)
##########################################CLUSTERING##################################################
normals = []
for p in persons:
pn = Person(p.x, p.z, p.angle)
#print('Pose x', pn.x, 'Pose z', pn.y, 'Rotacion', pn.th)
pn.draw(2., 1, 4. / 3., pi / 2 - pn.th, drawPersonalSpace=dibujar)
#normals.append(Normal(mu=[[pn.x], [pn.y]], sigma=[-pn.th - pi/2, 2.0, 2.0, 2.0], elliptical=True))
normals.append(
Normal(mu=[[pn.x], [pn.y]],
sigma=[-pn.th - pi / 2., 2, 1, 4. / 3],
elliptical=True))
#normals.append(Normal(mu=[[pn.x], [pn.y]], sigma=[-pn.th, 2, 1, 4. / 3], elliptical=True))
#print ("numero de gaussianas",len(normals))
# h = 0.1
# h = prox/100
resolution = 0.1
limits = [[lx_inf, lx_sup], [ly_inf, ly_sup]]
_, z = Normal.makeGrid(normals,
h,
2,
limits=limits,
resolution=resolution)
grid = GM.filterEdges(z, h)
#plt.figure()
#plt.imshow(z, shape=grid.shape, interpolation='none', aspect='equal', origin='lower', cmap='Greys', vmin=0, vmax=2)
#plt.figure()
#plt.imshow(grid, shape=grid.shape, interpolation='none', aspect='equal', origin='lower', cmap='Greys', vmin=0, vmax=2)
# if (dibujar):
# # plt.figure()
# plt.imshow(grid, extent=[lx_inf, lx_sup, ly_inf, ly_sup], shape=grid.shape, interpolation='none', aspect='equal', origin='lower', cmap='Greys', vmin=0, vmax=2)
# plt.xlabel('X')
# plt.ylabel('Y')
# plt.axis('equal')
np.savetxt('log.txt', grid, fmt='%i')
###########################LEO EL GRID Y SEPARO LAS POLILINEAS, DESPUES SE HACE CONVEXHULL####################################
polylines = []
totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf)
for pol in totalpuntosorden:
polyline = []
for pnt in pol:
punto = SNGPoint2D()
punto.x = pnt[0]
punto.z = pnt[1]
polyline.append(punto)
polylines.append(polyline)
if (dibujar):
##DIBUJO ZONA Social
_, z = Normal.makeGrid(normals,
0.1,
2,
limits=limits,
resolution=resolution)
grid = GM.filterEdges(z, 0.1)
polylines = []
totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf)
for pol in totalpuntosorden:
polyline = []
for pnt in pol:
punto = SNGPoint2D()
punto.x = pnt[0]
punto.z = pnt[1]
polyline.append(punto)
polylines.append(polyline)
for ps in polylines:
#plt.figure()
for p in ps:
plt.plot(p.x, p.z, "oc-")
plt.axis('equal')
plt.xlabel('X')
plt.ylabel('Y')
# plt.show()
###DIBUJO ZONA Personal
_, z = Normal.makeGrid(normals,
0.4,
2,
limits=limits,
resolution=resolution)
grid = GM.filterEdges(z, 0.4)
polylines = []
totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf)
for pol in totalpuntosorden:
polyline = []
for pnt in pol:
punto = SNGPoint2D()
punto.x = pnt[0]
punto.z = pnt[1]
polyline.append(punto)
polylines.append(polyline)
for ps in polylines:
# plt.figure()
for p in ps:
plt.plot(p.x, p.z, "om-")
plt.axis('equal')
plt.xlabel('X')
plt.ylabel('Y')
# plt.show()
###DIBUJO ZONA INTIMA
_, z = Normal.makeGrid(normals,
0.8,
2,
limits=limits,
resolution=resolution)
grid = GM.filterEdges(z, 0.8)
polylines = []
totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf)
for pol in totalpuntosorden:
polyline = []
for pnt in pol:
punto = SNGPoint2D()
punto.x = pnt[0]
punto.z = pnt[1]
polyline.append(punto)
polylines.append(polyline)
for ps in polylines:
# plt.figure()
for p in ps:
plt.plot(p.x, p.z, "or-")
plt.axis('equal')
plt.xlabel('X')
plt.ylabel('Y')
# plt.show()
plt.show()
return polylines
