def __init__(self, periscope, p_data):
"""
Setup the vision device using data from the supplied xml file.
"""
##get periscope properties from the dictionary returned by the api
datums = p_data[periscope]["datums"]
properties = p_data[periscope]["properties"]
self.name = periscope
##datums
self.trans_glass = datums["EXT_PSCOPE_OUTER"]["global"]
self.trans_inner_glass = datums["EXT_PSCOPE_INNER"]["global"]
##properties
self.focal_distance = float(properties["focal_distance"])
self.fv_left = float(properties["field_of_view_angle_left"])
self.fv_right = float(properties["field_of_view_angle_right"])
self.fv_down = float(properties["field_of_view_angle_down"])
self.fv_up = float(properties["field_of_view_angle_up"])
## Get transform to focal point in vehicle space and it's inverse
self.focal_point_trans = np.dot(
self.trans_glass, translate(np.array([0, 0,
-self.focal_distance])))
self.focal_point_trans_inv = np.linalg.inv(self.focal_point_trans)
## Axis to rotate around to scan horizontally from focal point
self.lens_hor_axis = np.array([0, 1, 0, 1])
def __init__(self, periscope, p_data):
"""
Setup the vision device using data from the supplied xml file.
"""
##get periscope properties from the dictionary returned by the api
datums = p_data[periscope]["datums"]
properties = p_data[periscope]["properties"]
self.name = periscope
##datums
self.trans_glass = datums["EXT_PSCOPE_OUTER"]["global"]
self.trans_inner_glass = datums["EXT_PSCOPE_INNER"]["global"]
##properties
self.focal_distance = float(properties["focal_distance"])
self.fv_left = float(properties["field_of_view_angle_left"])
self.fv_right = float(properties["field_of_view_angle_right"])
self.fv_down = float(properties["field_of_view_angle_down"])
self.fv_up = float(properties["field_of_view_angle_up"])
## Get transform to focal point in vehicle space and it's inverse
self.focal_point_trans = np.dot(self.trans_glass, translate(np.array([0, 0, -self.focal_distance])))
self.focal_point_trans_inv = np.linalg.inv(self.focal_point_trans)
## Axis to rotate around to scan horizontally from focal point
self.lens_hor_axis = np.array([0, 1, 0, 1])
def _is_target_point_visible(self, target_point_trans, c_periscope):
"""
Return True if the ``target_point_trans`` is visible from the current view device.
"""
## Target point in the focal point coord system
t_pt_foc = get_translation(c_periscope.focal_point_trans_inv, target_point_trans)
## Calculate angle to target w.r.t to periscope view axis
h_ang = atan2(t_pt_foc[0], t_pt_foc[2])
rad_in_plane = sqrt(t_pt_foc[0] * t_pt_foc[0] + t_pt_foc[2] * t_pt_foc[2])
v_ang = atan2(-t_pt_foc[1], rad_in_plane)
## Transform to project forward from the focal point to be slightly in front of the glass
lens_dist = translate([0.0, 0.0, c_periscope.focal_distance * self.focal_length_multiplier, 1.0])
## Check if within the periscope view frustrum
if c_periscope.is_ray_within_frustrum(h_ang, v_ang):
## Calculate the lens offset point
tran_hor = rotation_about_vector(c_periscope.lens_hor_axis, h_ang)
tran_vert = rotation_about_vector(mul(tran_hor, c_periscope.lens_hor_axis), v_ang)
lens_point = get_translation(c_periscope.focal_point_trans, tran_vert, tran_hor, lens_dist)
## Actually do the intersection here
t = self.b_tree.get_line_intersection(lens_point, get_translation(target_point_trans))
if t < 0:
## Visible
return True
## Not visible
return False
def _is_target_point_visible(self, target_point_trans, c_periscope):
"""
Return True if the ``target_point_trans`` is visible from the current view device.
"""
## Target point in the focal point coord system
t_pt_foc = get_translation(c_periscope.focal_point_trans_inv,
target_point_trans)
## Calculate angle to target w.r.t to periscope view axis
h_ang = atan2(t_pt_foc[0], t_pt_foc[2])
rad_in_plane = sqrt(t_pt_foc[0] * t_pt_foc[0] +
t_pt_foc[2] * t_pt_foc[2])
v_ang = atan2(-t_pt_foc[1], rad_in_plane)
## Transform to project forward from the focal point to be slightly in front of the glass
lens_dist = translate([
0.0, 0.0,
c_periscope.focal_distance * self.focal_length_multiplier, 1.0
])
## Check if within the periscope view frustrum
if c_periscope.is_ray_within_frustrum(h_ang, v_ang):
## Calculate the lens offset point
tran_hor = rotation_about_vector(c_periscope.lens_hor_axis, h_ang)
tran_vert = rotation_about_vector(
mul(tran_hor, c_periscope.lens_hor_axis), v_ang)
lens_point = get_translation(c_periscope.focal_point_trans,
tran_vert, tran_hor, lens_dist)
## Actually do the intersection here
t = self.b_tree.get_line_intersection(
lens_point, get_translation(target_point_trans))
if t < 0:
## Visible
return True
## Not visible
return False
def trace_rays(self):
"""
Trace the rays to calculate the raw hit points for each device.
"""
for p, periscope in enumerate(self.periscopes):
hit = []
hor_fan = []
hor_fan.append(get_translation(periscope.trans_glass))
dist_ver = get_translation(
periscope.trans_glass)[1] - self.z_ground
## Sweep around the global up direction to find horizontal view percent
for i, ang_hor in enumerate(self.hor_sweep):
## Global transform to target point
tran_hor_world = rotation_about_vector(self.up_direction,
ang_hor)
target_point_trans = mul(
self.tran_veh, tran_hor_world,
translate(np.array([self.far_dist, 0, 0])),
translate(np.array([0, dist_ver, 0])))
if self._is_target_point_visible(target_point_trans,
periscope):
self.target_points_horizon[i] += 2**p
hor_fan.append(get_translation(target_point_trans))
else:
hor_fan.append(get_translation(periscope.trans_glass))
self.hor_fans.append(hor_fan)
## Find the highest visible point in front of the vehicle
max_uplook = 50
accuracy = 0.001
self.uplook[p] = 0.0
tran_hor_world = rotation_about_vector(self.up_direction, pi)
upper_uplook = max_uplook
lower_uplook = 0.0
while (upper_uplook - lower_uplook) > accuracy:
## Global transform to target point
height = (upper_uplook + lower_uplook) * 0.5
target_point_trans = mul(
self.tran_veh, tran_hor_world,
translate(np.array([0.0, height, 50.0])))
if self._is_target_point_visible(target_point_trans,
periscope):
self.uplook[p] = height
lower_uplook = height
else:
upper_uplook = height
if self.uplook[p] > 0.0:
hit.extend((get_translation(target_point_trans),
get_translation(target_point_trans),
get_translation(periscope.trans_glass)))
## Find the closest visible ground point fore and aft
max_radius = 2e6
self.fore_aft[p] = [max_radius, max_radius]
for i, rot in enumerate([pi, 0.0]):
tran_hor_world = rotation_about_vector(self.up_direction, rot)
upper_radius = max_radius
lower_radius = 0.0
while (upper_radius - lower_radius) > accuracy:
## Global transform to target point
radius = (upper_radius + lower_radius) * 0.5
target_point_trans = mul(
self.tran_veh, tran_hor_world,
translate(np.array([0, 0, radius])))
if self._is_target_point_visible(target_point_trans,
periscope):
self.fore_aft[p][i] = radius
upper_radius = radius
else:
lower_radius = radius
if self.fore_aft[p][i] < max_radius * 0.5:
hit.extend((get_translation(target_point_trans),
get_translation(target_point_trans),
get_translation(periscope.trans_glass)))
self.hit.append(hit)
def __init__(self, settings_f):
"""
Setup vision metrics based on settings read in from file.
"""
##pass setting to the test bench api
settings_dict = tba.load_settings(settings_f)
## Optional test bench settings
self.show_3d = settings_dict.get("show_3d", False)
horizontal_divs = settings_dict.get("horizontal_divs", 720)
self.focal_length_multiplier = settings_dict.get(
"focal_length_multiplier", 1.5)
self.far_dist = settings_dict.get("far_dist", 20.0)
self.up_direction = settings_dict.get("up_direction",
np.array([0, 1.0, 0]))
self.fore_direction = settings_dict.get("fore_direction",
np.array([0, 0, -1.0]))
self.ground = settings_dict.get("ground", np.array([0, 1.0, 0]))
## Set up some constant parameters
self.hor_sweep = np.linspace(0, 2 * pi, horizontal_divs)
self.target_points_horizon = np.zeros(horizontal_divs, np.uint32)
## Get a dictionary of parts and properties form the api (periscopes, manikins, screens)
fov_data = tba.get_data(parts_of_interest)
## Get Periscope objects for any that are found
self.periscopes = [
Vision_Device(p, fov_data["Periscope"])
for p in fov_data["Periscope"]
]
## Get Manikin objects for the vehicle occupants that have vision requirements
m_data = fov_data["Manikin"]
req_roles = ["driver", "vehicle_commander", "troop_commander"]
vis_roles = lambda m: m_data[m]["properties"]["vehicle_role"
] in req_roles
self.manikins = [
Manikin(m, fov_data["Manikin"]) for m in m_data if vis_roles(m)
]
## Need exactly one of each
roles = [m.vehicle_role for m in self.manikins]
if len(set(roles)) != len(req_roles):
msg = "Didn't get 1 of each vehicle_role={} instead got={}".format(
req_roles, roles)
raise ValueError(msg)
## Specify the classes of objects we want geometry for.
class_set = tba.get_all_geom_set(
) - tba.geom_sets["never_exterior_classes"]
## And load them all as an effective single file
surface = tba.load_geometry(class_set, single_file=True)
## Make a binary space partition to speed up intersection calculations
self.nodes = np.vstack((surface['x'], surface['y'], surface['z'])).T
self.tris = surface['tris']
self.b_tree = BSP_Tree(self.nodes, self.tris)
self.b_tree.generate_tree()
## TODO this won't cope with unusual orientations
## determine orientation of the vehicle given the direction up and forward
fore = np.dot(self.nodes, self.fore_direction).min()
fore = np.min(surface["z"])
self.side_direction = np.cross(self.up_direction, self.fore_direction)
side_array = np.dot(self.nodes, self.side_direction)
self.z_ground = np.dot(self.ground, self.up_direction)
center = (side_array.min() + side_array.max()) / 2.0
center = (np.min(surface["x"]) + np.max(surface["x"])) * 0.5
## Find the vehicle origin (point on ground at front on centerline)
self.veh_origin = np.array([center, self.z_ground, fore])
logging.info("Vehicle origin is at {}".format(self.veh_origin))
self.tran_veh = translate(self.veh_origin)
self.fore_aft = [None] * len(self.periscopes)
self.uplook = np.array([-1000.0] * len(self.periscopes))
self.hit = []
self.hor_fans = []
def trace_rays(self):
"""
Trace the rays to calculate the raw hit points for each device.
"""
for p, periscope in enumerate(self.periscopes):
hit = []
hor_fan = []
hor_fan.append(get_translation(periscope.trans_glass))
dist_ver = get_translation(periscope.trans_glass)[1] - self.z_ground
## Sweep around the global up direction to find horizontal view percent
for i, ang_hor in enumerate(self.hor_sweep):
## Global transform to target point
tran_hor_world = rotation_about_vector(self.up_direction, ang_hor)
target_point_trans = mul(
self.tran_veh,
tran_hor_world,
translate(np.array([self.far_dist, 0, 0])),
translate(np.array([0, dist_ver, 0])),
)
if self._is_target_point_visible(target_point_trans, periscope):
self.target_points_horizon[i] += 2 ** p
hor_fan.append(get_translation(target_point_trans))
else:
hor_fan.append(get_translation(periscope.trans_glass))
self.hor_fans.append(hor_fan)
## Find the highest visible point in front of the vehicle
max_uplook = 50
accuracy = 0.001
self.uplook[p] = 0.0
tran_hor_world = rotation_about_vector(self.up_direction, pi)
upper_uplook = max_uplook
lower_uplook = 0.0
while (upper_uplook - lower_uplook) > accuracy:
## Global transform to target point
height = (upper_uplook + lower_uplook) * 0.5
target_point_trans = mul(self.tran_veh, tran_hor_world, translate(np.array([0.0, height, 50.0])))
if self._is_target_point_visible(target_point_trans, periscope):
self.uplook[p] = height
lower_uplook = height
else:
upper_uplook = height
if self.uplook[p] > 0.0:
hit.extend(
(
get_translation(target_point_trans),
get_translation(target_point_trans),
get_translation(periscope.trans_glass),
)
)
## Find the closest visible ground point fore and aft
max_radius = 2e6
self.fore_aft[p] = [max_radius, max_radius]
for i, rot in enumerate([pi, 0.0]):
tran_hor_world = rotation_about_vector(self.up_direction, rot)
upper_radius = max_radius
lower_radius = 0.0
while (upper_radius - lower_radius) > accuracy:
## Global transform to target point
radius = (upper_radius + lower_radius) * 0.5
target_point_trans = mul(self.tran_veh, tran_hor_world, translate(np.array([0, 0, radius])))
if self._is_target_point_visible(target_point_trans, periscope):
self.fore_aft[p][i] = radius
upper_radius = radius
else:
lower_radius = radius
if self.fore_aft[p][i] < max_radius * 0.5:
hit.extend(
(
get_translation(target_point_trans),
get_translation(target_point_trans),
get_translation(periscope.trans_glass),
)
)
self.hit.append(hit)
def __init__(self, settings_f):
"""
Setup vision metrics based on settings read in from file.
"""
##pass setting to the test bench api
settings_dict = tba.load_settings(settings_f)
## Optional test bench settings
self.show_3d = settings_dict.get("show_3d", False)
horizontal_divs = settings_dict.get("horizontal_divs", 720)
self.focal_length_multiplier = settings_dict.get("focal_length_multiplier", 1.5)
self.far_dist = settings_dict.get("far_dist", 20.0)
self.up_direction = settings_dict.get("up_direction", np.array([0, 1.0, 0]))
self.fore_direction = settings_dict.get("fore_direction", np.array([0, 0, -1.0]))
self.ground = settings_dict.get("ground", np.array([0, 1.0, 0]))
## Set up some constant parameters
self.hor_sweep = np.linspace(0, 2 * pi, horizontal_divs)
self.target_points_horizon = np.zeros(horizontal_divs, np.uint32)
## Get a dictionary of parts and properties form the api (periscopes, manikins, screens)
fov_data = tba.get_data(parts_of_interest)
## Get Periscope objects for any that are found
self.periscopes = [Vision_Device(p, fov_data["Periscope"]) for p in fov_data["Periscope"]]
## Get Manikin objects for the vehicle occupants that have vision requirements
m_data = fov_data["Manikin"]
req_roles = ["driver", "vehicle_commander", "troop_commander"]
vis_roles = lambda m: m_data[m]["properties"]["vehicle_role"] in req_roles
self.manikins = [Manikin(m, fov_data["Manikin"]) for m in m_data if vis_roles(m)]
## Need exactly one of each
roles = [m.vehicle_role for m in self.manikins]
if len(set(roles)) != len(req_roles):
msg = "Didn't get 1 of each vehicle_role={} instead got={}".format(req_roles, roles)
raise ValueError(msg)
## Specify the classes of objects we want geometry for.
class_set = tba.get_all_geom_set() - tba.geom_sets["never_exterior_classes"]
## And load them all as an effective single file
surface = tba.load_geometry(class_set, single_file=True)
## Make a binary space partition to speed up intersection calculations
self.nodes = np.vstack((surface["x"], surface["y"], surface["z"])).T
self.tris = surface["tris"]
self.b_tree = BSP_Tree(self.nodes, self.tris)
self.b_tree.generate_tree()
## TODO this won't cope with unusual orientations
## determine orientation of the vehicle given the direction up and forward
fore = np.dot(self.nodes, self.fore_direction).min()
fore = np.min(surface["z"])
self.side_direction = np.cross(self.up_direction, self.fore_direction)
side_array = np.dot(self.nodes, self.side_direction)
self.z_ground = np.dot(self.ground, self.up_direction)
center = (side_array.min() + side_array.max()) / 2.0
center = (np.min(surface["x"]) + np.max(surface["x"])) * 0.5
## Find the vehicle origin (point on ground at front on centerline)
self.veh_origin = np.array([center, self.z_ground, fore])
logging.info("Vehicle origin is at {}".format(self.veh_origin))
self.tran_veh = translate(self.veh_origin)
self.fore_aft = [None] * len(self.periscopes)
self.uplook = np.array([-1000.0] * len(self.periscopes))
self.hit = []
self.hor_fans = []
