def get_cross_section(verts, faces, z=0.1):
#z = np.min(verts[:, -1]) + z # 0.5 is the height of husky, actually it should be 0.37
cross_section = meshcut.cross_section(verts,
faces,
plane_orig=(0, 0, z),
plane_normal=(0, 0, 1))
return cross_section
def main():
fn = args.mesh_name
verts, faces = load_obj(fn)
z = np.min(
verts[:, -1]
) + 0.5 # 0.5 is the height of husky, actually it should be 0.37
# cut the mesh with a surface whose value on z-axis is plane_orig, and its normal is plane_normal vector
#print('verts: {}'.format(verts[0]))
cross_section = meshcut.cross_section(verts,
faces,
plane_orig=(0, 0, z),
plane_normal=(0, 0, 1))
for item in cross_section:
for i in range(len(item) - 1):
#print('item:{}', item[i, 0])
plt.plot(item[i:i + 2, 0], item[i:i + 2, 1], 'b')
#print('item:{}'.format(item[i:i+2,0]))
if args.trajFilePath:
print('Process trajectory file: {}'.format(args.trajFilePath))
points = np.load(args.trajFilePath)
for i in range(len(points) - 1):
# infos is of structure [{'eye_pos':, 'eye_quat':, 'episode':}, {}]
first = points[i]
second = points[i + 1]
plt.plot([first[0], second[0]], [first[1], second[1]], 'k')
## save image to the path
if args.save_name is not None:
print('save image to {}'.format(args.save_name))
plt.savefig(args.save_name, dpi=600)
else:
plt.show()
def slice(self, verts, faces, z, zout=None):
block = Block("slice %f" % (float(z)))
#FIXME: slice along different axes
plane_orig = (0, 0, z) #z height to slice
plane_norm = (0, 0, 1)
#Crosscut
contours = meshcut.cross_section(verts, faces, plane_orig, plane_norm)
#Flatten contours
if zout is not None:
for contour in contours:
for segment in contour:
segment[2] = zout
#Contours to G-code
for contour in contours:
#print(contour)
first = contour[0]
block.append("g0 x%f y%f z%f" % (first[0], first[1], first[2]))
for segment in contour:
block.append("g1 x%f y%f z%f" %
(segment[0], segment[1], segment[2]))
block.append("g1 x%f y%f z%f" % (first[0], first[1], segment[2]))
block.append("( ---------- cut-here ---------- )")
if block: del block[-1]
if not block: block = None
return block
def main():
fn = args.meshFilePath
verts, faces = load_obj(fn)
z = np.min(
verts[:, -1]
) + 0.5 # 0.5 is the height of husky, actually it should be 0.37
# cut the mesh with a surface whose value on z-axis is plane_orig, and its normal is plane_normal vector
#print('verts: {}'.format(verts[0]))
print('meshcut ...')
cross_section = meshcut.cross_section(verts,
faces,
plane_orig=(0, 0, z),
plane_normal=(0, 0, 1))
plt.figure()
for item in cross_section:
for i in range(len(item) - 1):
#print('item:{}', item[i, 0])
plt.plot(item[i:i + 2, 0], item[i:i + 2, 1], 'b')
#print('item:{}'.format(item[i:i+2,0]))
plt.xticks(np.arange(-2, 10, 0.5))
plt.yticks(np.arange(-2, 9, 0.5))
plt.grid(True)
## save image to the path
if args.save_name is not None:
print('save image to {}'.format(args.save_name))
plt.savefig(args.save_name)
else:
plt.show()
def main(mesh_file_name):
verts, faces = load_obj(mesh_file_name)
z = np.min(
verts[:, -1]
) + 0.5 # 0.5 is the height of husky, actually it should be 0.37
# cut the mesh with a surface whose value on z-axis is plane_orig, and its normal is plane_normal vector
#print('verts: {}'.format(verts[0]))
print('meshcut ...')
cross_section = meshcut.cross_section(verts,
faces,
plane_orig=(0, 0, z),
plane_normal=(0, 0, 1))
n_nodes = sum([len(x) for x in cross_section])
n_edges = n_nodes - len(cross_section)
output_file_name = mesh_file_name.replace('.obj', '_2d.obj')
with open(output_file_name, 'w') as output_file:
output_file.write("# {} vertices, {} edges\n".format(n_nodes, n_edges))
for item in cross_section:
for node in item:
output_file.write("v {:0.6f} {:0.6f}\n".format(
node[0], node[1]))
i = 0
for item in cross_section:
print("{}".format(len(item)))
for _ in range(len(item) - 1):
output_file.write("e {} {}\n".format(i, i + 1))
i += 1
i += 1
def main():
fn = args.meshFilePath
verts, faces = load_obj(fn)
z = np.min(
verts[:, -1]
) + 0.5 # 0.5 is the height of husky, actually it should be 0.37
'''
# find better z, because the scene might have numbers of floors
if args.trajFilePath:
print('Process trajectory file: {}'.format(args.trajFilePath))
infos = np.load(args.trajFilePath)
for i in range(len(infos)):
first = infos[i]['eye_pos']
if z > first[2]:
z = first[2]
'''
# cut the mesh with a surface whose value on z-axis is plane_orig, and its normal is plane_normal vector
#print('verts: {}'.format(verts[0]))
print('meshcut ...')
cross_section = meshcut.cross_section(verts,
faces,
plane_orig=(0, 0, z),
plane_normal=(0, 0, 1))
plt.figure()
for item in cross_section:
for i in range(len(item) - 1):
#print('item:{}', item[i, 0])
plt.plot(item[i:i + 2, 0], item[i:i + 2, 1], 'b')
#print('item:{}'.format(item[i:i+2,0]))
print('draw trajectory ...')
if args.trajFilePath:
print('Process trajectory file: {}'.format(args.trajFilePath))
infos = np.load(args.trajFilePath)
for i in range(len(infos) - 1):
# infos is of structure [{'eye_pos':, 'eye_quat':, 'episode':}, {}]
first = infos[i]['eye_pos']
second = infos[i + 1]['eye_pos']
plt.plot([first[0], second[0]], [first[1], second[1]], 'k')
# draw start point as yellow circle and end point as yellow star
start_point = infos[0]['eye_pos']
end_point = infos[len(infos) - 1]['eye_pos']
plt.plot(start_point[0], start_point[1], color='r', marker='o')
plt.plot(end_point[0], end_point[1], color='r', marker='*')
#plt.show()
#print('start_point: {}'.format(start_point))
#print('end_point: {}'.format(end_point))
## draw target on image
if args.targetX is not None and args.targetY is not None:
print('draw target to image ...')
plt.plot(args.targetX, args.targetY, color='y', marker='*')
## save image to the path
plt.title('{}'.format(args.save_name[args.save_name.rfind('/') + 1:-4]))
if args.save_name is not None:
print('save image to {}'.format(args.save_name))
plt.savefig(args.save_name)
else:
plt.show()
def mesh(model_id="", episode=0, waypoint=False):
C1 = '\033[91m'
C1END = '\033[0m'
print(C1 + "PLOTTING EPISODE:" + C1END)
plt.style.use('default') # switches back to matplotlib style
fn = os.path.join(
os.path.expanduser("~"),
"PycharmProjects/Gibson_Exercise/gibson/assets/dataset/") + str(
model_id) + "/mesh_z_up.obj"
verts, faces = load_obj(fn)
z = np.min(verts[:, -1]) + 0.5 # your robot height
cross_section = meshcut.cross_section(verts,
faces,
plane_orig=(0, 0, z),
plane_normal=(0, 0, 1))
plt.figure(figsize=(16, 8))
plt.subplot(1, 2, 1)
for item in cross_section:
for i in range(len(item) - 1):
plt.plot(item[i:i + 2, 0], item[i:i + 2, 1], 'k')
plt.title('Map of Navigation')
plt.xlabel('X Position')
plt.ylabel('Y Position')
plt.grid(True)
if waypoint:
#df = pandas.read_csv('euharlee_waypoints_sort.csv')
df = pandas.read_csv(WAY_PATH +
str('euharlee_waypoints_clipped_sort.csv'))
#df = pandas.read_csv('aloha_waypoints_clipped_sort.csv')
points = df.values
length = len(points)
sp = np.zeros((length, 3))
ang = np.zeros((length, 1))
gp = np.zeros((length, 3))
complexity = np.zeros((length, 1))
for r in range(length):
sp[r] = np.array([points[r][2], points[r][3], points[r][4]])
ang[r] = np.array([points[r][5]])
gp[r] = np.array([points[r][6], points[r][7], points[r][8]])
complexity[r] = np.array([points[r][10] / points[r][9]])
print("ModelID:", points[0][1])
plt.plot(sp[episode % len(sp) - 1][0], sp[episode % len(sp) - 1][1],
'r*')
plt.plot(gp[episode % len(sp) - 1][0], gp[episode % len(sp) - 1][1],
'g*')
print("(%i) Nav Complexity ---> %.3f" %
(episode, complexity[episode % len(sp) - 1]))
debug = 0
if debug:
plt.show()
def get_zslice(z_ind):
"""Use meshcut to get z slices of mesh, create 2D masks of each slice, and combine slices to get 3D mask.
:param z_ind: index of z slice mask to cut from mesh
:return: 2D image as binary mask of z slice at this index
"""
im = Image.fromarray(np.zeros((imsize, imsize), np.uint8).T)
if zmin < z_ind < zmax:
cut = meshcut.cross_section(verts_shift, faces, [0, 0, z_ind],
[0, 0, 1])
polygon = cut[0][:, :2]
draw = ImageDraw.Draw(im)
draw.polygon(polygon.round().astype(np.uint8).flatten().tolist(),
fill=255)
return np.array(im) / 255
def slice_with_plane(self, origin: np.ndarray, normal: np.ndarray) -> list:
# Returns a list with the slices (in case we have a concave mesh)
def intersect_edges(vertices_a: np.ndarray,
vertices_b: np.ndarray) -> np.ndarray:
# Returns a mask of all edges that *might* be sliced by the plane
# Adapted from https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection
epsilon = 1e-6
diff_dot_n = np.dot(origin - vertices_a, normal)
l_dot_n = np.dot(vertices_b - vertices_a, normal)
t = diff_dot_n / l_dot_n
# Edge is completely contained in plane
mask_full = abs(l_dot_n) < epsilon
mask_full &= abs(diff_dot_n) < epsilon
# One intersection
mask_point = 0.0 <= t
mask_point &= t <= 1.0
return mask_full | mask_point
def preprocess_triangles(triangles: np.ndarray) -> np.ndarray:
# Pre-processes the triangles by each of their edges
mask_a = intersect_edges(triangles[0::3], triangles[1::3])
mask_b = intersect_edges(triangles[1::3], triangles[2::3])
mask_c = intersect_edges(triangles[2::3], triangles[0::3])
return mask_a | mask_b | mask_c
mask = preprocess_triangles(self.triangles)
try:
return meshcut.cross_section(self.vertices, self.indices[mask],
np.array(origin), np.array(normal))
except AssertionError:
# Meshcut doesn't want to slice
print(
f'WARNING: Mesh {self.name} (id = {self.id}) cannot be sliced, fix your mesh!'
)
return []
def slice(self, verts, faces, z, zout=None, axis='z'):
tags = '[slice]'
if axis == 'z': tags = '[slice,minz:%f]' % (float(z))
block = Block("slice %s%f %s" % (axis, float(z), tags))
#FIXME: slice along different axes
if axis == 'x':
plane_orig = (z, 0, 0)
plane_norm = (1, 0, 0)
elif axis == 'y':
plane_orig = (0, z, 0)
plane_norm = (0, 1, 0)
else:
plane_orig = (0, 0, z) #z height to slice
plane_norm = (0, 0, 1)
#Crosscut
contours = meshcut.cross_section(verts, faces, plane_orig, plane_norm)
#Flatten contours
if zout is not None:
for contour in contours:
for segment in contour:
segment[2] = zout
#Contours to G-code
for contour in contours:
#print(contour)
gtype = 0
for segment in contour:
block.append("g%s x%f y%f z%f" %
(gtype, segment[0], segment[1], segment[2]))
gtype = 1
block.append(
"g1 x%f y%f z%f" %
(contour[0][0], contour[0][1], contour[0][2])) #Close shape
block.append("( ---------- cut-here ---------- )")
if block: del block[-1]
if not block: block = None
return block
def gen_map(obj_filepath, mesh_dir, img_filename_format='floor_{}.png'):
vertices, faces = load_obj_np(obj_filepath)
xmin, ymin, _ = vertices.min(axis=0)
xmax, ymax, _ = vertices.max(axis=0)
max_length = np.max(
[np.abs(xmin), np.abs(ymin),
np.abs(xmax), np.abs(ymax)])
max_length = np.ceil(max_length).astype(np.int)
with open(os.path.join(mesh_dir, 'floors.txt')) as f:
floors = map(float, f.readlines())
floors = sorted(floors)
print(floors)
for i_floor, floor in enumerate(floors):
z = float(floor) + 0.5
cross_section = meshcut.cross_section(vertices,
faces,
plane_orig=(0, 0, z),
plane_normal=(0, 0, 1))
floor_map = np.ones((2 * max_length * 100, 2 * max_length * 100))
for item in cross_section:
for i in range(len(item) - 1):
x1, x2 = (item[i:i + 2, 0] + max_length) * 100
y1, y2 = (item[i:i + 2, 1] + max_length) * 100
cv2.line(floor_map, (x1, y1), (x2, y2),
color=(0, 0, 0),
thickness=2)
cur_img = Image.fromarray((floor_map * 255).astype(np.uint8))
#cur_img = Image.fromarray(np.flipud(cur_img))
img_filename = img_filename_format.format(i_floor)
cur_img.save(os.path.join(mesh_dir, img_filename))
write_yaml(mesh_dir,
np.array(cur_img),
img_filename,
'floor_{}.yaml'.format(i_floor),
resolution=0.01)
def slice(self, verts, faces, z, zout=None, axis='z'):
tags = '[slice]'
if axis=='z': tags = '[slice,minz:%f]'%(float(z))
block = Block("slice %s%f %s"%(axis,float(z),tags))
#FIXME: slice along different axes
if axis == 'x':
plane_orig = (z, 0, 0)
plane_norm = (1, 0, 0)
elif axis == 'y':
plane_orig = (0, z, 0)
plane_norm = (0, 1, 0)
else:
plane_orig = (0, 0, z) #z height to slice
plane_norm = (0, 0, 1)
#Crosscut
contours = meshcut.cross_section(verts, faces, plane_orig, plane_norm)
#Flatten contours
if zout is not None:
for contour in contours:
for segment in contour:
segment[2] = zout
#Contours to G-code
for contour in contours:
#print(contour)
gtype = 0
for segment in contour:
block.append("g%s x%f y%f z%f"%(gtype, segment[0],segment[1],segment[2]))
gtype = 1
block.append("g1 x%f y%f z%f"%(contour[0][0],contour[0][1],contour[0][2])) #Close shape
block.append("( ---------- cut-here ---------- )")
if block: del block[-1]
if not block: block = None
return block
def main(scene_idx):
meshFilePath = 'gibson/assets/dataset/{}_for_rrt/mesh_z_up.obj'.format(
Test_Scenes[scene_idx])
verts, faces = load_obj(meshFilePath)
z = mapper_scene2z[Test_Scenes[
scene_idx]] # 0.5 is the height of husky, actually it should be 0.37
# cut the mesh with a surface whose value on z-axis is plane_orig, and its normal is plane_normal vector
#print('verts: {}'.format(verts[0]))
print('meshcut ...')
cross_section = meshcut.cross_section(verts,
faces,
plane_orig=(0, 0, z),
plane_normal=(0, 0, 1))
plt.figure()
for item in cross_section:
for i in range(len(item) - 1):
#print('item:{}', item[i, 0])
plt.plot(item[i:i + 2, 0], item[i:i + 2, 1], 'b')
#print('item:{}'.format(item[i:i+2,0]))
left_x, bottom_y, right_x, top_y = mapper_scene2xyticks[
Test_Scenes[scene_idx]]
x_major_ticks = np.arange(left_x, right_x, 0.5)
x_minor_ticks = np.arange(left_x, right_x, 0.5)
y_major_ticks = np.arange(bottom_y, top_y, 0.5)
y_minor_ticks = np.arange(bottom_y, top_y, 0.5)
plt.xticks(x_major_ticks)
#plt.xticks(x_minor_ticks, minor=True)
plt.yticks(y_major_ticks)
#plt.yticks(y_minor_ticks, minor=True)
plt.grid(True)
## save image to the path
file_addr = '/home/reza/Datasets/GibsonEnv/my_code/rrt/scene_grid_map'
img_name = '{}_grid.png'.format(Test_Scenes[scene_idx])
plt.savefig('{}/{}'.format(file_addr, img_name),
bbox_inches='tight',
dpi=(400))
plt.close()
def line_map(superp,
path,
writing_path,
multi_floor,
object_map=False,
draw=False,
resolution=0.04):
'''
generate the line map
:param superp: how many superpostion for the line map
:param path: reading and saving path
:param draw: whether to display the map
:param resolution: set default to be 0.4
:return: the significant parameters that influence the map
'''
_, _, floor_height, robot_h, min_height = multi_floor
fn = path + 'mesh_z_up.obj'
verts, faces = load_obj(fn)
# robot_h = np.min(verts[:, -1]) + 0.5 # your robot height #probably using this to optimize the algo
plt.figure(0)
x_max_list = []
y_max_list = []
x_min_list = []
y_min_list = []
cross_section = meshcut.cross_section(verts,
faces,
plane_orig=(0, 0, floor_height),
plane_normal=(0, 0, 1))
for item in cross_section:
#create the map
x_max, y_max = np.max(item[:, :2], axis=0) # find the largest x and y
x_min, y_min = np.min(item[:, :2], axis=0) # find the minimum x and y
x_max_list.append(x_max)
y_max_list.append(y_max)
x_min_list.append(x_min)
y_min_list.append(y_min)
min_point = (np.min(x_min_list), np.min(y_min_list))
max_point = (np.max(x_max_list), np.max(y_max_list))
raw_map = np.zeros((int(
(max_point[1] - min_point[1]) / resolution) + int(2 / resolution),
int((max_point[0] - min_point[0]) / resolution) +
int(2 / resolution))) #generate the map
if object_map == True:
rawmap_with_obj = raw_map.copy()
z = min_height
cross_section = meshcut.cross_section(verts,
faces,
plane_orig=(0, 0, z),
plane_normal=(0, 0, 1))
for item in cross_section:
for i in range(len(item) - 1):
(x1, y1) = transform_real_to_cv(item[i, 0], item[i,
1], min_point,
max_point, resolution)
(x2, y2) = transform_real_to_cv(item[i + 1, 0], item[i + 1, 1],
min_point, max_point,
resolution)
cv2.line(rawmap_with_obj, (x1, y1), (x2, y2), 150, 2)
return z, rawmap_with_obj, max_point, min_point, resolution
for i in range(superp):
z = min_height + (i + 1) * (
robot_h - min_height
) / superp #make a croos section between floor height and robot_h
cross_section = meshcut.cross_section(verts,
faces,
plane_orig=(0, 0, z),
plane_normal=(0, 0, 1))
for item in cross_section:
for i in range(len(item) - 1):
(x1, y1) = transform_real_to_cv(item[i, 0], item[i,
1], min_point,
max_point, resolution)
(x2, y2) = transform_real_to_cv(item[i + 1, 0], item[i + 1, 1],
min_point, max_point,
resolution)
cv2.line(raw_map, (x1, y1), (x2, y2), 150, 2)
if draw == True:
cv2.namedWindow('testwindow')
cv2.imshow('testwindow', raw_map)
cv2.waitKey(0)
cv2.imwrite(writing_path + 'raw_map.png', raw_map)
return robot_h, raw_map, max_point, min_point, resolution
def mesh(model_id="", waypoint=False):
C1 = '\033[91m'
C1END = '\033[0m'
print(C1 + "PLOTTING EPISODE:" + C1END)
plt.style.use('default') # switches back to matplotlib style
fn = os.path.join(
os.path.expanduser("~"),
"PycharmProjects/Gibson_Exercise/gibson/assets/dataset/") + str(
model_id) + "/mesh_z_up.obj"
verts, faces = load_obj(fn)
z = np.min(verts[:, -1]) + 0.5 # your robot height
cross_section = meshcut.cross_section(verts,
faces,
plane_orig=(0, 0, z),
plane_normal=(0, 0, 1))
plt.figure(figsize=(8, 8))
for item in cross_section:
for i in range(len(item) - 1):
plt.plot(item[i:i + 2, 0], item[i:i + 2, 1], 'k')
plt.title('Map of Navigation')
plt.xlabel('X Position')
plt.ylabel('Y Position')
plt.grid(True)
if waypoint:
#df = pandas.read_csv(WAY_PATH + str('aloha_waypoints_sort_test.csv'))
#df = pandas.read_csv(WAY_PATH + str('aloha_waypoints_clipped_sort.csv'))
df = pandas.read_csv(WAY_PATH +
str('euharlee_waypoints_sort_test.csv'))
#df = pandas.read_csv(WAY_PATH + str('euharlee_waypoints_clipped_sort.csv'))
points = df.values
length = len(points)
sp = np.zeros((length, 3))
ang = np.zeros((length, 1))
gp = np.zeros((length, 3))
complexity = np.zeros((length, 1))
for r in range(length):
sp[r] = np.array([points[r][2], points[r][3], points[r][4]])
ang[r] = np.array([points[r][5]])
gp[r] = np.array([points[r][6], points[r][7], points[r][8]])
complexity[r] = np.array([points[r][10] / points[r][9]])
for k in range(length):
plt.plot(sp[k][0], sp[k][1], 'r*')
plt.plot(gp[k][0], gp[k][1], 'g*')
line = plt.plot([sp[k][0], gp[k][0]], [sp[k][1], gp[k][1]],
color='dodgerblue',
linewidth=1)
m1 = (sp[k][0] + gp[k][0]) / 2
m2 = (sp[k][1] + gp[k][1]) / 2
plt.annotate(s='',
xy=(gp[k][0], gp[k][1]),
xytext=(sp[k][0], sp[k][1]),
arrowprops=dict(arrowstyle='->', color='grey'))
#plt.arrow([sp[k][0], gp[k][0]], [sp[k][1], gp[k][1]], [dx],[dy], shape='full', lw=0, length_includes_head=True, head_width=.05)
print("%i Waypoint Navigation Complexity ---> %.3f" %
(k + 1, complexity[k]))
debug = 1
if debug:
plt.savefig(os.path.join(SAVE_PATH + 'waypoints_map_test.png'))
#plt.savefig(os.path.join(SAVE_PATH + 'waypoints_map.png'))
plt.show()
