def plot_particle_3d(particle, environment, cameras=None, placed_cameras=None, view_time=0.0, figure_number=0,
opt_options=CameraPlacementOptions(), reevaluate_state=False):
"""
Plots cameras and environment in 3D
:param particle: particle to be plotted (np.array)
:param environment: Environment class instance
:param cameras: a list of Camera objects corresponding to the cameras which have already been placed in an
environment
:param placed_cameras: PlacedCameras class instance containing already placed cameras
:param view_time: how long the plot should be visible
:param figure_number: figure number on which to draw environment
:param opt_options: optimization options
:param reevaluate_state: whether or not to re-evaluate the camera state using the particle input (defaults to false)
:return: none
"""
figure = plt.figure(figure_number)
plt.clf()
ax = Axes3D(figure)
# cameras should already have correct state following particle evaluation. only re-evaluate if needed
if reevaluate_state:
cameras = convert_particle_to_state(environment, particle, cameras, opt_options)
draw_room_3d(environment=environment, cameras=cameras, placed_cameras=placed_cameras, fill_alpha=0.1, ax=ax)
ax.elev = 15
ax.azim = -140
# ax.elev = 54
# ax.azim = -70
if view_time > 0:
plt.pause(view_time) # block for view_time seconds
elif view_time < 0:
plt.show() # block until closed
else:
plt.waitforbuttonpress() # block until any button is pressed
def initialize_opt_options():
np.random.seed(round(time.time()))
# loading configuration
config = configparser.ConfigParser()
rospack = rospkg.RosPack()
config.read(rospack.get_path("perch_placement") + "/src/config/opt.ini")
# extract configuration values
env_conf = config['ENVIRONMENT']
mesh_env = env_conf.getboolean('mesh_env')
world_frame = np.asarray(np.matrix(env_conf['world_frame'])).squeeze()
angle_threshold = env_conf.getfloat('angle_threshold')
dist_threshold = env_conf.getfloat('dist_threshold')
min_room_height = env_conf.getfloat('min_room_height')
erosion_raster_density = env_conf.getint('erosion_raster_density')
min_obstacle_radius = env_conf.getfloat('min_obstacle_radius')
nearest_neighbor_restriction = env_conf.getboolean('enable_nearest_neighbor_restriction')
target_volume_height = env_conf.getfloat('target_volume_height')
cam_conf = config['CAMERA']
variable_pan = cam_conf.getboolean('variable_pan')
variable_tilt = cam_conf.getboolean('variable_tilt')
variable_zoom = cam_conf.getboolean('variable_zoom')
drone_conf = config['DRONE']
perch_on_ceiling = drone_conf.getboolean('perch_on_ceiling')
perch_on_walls = drone_conf.getboolean('perch_on_walls')
land_on_floor = drone_conf.getboolean('land_on_floor')
perch_on_intermediate_angles = drone_conf.getboolean('perch_on_intermediate_angles')
min_perch_window = np.asarray(np.matrix(drone_conf['min_perch_window'])).squeeze()
perch_window_shape = drone_conf['perch_window_shape']
min_recovery_height = drone_conf.getfloat('min_recovery_height')
variable_height = drone_conf.getboolean('variable_height')
search_env = config['SEARCH']
angle_mode = search_env['angle_mode']
target_deviation = np.asarray(np.matrix(search_env['target_deviation'])).squeeze()
min_vertices = search_env.getint('min_vertices')
individual_surface_opt = search_env.getboolean('individual_surface_opt')
map_to_flat_surface = search_env.getboolean('map_to_flat_surface')
minimum_score_deviation = search_env.getfloat('minimum_score_deviation')
minimum_particle_deviation = search_env.getfloat('minimum_particle_deviation')
inside_out_search = search_env.getboolean('inside_out_search')
if individual_surface_opt:
surface_number = 0 # placeholder for now. will modify in loop below
else:
surface_number = -1
vary_position_over_face = search_env.getboolean('vary_position_over_face')
noise_resistant_particles = search_env.getint('noise_resistant_particles')
noise_resistant_sample_size = search_env.getint('noise_resistant_sample_size')
N_points = search_env.getint('N_points')
# Optimizer Options:
optimization_options = CameraPlacementOptions(variable_pan=variable_pan, variable_tilt=variable_tilt,
variable_zoom=variable_zoom, perch_on_ceiling=perch_on_ceiling,
perch_on_intermediate_angles=perch_on_intermediate_angles,
perch_on_walls=perch_on_walls, land_on_floor=land_on_floor,
variable_height=variable_height, mesh_env=mesh_env,
angle_mode=angle_mode, angle_threshold=angle_threshold,
dist_threshold=dist_threshold, min_room_height=min_room_height,
target_deviation=target_deviation, min_perch_window=min_perch_window,
vary_position_over_face=vary_position_over_face,
erosion_raster_density=erosion_raster_density,
min_obstacle_radius=min_obstacle_radius,
nearest_neighbor_restriction=nearest_neighbor_restriction,
target_volume_height=target_volume_height,
world_frame=world_frame, min_vertices=min_vertices,
surface_number=surface_number,
map_to_flat_surface=map_to_flat_surface,
perch_window_shape=perch_window_shape,
min_recovery_height=min_recovery_height,
minimum_score_deviation=minimum_score_deviation,
minimum_particle_deviation=minimum_particle_deviation,
inside_out_search=inside_out_search,
noise_resistant_particles=noise_resistant_particles,
noise_resistant_sample_size=noise_resistant_sample_size,
n_points=N_points)
return optimization_options
def convert_particle_to_angle(particle_angle, wall_normal, camera, environment, axis=2,
opt_options=CameraPlacementOptions(), target_center=False):
"""
Converts a particle weight into a pan or tilt angle
:param particle_angle: particle weight, on range [0, 1]
:param wall_normal: normal direction of the surface on which the camera is placed
:param camera: Camera instance
:param environment: Environment Class instance containing the environment in which to place cameras
:param axis: 0 (roll, not used) 1 (tilt/pitch axis), or 2 (pan/yaw axis)
:param opt_options: used to specify the optimization options which impact how particles are mapped to angles.
Relevant options include:
angle_mode
"TOWARDS_TARGET" - the camera is always pointed to within some angular variation (currently +/- 15 deg) of the
target centroid
"WALL_LIMITED" - the camera is always pointed within the internal 180 degrees of the surface which it is
perching on. i.e. [-90, 90]
"FOV_LIMITED" - the camera is always pointed within the range of angles which the FOV first contacts the surface
which it is perching on.
(i.e. [-90+FOV/2, 90-FOV/2])
"GIMBAL_LIMITED" - the intersection of the FOV_LIMITED range and the camera's specified gimbal limits
"GIMBAL_LIMITED_TARGETING" - this mode attempts to point TOWARDS_TARGET, but still conforms with gimbal limits
All other opt result in a 0 to 360 value to be specified
target_deviation - a 2d array or list containing the maximum allowable angular deviation away from the centroid of
the target region
:param target_center: bool. if true, disregard the particle and point either with 0 gimbal offset (for WALL, FOV, or
GIMBAL limited options), directly towards the target center, for TOWARDS_TARGET, or as close to target center as
possible for GIMBAL_LIMITED_TARGETING
:return: converted angle to assign to camera
"""
# extract euler angles from 0-direction of camera at surface
eul = vec2eul(np.dot(camera.camera_rotation, wall_normal))
norm_offset = eul[axis] # uses standard convention; eul[0] = roll, eul[1] = pitch/tilt, eul[2] = pan/yaw
angle_mode = opt_options.angle_mode
target_deviation = opt_options.target_deviation
if target_center:
particle_angle = 0.5
if angle_mode == "WALL_LIMITED":
# recenter to -90 to 90 about edge normal
angle = (particle_angle * 360 - 180) / 2 + norm_offset
elif angle_mode == "FOV_LIMITED":
# recenter about edge normal between -(90-fov/2) and (90-fov/2)
angle = ((particle_angle * 360 - 180) / 2) * ((180 - camera.fov[1-axis]) / 180) + norm_offset
# using 1-axis b/c FOV is listed as PAN (Horizontal) then TILT (vertical)
# while you could consider adding different cases for corner points where the view is more or less than 180 deg,
# it doesn't seem worth the complexity. A camera cannot perch on a corner and wouldn't benefit from increased
# FOV. A drone also can't perch too near an inner corner, limiting the need for consideration over inner corner
# occlusions.
elif angle_mode == "GIMBAL_LIMITED":
min_angle = np.min([90-camera.fov[1-axis]/2, camera.gimbal_limit[axis-1]])
angle = (particle_angle * 2 * min_angle - min_angle) + norm_offset
elif angle_mode == "TOWARDS_TARGET":
# restrict to +/- X degrees about the center of the target region
# find vector between camera location and target centroid
vec_dir = np.array([environment.target_centroid[0] - camera.pose[0],
environment.target_centroid[1] - camera.pose[1],
environment.target_centroid[2] - camera.pose[2]])
vec_dir = vec_dir / dist(vec_dir, np.zeros(3)) # normalize to get unit vector
eul = vec2eul(vec_dir) # convert to euler angles. use pitch for tilt, use yaw for pan
angle = (particle_angle * target_deviation[axis-1] - target_deviation[axis-1]/2) + eul[axis]
elif angle_mode == "GIMBAL_LIMITED_TARGETING":
# point towards target center, if allowed by gimbal limits. else get as close as possible
# find vector between camera location and target centroid
vec_dir = np.array([environment.target_centroid[0] - camera.pose[0],
environment.target_centroid[1] - camera.pose[1],
environment.target_centroid[2] - camera.pose[2]])
vec_dir = vec_dir / dist(vec_dir, np.zeros(3)) # normalize to get unit vector
eul = vec2eul(vec_dir) # convert to euler angles. use pitch for tilt, use yaw for pan
# simplify notation a bit here... t=target direction, n=normal direction, g=gimbal limit, d=target deviation
t = eul[axis]
n = norm_offset
g = camera.gimbal_limit[axis-1]
d = target_deviation[axis-1]
# min angular distance between normal and target directions
delta_angle = min_ang_dist(t, n)
if np.abs(delta_angle) < g + d:
# set of deviations about target direction intersects set of angles within gimbal limit, about the normal
# direction. In this case, want the union of the two sets
if target_center: # disregard particle and try to get as close as possible to target direction
if np.abs(delta_angle) < g:
return t # target is within gimbal limits; aim at target
else: # return closest gimbal bound to target
if np.abs(min_ang_dist(t, n+g)) < np.abs(min_ang_dist(t, n-g)):
return n+g
else:
return n-g
bounds = np.array([n + max(-g, delta_angle - d),
n + min(g, delta_angle + d)]) # upper and lower bound of resultant set
# select point within set
set_range = bounds[1]-bounds[0]
angle = (particle_angle * set_range) + bounds[0]
else:
# target is not within gimbal limits. set angular range as the
ubd = min_ang_dist(t, n+g)
lbd = min_ang_dist(t, n-g)
if np.abs(ubd) < np.abs(lbd):
angle = n+g
else:
angle = n+g
else:
# angle_mode == "NO_LIMIT":
# recenter to -180 to 180
angle = (particle_angle * 360 - 180)
return angle
def convert_particle_to_state(environment, particle, cameras, opt_options=CameraPlacementOptions(), debug=False):
"""
Converts the weights of the particle into the state of the Camera to be evaluated
:param environment: Environment Class instance containing the environment in which to place cameras
:param particle: individual particle as np.array
:param cameras: a list of Camera objects corresponding to the cameras which have already been placed in an
environment
:param opt_options: CameraPlacementOptions class containing opt opt
:param debug: flag, if true print and draw relevant information.
:return: list of cameras with updated state
"""
n_cameras = len(cameras)
ppc = opt_options.get_particle_size() # particles per camera
for i in range(n_cameras):
particle_index = 0
if opt_options.mesh_env:
particle_index = 1 # 1D by default (to select face center on preselected surface)
surf_index = -1
if opt_options.surface_number < 0:
particle_index += 1 # extra term required for surface selection
elif 0 <= opt_options.surface_number < len(environment.perch_regions):
surf_index = opt_options.surface_number
else:
print("ERROR: SURFACE ID TOO LARGE. THIS SHOULD NOT HAPPEN")
cameras[i].pose *= np.nan
break
if opt_options.map_to_flat_surface:
particle_index += 1
elif opt_options.vary_position_over_face:
particle_index += 2
if opt_options.map_to_flat_surface:
surf_point, cameras[i].wall_normal, on_ceiling, surf_id = \
environment.map_particle_to_flattened_surface(particle[i * ppc:i*ppc+particle_index],
surface_index=surf_index)
if np.isnan(surf_point).any():
cameras[i].pose[:3] *= np.nan
break
else:
surf_point, cameras[i].wall_normal, on_ceiling, surf_id = \
environment.map_particle_to_surface(particle[i * ppc:i*ppc+particle_index],
surface_index=surf_index)
R = rot3d_from_z_vec(cameras[i].wall_normal)
cameras[i].pose[0:3] = surf_point + np.dot(R, cameras[i].camera_offset)
else: # DEPRECATED
edge_perimeter = environment.perch_perimeter
if opt_options.perch_on_ceiling:
cameras[i].pose[0:3], cameras[i].wall_normal, on_ceiling = \
environment.map_particle_to_surface(particle=particle, start_index=i*ppc+particle_index)
particle_index = particle_index + 3
else:
cameras[i].pose[0:2], cameras[i].wall_normal = \
map_line_to_edge(environment, particle[i*ppc + particle_index] * edge_perimeter)
on_ceiling = False
# vertical position is already covered by perch on ceiling if it is active. only include height
if opt_options.variable_height and not opt_options.perch_on_ceiling:
cameras[i].pose[2] = convert_particle_to_height(particle[i*ppc + particle_index], on_ceiling,
environment)
else:
cameras[i].pose[2] = 2 # constant height. typical ceiling is ~2.75m high; set in environment.max_height
# Recall: the camera's axes are X=RIGHT, Y=DOWN, Z=FORWARD; (before was erroneously using X=fwd, Y=left, z=up)
# therefore camera PAN is about Y, TILT is about X, ROLL is about Z
if opt_options.variable_pan:
cameras[i].pose[-2] = -convert_particle_to_angle(particle_angle=particle[i * ppc + particle_index],
wall_normal=cameras[i].wall_normal, camera=cameras[i],
axis=2, environment=environment, opt_options=opt_options)
particle_index = particle_index + 1
else:
# passing 0.5 in as particle value and enabling target_center better respects angle mode (either point
# normal to wall, or towards target) without adding search complexity
cameras[i].pose[-2] = -convert_particle_to_angle(0.5, wall_normal=cameras[i].wall_normal,
camera=cameras[i], axis=2, environment=environment,
opt_options=opt_options, target_center=True)
if opt_options.variable_tilt:
cameras[i].pose[-3] = -convert_particle_to_angle(particle_angle=particle[i * ppc + particle_index],
wall_normal=cameras[i].wall_normal, camera=cameras[i],
axis=1, environment=environment, opt_options=opt_options)
particle_index = particle_index + 1
else:
# passing 0.5 in as particle value and enabling target_center better respects angle mode (either point
# normal to wall, or towards target) without adding search complexity
cameras[i].pose[-3] = -convert_particle_to_angle(0.5, wall_normal=cameras[i].wall_normal,
camera=cameras[i], axis=1, environment=environment,
opt_options=opt_options, target_center=True)
if opt_options.variable_zoom:
cameras[i].fov = convert_particle_to_zoom(particle[i * ppc + particle_index], cameras[i])
# particle_index = particle_index + 1 # re-add if more options are added in the future
else:
cameras[i].fov = cameras[i].base_fov
# for the time being, never add any roll. Could consider adding roll as "noise"... might want noise resistance
cameras[i].pose[-1] = 0
cameras[i].surf_id = surf_id
# if debug:
# print("Camera: " + str(i))
# print("Particle: " + str(particle[i*2:i*2+2]))
# print("FOV: " + str(cameras[i].fov[0]))
# print("Edge Normal Direction: " + str(cameras[i].wall_normal))
# print("Camera Pose: " + str(cameras[i].pose))
return cameras
def evaluate_particle(particle, environment, cameras, placed_cameras=PlacedCameras(),
opt_options=CameraPlacementOptions(), debug=False, vedo_plt=None, maximization=False):
"""
This function evaluates a single particle using the heuristic function defined in evaluate_arrangement()
:param particle: individual particle as np.array
:param environment: Environment class instance
:param cameras: a list of Camera objects corresponding to the cameras which have already been placed in an
environment
:param placed_cameras: PlacedCameras class instance
:param opt_options: CameraPlacementOptions class containing opt opt
:param debug: flag, if true print and draw relevant information.
:return: score of particle
"""
# convert particle into camera pose; evaluate edge normal at each camera position
cameras = convert_particle_to_state(environment=environment, particle=particle, cameras=cameras,
opt_options=opt_options, debug=debug)
# plot particles to debug...
# if debug:
# if environment.dimension == 2:
# plot_particle_2d(particle=np.squeeze(particle), environment=environment, cameras=cameras,
# placed_cameras=placed_cameras, view_time=0.0001, figure_number=figure_number)
# else:
# # if len(placed_cameras.cameras) > 0:
# plot_particle_3d(particle=particle, environment=environment, cameras=cameras, placed_cameras=placed_cameras,
# view_time=0.0001, figure_number=figure_number)
score = evaluate_arrangement_covariance(environment=environment, cameras=cameras, placed_cameras=placed_cameras,
debug=debug, maximization=maximization)
if vedo_plt is not None:
if len(placed_cameras.cameras) >= 10:
geom_list = []
for i in range(len(cameras)):
pymesh_frustum = cameras[i].generate_discrete_camera_mesh(degrees_per_step=5, environment=environment)
if len(pymesh_frustum.faces) > 0 and not np.isnan(pymesh_frustum.vertices).any():
pymesh_verts = pymesh_frustum.vertices.copy()
pymesh_verts.flags.writeable = True
pymesh_faces = pymesh_frustum.faces.copy()
pymesh_faces.flags.writeable = True
frustum = trimesh.Trimesh(vertices=pymesh_frustum.vertices.copy(),
faces=pymesh_frustum.faces.copy())
vedo_frustum = vedo.mesh.Mesh(frustum)
vedo_frustum.alpha(0.2)
vedo_frustum.color("c")
quad_mesh = trimesh.load(
"/home/simon/catkin_ws/src/perch_placement/src/ui/models/white-red-black_quad2.ply")
R = rot3d_from_x_vec(cameras[i].wall_normal)
R2 = rot3d_from_rtp(np.array([0, -90, 0]))
R_aug = np.zeros([4, 4])
R_aug[:3, :3] = R.dot(R2)
R_aug[:3, -1] = cameras[i].pose[:3]
quad_mesh.vertices = trimesh.transform_points(quad_mesh.vertices, R_aug)
quad_mesh_vedo = vedo.mesh.Mesh(quad_mesh)
quad_mesh_vedo.cellIndividualColors(quad_mesh.visual.face_colors / 255, alphaPerCell=True)
geom_list.append(quad_mesh_vedo)
geom_list.append(vedo_frustum)
for actor in geom_list:
vedo_plt.add(actor)
# p_.camera_position = [
# (R * np.cos(t), R * np.sin(t), z),
# (c[0], c[1], c[2]), # (-0.026929191045848594, 0.5783514020506139, 0.8268966663940324),
# (0, 0, 1),
# ]
vedo_plt.camera.SetPosition(7*np.cos(-145*np.pi/180.0), 7*np.sin(-145*np.pi/180.0), 6.25)
vedo_plt.camera.SetFocalPoint(-0.026929191045848594, 0.5783514020506139, 0.9268966663940324)
vedo_plt.camera.SetViewUp(np.array([0, 0, 1]))
vedo_plt.camera.SetDistance(7.8)
vedo_plt.camera.SetClippingRange([0.25, 30])
vedo_plt.camera
vedo_plt.show(interactive=False, rate=30, resetcam=False, fullscreen=True)
time.sleep(0.5)
actors = vedo_plt.actors
for i in range(len(cameras)):
vedo_plt.remove(actors.pop())
vedo_plt.remove(actors.pop())
# plot_particle_3d(particle=particle, environment=environment, cameras=cameras, placed_cameras=placed_cameras,
# view_time=0.0001, figure_number=0)
# print("Particle score: " + str(score) + "; Pose: " + str(cameras[0].pose))
if debug:
print(score)
return score
def evaluate_swarm(x, environment, cameras, placed_cameras=PlacedCameras(),
opt_options=CameraPlacementOptions(), debug=False, figure_number=0, vedo_plt=None,
maximization=False):
"""
The main function used by PySwarms to evaluate particles corresponding with camera placement
:param x: the list of particles returned by PySwarms for the current iteration
:param environment: Environment Class instance containing the environment in which to place cameras
:param cameras: list of cameras, in order of which they are placed
:param placed_cameras: PlacedCameras class instance
:param opt_options: CameraPlacementOptions class containing opt opt
:param debug: flag, if true print and draw relevant information.
:param figure_number: figure window number on which to plot placements
:return: particle scores
"""
particles = np.copy(x)
n_particles = np.shape(particles)[0] # extract swarm dimensions
# pts_searched = 0
# fitness_deviation = 0
# max_fitness = np.finfo(float).max
if not opt_options.continue_searching:
# print("would stop here")
return np.ones(n_particles)*np.finfo(float).max/2
else:
# Check particle variance
if opt_options.minimum_particle_deviation is not None and opt_options.minimum_particle_deviation > 0:
deviations = np.std(particles, axis=0)
# print("Particle Deviations: " + str(deviations))
if (deviations < opt_options.minimum_particle_deviation).all():
# print("Ending search early. Particle deviation too low.")
opt_options.continue_searching = False
# print("Would stop here")
return np.ones(n_particles)*np.finfo(float).max/2
results = np.ones([n_particles, 1])*np.inf # start with max possible score...
# print("There are " + str(len(placed_cameras.cameras)) + "placed cameras")
for p in range(n_particles):
results[p] = evaluate_particle(particle=particles[p, :], environment=environment, cameras=cameras,
placed_cameras=placed_cameras, opt_options=opt_options, debug=debug,
vedo_plt=vedo_plt, maximization=maximization)
# if opt_options.noise_resistant_particles > 0:
# n_top = min(opt_options.noise_resistant_particles, n_particles)
# top_particles = np.argpartition(np.squeeze(results), -n_top)[-n_top:]
# environment.noise_resistant_search = True
# for p in top_particles:
# results[p] = evaluate_particle(particle=particles[p, :], environment=environment, cameras=cameras,
# placed_cameras=placed_cameras, opt_options=opt_options, debug=debug,
# maximization=maximization)
# environment.noise_resistant_search = False
if opt_options.log_performance:
c = len(placed_cameras.cameras)
i = opt_options.iteration
j = opt_options.data_index + 1
opt_options.search_time[i, c, j] = time.time()
# opt_options.fitness_deviation[i, c, j] = np.nanstd(results)
opt_options.best_fitness[i, c, j] = np.nanmin(results)
opt_options.pts_searched[i, c, j] = n_particles
opt_options.data_index += 1
if debug:
if environment.dimension == 2:
plot_particles_in_2d_search_space(particles, figure_number)
# if environment.opt_options.map_to_flat_surface:
# plot_particles_on_2D_surface(particles, surf=environment.perch_regions[environment.opt_options.surface_number],
# figure_num=1, view_time=0.1, persistent_points=False)
# fig = plt.figure(1)
# ax = Axes3D(fig)
# plot_particles_on_3D_surface(particles, environment=environment, surf_id=environment.opt_options.surface_number,
# ax=ax, view_time=0.1, persistent_points=False, ppc=opt_options.get_particle_size())
if opt_options.minimum_score_deviation is not None and opt_options.minimum_score_deviation > 0:
deviation = np.std(results)
# print("Score Deviation: " + str(deviation))
if (deviation < opt_options.minimum_score_deviation).all():
opt_options.stagnant_loops += 1
# print("Score variation too low... (# consecutive stagnant loops = " +
# str(opt_options.stagnant_loops) + ")")
# print("Scores: " + str(results.reshape([-1])))
if opt_options.stagnant_loops >= opt_options.max_stagnant_loop_count:
# print("Ending search early. Score deviation too low.")
opt_options.continue_searching = False
else:
opt_options.stagnant_loops = 0
# return np.squeeze(results)
return results.reshape([-1])
def ply_environment(file_path_prototype,
target_path_prototype=None,
cluster_env_path=None,
optimization_options=CameraPlacementOptions(),
reorient_mesh=False,
full_env_path=None,
N_points=50): # , surface_number=-1):
rospy.loginfo(rospy.get_caller_id() + " Importing mesh environment")
t0 = time.time()
if cluster_env_path is not None:
cluster_env = import_ply(cluster_env_path,
post_process=False,
options=optimization_options,
is_plane_mesh=False)
if optimization_options.nearest_neighbor_restriction:
# use color codes to identify unique clusters of faces
colors, clusters, cluster_ids = np.unique(cluster_env.face_colors,
axis=0,
return_index=True,
return_inverse=True)
# make a map between point IDs and faces
point_face_map = [[] for i in range(cluster_env.num_points)]
for f in range(cluster_env.num_faces):
for i in range(3):
point_face_map[cluster_env.faces[f, i]].append(f)
tree = KDTree(
cluster_env.points) # make tree with set of unique points
kdt_data = KDTreeData(tree, colors, clusters, cluster_ids,
cluster_env.points, cluster_env.faces,
point_face_map)
else:
kdt_data = None
t1 = time.time()
print("Preprocessing the cluster environment takes: " + str(t1 - t0))
t0 = time.time()
# cluster_env_remeshed = pymesh.form_mesh(cluster_env.points, cluster_env.faces)
# cluster_env_remeshed, _ = pymesh.split_long_edges(cluster_env_remeshed, optimization_options.dist_threshold)
# cluster_env_remeshed = Mesh(cluster_env_remeshed.vertices, cluster_env_remeshed.faces,
# options=optimization_options) # TODO: REMOVE
cluster_env_remeshed = copy.deepcopy(
cluster_env) # BANDAID SPEEDUP PRIOR TO PROPER REMOVAL.
t1 = time.time()
print("Remeshing the cluster environment takes: " + str(t1 - t0))
t0 = time.time()
else:
cluster_env = None
kdt_data = None
cluster_env_remeshed = None
surfaces = []
surf_files = glob.glob(file_path_prototype + "*")
# print("Surf Files:")
# print(str(surf_files))
for fp in range(len(surf_files)):
# print("Processing: " + str(fp))
mesh = import_ply(surf_files[fp],
post_process=False,
options=optimization_options,
kdt_data=kdt_data)
if mesh.faces is None or mesh.points is None:
print("PROBLEM!")
if mesh.is_valid:
surfaces.append(mesh)
else:
warnings.warn("Warning: mesh " + str(surf_files[fp]) +
" omitted due to invalid geometry.")
if len(surfaces) == 0:
surfaces = None
t1 = time.time()
print("Loading min area surfaces took: " + str(t1 - t0))
t0 = time.time()
if target_path_prototype is not None and target_path_prototype != "":
surf_files = glob.glob(target_path_prototype)
targets = []
for fp in surf_files:
# print("Processing: " + str(fp))
mesh = import_ply(fp,
post_process=False,
options=optimization_options)
if mesh.is_valid:
targets.append(mesh)
else:
warnings.warn("Warning: mesh " + str(fp) +
" omitted due to invalid geometry.")
if len(targets) > 0:
target = targets[0]
for t in range(len(targets)):
if t != 0:
target += targets[t]
else:
target = None
else:
target = None
t1 = time.time()
print("Loading target mesh took: " + str(t1 - t0))
t0 = time.time()
if full_env_path is not None:
full_env = import_ply(full_env_path, is_plane_mesh=False)
else:
full_env = None
t1 = time.time()
print("Loading the full mesh environment took: " + str(t1 - t0))
env = MeshEnvironment(surfaces=surfaces,
n_points=N_points,
target=target,
obstacles=None,
clust_env=cluster_env,
full_env=full_env,
opt_options=optimization_options,
reorient_mesh=reorient_mesh,
cluster_env_remeshed=cluster_env_remeshed,
full_env_path=full_env_path,
clust_env_path=cluster_env_path)
return env
def environment_1(obstructions=True,
target="A",
dimension=2,
optimization_options=CameraPlacementOptions()):
height = 2.75
room_coords = np.array([[20, 10], [20, 20], [10, 20], [10, 15], [5, 15],
[5, 10]])
ceiling_coords = room_coords
perch_areas = np.array([[[5.5, 10], [10.75, 10]], [[13.5, 10], [18.75,
10]],
[[20, 10.25], [20, 18.75]],
[[19.75, 20], [10.5, 20]],
[[10, 19.25], [10, 15.25]], [[7.5, 15], [5.25,
15]],
[[5, 14.75], [5, 10.5]]])
if target == "A": # center of main room portion
target_coords = np.array([[13, 14], [15, 14], [14, 16], [12, 17]])
elif target == "B": # center of side annex
target_coords = np.array([[6, 12], [8, 12], [7, 14], [5.5, 14.5]])
elif target == "C": # hugging bottom right corner
target_coords = np.array([[18, 10.5], [19.5, 11], [19, 11.5],
[18.5, 12]])
else: # entire room is target
target_coords = room_coords
obstruction1 = np.array([[10, 9.98], [20.02, 9.98], [20.02, 20.02],
[9.98, 20.02], [9.98, 15.02], [4.98, 15.02],
[4.98, 9.98]])
obstruction2 = np.array([[13, 12], [15, 12.5], [15, 13.], [13, 12.75]])
obstruction3 = np.array([[13.25, 18], [14, 18], [14, 18.5], [13.25, 18.5]])
if obstructions:
obstruction_polygons = [obstruction1, obstruction2, obstruction3]
else:
obstruction_polygons = [obstruction1] # room walls only
using_o3d = False #
if dimension == 3:
# IN 3D, DEFINE EVERYTHING AS OPEN3D POINT CLOUD
ceiling_coords = add_z_axis(room_coords, height)
target_coords = add_z_axis(target_coords, 0)
# convert ceiling to point cloud
if using_o3d:
ceiling = o3d.geometry.PointCloud()
ceiling.points = o3d.utility.Vector3dVector(ceiling_coords)
ceiling_coords = ceiling
ceiling_poly = convert_PointCloud_to_Mesh(ceiling_coords)
# visualize_o3d(ceiling_coords)
visualize_o3d(ceiling_poly)
room_coords = convert_2D_poly_list_to_PointCloud(
room_coords, height)
perch_areas = convert_2D_poly_list_to_PointCloud(
perch_areas, height)
obstruction1 = convert_2D_poly_list_to_PointCloud(
obstruction1, height)
obstruction2 = convert_2D_poly_list_to_PointCloud(
obstruction2, height)
obstruction3 = convert_2D_poly_list_to_PointCloud(
obstruction3, height)
else:
room_coords = convert_2D_Poly_to_3D_Walls(room_coords, height)
pa = []
for i in range(len(perch_areas)):
pa.extend(
convert_2D_Poly_to_3D_Walls_PA_ONLY(
perch_areas[i, :, :], height))
perch_areas = pa
obstruction1 = convert_2D_Poly_to_3D_Walls(obstruction1, height)
obstruction2 = convert_2D_Poly_to_3D_Walls(obstruction2, height)
obstruction3 = convert_2D_Poly_to_3D_Walls(obstruction3, height)
# compile all obstructions into a single list
if obstructions:
obstruction2.extend(obstruction3)
obstruction1.extend(obstruction2)
obstruction_polygons = obstruction1
else:
obstruction_polygons = obstruction1 # room walls only
env = Environment(walls=room_coords,
n_points=100,
dimension=dimension,
target=target_coords,
obstacles=obstruction_polygons,
perch_regions=perch_areas,
opt_options=optimization_options,
ceiling=ceiling_coords)
return env