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 = []