def myConstructor(self, start_node): self.Start_node = start_node self.RootNode_ = avango.gua.nodes.TransformNode( Name = "ConeTreeRoot", ) self.ScaleNode_ = avango.gua.nodes.TransformNode( Name = "ConeTreeScale", ) self.TransformNode_ = avango.gua.nodes.TransformNode( Name = "ConeTreeTransform", ) self.RootCone_ = Cone(self.Start_node, None) self.FocusCone_ = self.RootCone_ avango.gua.load_materials_from("data/materials") self.FocusEdge_ = -1 self.layout()
def read_data(self): data_files = os.listdir(self.datadir) data_files.remove('data.json') dataloc = op.join(self.datadir, 'data.json') for det in data_files: detpath = op.join(self.datadir, det) detdf = pd.read_json(detpath) self.detectors[detdf.at['ID', 'value']] = detdf self.data = pd.read_json(dataloc) self.data['Detector'] = self.data['ID'].str.split('_').apply( lambda row: row[0]) # Organise data in cones row = self.data.loc[self.data['ID'] == self.coneID].squeeze() thiscone = Cone(self, row['ID'], row) self.cones[row['ID']] = thiscone
def find_velocity(agent, RVOi, apexes, v_pref, max_vel): #Assumes all agents and obstacles are convex cones = [] for apex, points in zip(apexes, RVOi): if len(points) > 0: min_angle = math.pi max_angle = -math.pi min_point = points[0] max_point = points[0] angles = [] u = [(points[0][0] - apex[0]), (points[0][1] - apex[1])] for point in points: moved_point = [(point[0] - apex[0]), (point[1] - apex[1])] angle_dif = angle(u, moved_point) angles.append(angle_dif) if angle_dif < min_angle: min_angle = angle_dif min_point = point if angle_dif > max_angle: max_angle = angle_dif max_point = point cones.append(Cone.Cone(apex, min_point, max_point)) within_VO = False for cone in cones: if cone.contains(v_pref): within_VO = True if not within_VO: return v_pref, cones, [] else: potential_v_left = [] potential_v_right = [] all_v = [] for i in range(20, 1, -1): for j in range(0, 121): new_v = rotate(v_pref, j / 120 * math.pi) magnitude = math.sqrt(new_v[0]**2 + new_v[1]**2) new_v_normalised = (new_v[0] / magnitude, new_v[1] / magnitude) new_v = [ new_v_normalised[0] * i * max_vel / 20, new_v_normalised[1] * i * max_vel / 20 ] new_v_loc = [agent.p[0] + new_v[0], agent.p[1] + new_v[1]] within_VO = False for cone in cones: if cone.contains(new_v): within_VO = True if not within_VO: potential_v_left.append(new_v) all_v.append(new_v) new_v = rotate(v_pref, -j / 120 * math.pi) magnitude = math.sqrt(new_v[0]**2 + new_v[1]**2) new_v_normalised = (new_v[0] / magnitude, new_v[1] / magnitude) new_v = [ new_v_normalised[0] * i * max_vel / 20, new_v_normalised[1] * i * max_vel / 20 ] new_v_loc = [agent.p[0] + new_v[0], agent.p[1] + new_v[1]] within_VO = False for cone in cones: if cone.contains(new_v): within_VO = True if not within_VO: potential_v_right.append(new_v) all_v.append(new_v) potential_v = potential_v_left + potential_v_right if len(potential_v) > 0: new_v = potential_v[0] new_v_dist = dist(new_v, v_pref) for v in potential_v: v_dist = dist(v, v_pref) v_angle = angle(agent.v, v) if new_v_dist > v_dist and abs(v_angle) < 0.5 * math.pi: new_v_dist = v_dist new_v = v #new_v = potential_v[0] return new_v, cones, all_v else: return (0, 0), cones, all_v
class ConeTree(avango.script.Script): COLORMODE = "NODETYPE" FocusCTNode = avango.gua.SFNode() FocusSceneNode = avango.gua.SFNode() def __init__(self): self.super(ConeTree).__init__() # Constructor, start_node is the input node from wich one the ConeTree Representation starts def myConstructor(self, start_node): self.Start_node = start_node self.RootNode_ = avango.gua.nodes.TransformNode( Name = "ConeTreeRoot", ) self.ScaleNode_ = avango.gua.nodes.TransformNode( Name = "ConeTreeScale", ) self.TransformNode_ = avango.gua.nodes.TransformNode( Name = "ConeTreeTransform", ) self.RootCone_ = Cone(self.Start_node, None) self.FocusCone_ = self.RootCone_ avango.gua.load_materials_from("data/materials") self.FocusEdge_ = -1 self.layout() def update_focus_nodes(self): if self.FocusEdge_ == -1: self.FocusCTNode.value = self.FocusCone_.outNode_.geometry_ self.FocusSceneNode.value = self.FocusCone_.Input_node_ else: self.FocusCTNode.value = self.FocusCone_.ChildrenCones_[self.FocusEdge_].outNode_.geometry_ self.FocusSceneNode.value = self.FocusCone_.ChildrenCones_[self.FocusEdge_].Input_node_ def get_scene_node(self, CT_node): cones = [] cones.append(self.RootCone_) # search for the node while (not len(cones) == 0): current = cones.pop() # when found set collapsed if current.outNode_.geometry_ == CT_node: return current.Input_node_ for child in current.ChildrenCones_: cones.append(child) def get_CT_node(self, scene_node): cones = [] cones.append(self.RootCone_) # search for the node while (not len(cones) == 0): current = cones.pop() # when found set collapsed if current.Input_node_ == scene_node: return current.outNode_.geometry_ for child in current.ChildrenCones_: cones.append(child) def create_scenegraph_structure(self): node = self.RootCone_.get_scenegraph() self.RootNode_.Children.value = [self.ScaleNode_] self.ScaleNode_.Children.value = [self.TransformNode_] self.TransformNode_.Children.value = [node] return self.RootNode_ def get_root(self): return self.RootNode_ def print_ConeTree(self): self.RootCone_.print_cone(0) def layout(self): self.RootCone_.layout() self.RootCone_.apply_layout(root = True) self.FocusCone_.highlight(True) def set_colormode(self, colormode = "NODETYPE", flip = False): if flip: if ConeTree.COLORMODE == "NODETYPE": ConeTree.COLORMODE = "DEPTH" else: ConeTree.COLORMODE = "NODETYPE" else: ConeTree.COLORMODE = colormode def reapply_materials(self): self.RootCone_.reapply_material() self.FocusCone_.highlight(True) def scale(self): if self.FocusCone_.is_leaf(): used_Cone = self.FocusCone_.Parent_ else: used_Cone = self.FocusCone_ bb = used_Cone.outNode_.geometry_.BoundingBox.value bb_sides = (bb.Max.value.x - bb.Min.value.x ,bb.Max.value.y - bb.Min.value.y ,bb.Max.value.z - bb.Min.value.z) # scale = 0.5 / max(bb_sides) scale = 1.0 / max(bb_sides) # scale = 1.5 / max(bb_sides) self.ScaleNode_.Transform.value *= avango.gua.make_scale_mat(scale) def reposition(self): if self.FocusCone_.is_leaf(): used_Cone = self.FocusCone_.Parent_ else: used_Cone = self.FocusCone_ if not used_Cone.Parent_ == None: current = used_Cone parent = current.Parent_ matrix = current.outNode_.geometry_.Transform.value while not parent == self.RootCone_: current = parent parent = current.Parent_ matrix *= current.outNode_.geometry_.Transform.value self.TransformNode_.Transform.value = avango.gua.make_inverse_mat(matrix) else: self.TransformNode_.Transform.value = avango.gua.make_identity_mat() def set_camera_on_Focus(self): return # if not self.FocusCone_.is_leaf(): # bb = self.FocusCone_.outNode_.geometry_.BoundingBox.value # nodePosition = self.FocusCone_.outNode_.geometry_.WorldTransform.value.get_translate() # diff_x_left = nodePosition.x - bb.Min.value.x # diff_x_right = bb.Max.value.x - nodePosition.x # diff_y_left = nodePosition.y - bb.Min.value.y # diff_y_right = bb.Max.value.y - nodePosition.y # size_x = max(diff_x_left,diff_x_right) * 2 # size_y = bb.Max.value.y - bb.Min.value.y # size_z = bb.Max.value.z - bb.Min.value.z # eye_from_screen = self.EyeTransform.value.get_translate().length() # distance_x = ((eye_from_screen*size_x)/self.Screen.Width.value) - eye_from_screen # distance_y = ((eye_from_screen*size_y)/self.Screen.Height.value) - eye_from_screen # distance = max(distance_x,distance_y) + 0.5 * size_z # depth = - size_y / 2 # self.OutMatrix.value = avango.gua.make_trans_mat( nodePosition + avango.gua.Vec3(0,depth,distance) ) # rotate def rotate_by_id(self, id, angle): cones = [] cones.append(self.RootCone_) # search for the id while (not len(cones) == 0): current = cones.pop() # when found set highlighted if current.id_ == id : current.rotate(angle) print "Rotate: " + str(current.id_) + " : " + str(angle) self.layout() return True for child in current.ChildrenCones_: cones.append(child) return False # highlighting def highlight(self, selector, highlight = True, path = False): cones = [] cones.append(self.RootCone_) # search for the selector while (not len(cones) == 0): current = cones.pop() # when found set highlighted if current.id_ == selector or current.Input_node_ == selector or current.outNode_.geometry_ == selector: if path: current.highlight_path(highlight) print "Highlight path: " + str(current.id_) + " : " + str(highlight) else: current.highlight(highlight) print "Highlight: " + str(current.id_) + " : " + str(highlight) for child in current.ChildrenCones_: cones.append(child) def highlight_by_level(self, level, highlight): cones = [] cones.append(self.RootCone_) # search for the id while (not len(cones) == 0): current = cones.pop() # when found set highlighted if current.Level_ == level : current.highlight(highlight) print "Highlight: " + str(current.id_) + " : " + str(highlight) #self.layout() for child in current.ChildrenCones_: cones.append(child) # collapsing def flip_collapse_at_focus(self): if self.FocusCone_.collapsed_: self.FocusCone_.collapse(False) else: self.FocusCone_.collapse(True) self.layout() def collapse_by_level(self, level, collapse): cones = [] cones.append(self.RootCone_) # search for the level while (not len(cones) == 0): current = cones.pop() # when found set highlighted if current.Level_ == level : current.collapse(collapse) self.layout() for child in current.ChildrenCones_: cones.append(child) def collapse(self, selector, collapse = True): cones = [] cones.append(self.RootCone_) # search for the selector while (not len(cones) == 0): current = cones.pop() # when found set highlighted if current.id_ == selector or current.Input_node_ == selector or current.outNode_.geometry_ == selector: current.collapse(collapse) print "Collapsed: " + str(current.id_) + " : " + str(collapse) for child in current.ChildrenCones_: cones.append(child) # deal with focus def focus(self, selector): cones = [] cones.append(self.RootCone_) # search for the selector in whole COneTree while (not len(cones) == 0): current = cones.pop() # when found set highlighted if current.id_ == selector or current.Input_node_ == selector or current.outNode_.geometry_ == selector: # set new focus self.FocusCone_.highlight_path(0) self.FocusCone_ = current self.FocusCone_.highlight_path(1) self.FocusEdge_ = -1 self.update_focus_nodes() return True for child in current.ChildrenCones_: cones.append(child) return False def rotate_by_ray(self, ray, ray_scale): ray_start = ray.WorldTransform.value.get_translate() matrix = ray.WorldTransform.value * avango.gua.make_scale_mat(1.0/ray_scale.x , 1.0/ray_scale.y , 1.0/ray_scale.z) ray_direction = avango.gua.make_rot_mat(matrix.get_rotate()) * avango.gua.Vec3(0,0,-1) ray_direction = avango.gua.Vec3(ray_direction.x, ray_direction.y, ray_direction.z) ray_direction.normalize() if abs(ray_direction.x) > 0.5: if ray_direction.x > 0: self.FocusCone_.rotate((ray_direction.x - 0.5) / 10) else: self.FocusCone_.rotate((ray_direction.x + 0.5) / 10) # self.RootCone_.apply_layout(root = True) self.layout() def highlight_closest_edge(self, ray, ray_scale): ray_start = ray.WorldTransform.value.get_translate() matrix = ray.WorldTransform.value * avango.gua.make_scale_mat(1.0/ray_scale.x , 1.0/ray_scale.y , 1.0/ray_scale.z) ray_direction = avango.gua.make_rot_mat(matrix.get_rotate()) * avango.gua.Vec3(0,0,-1) ray_direction = avango.gua.Vec3(ray_direction.x, ray_direction.y, ray_direction.z) ray_direction_length = ray_direction.length() # calculate distance to Focus Node point = self.FocusCone_.outNode_.geometry_.WorldTransform.value.get_translate() tmp = ray_direction.cross((point - ray_start)) closest_distance = tmp.length() / ray_direction_length result = -1 # and calculate distance to all Children for i in range(len(self.FocusCone_.ChildrenCones_)): point = self.FocusCone_.ChildrenCones_[i].outNode_.geometry_.WorldTransform.value.get_translate() tmp = ray_direction.cross((point - ray_start)) tmp_distance = tmp.length() / ray_direction_length # closer ? if tmp_distance < closest_distance: closest_distance = tmp_distance result = i # the closest one ist highlighted if not result == -1: self.FocusCone_.weak_highlight_edge(self.FocusEdge_,0) self.FocusEdge_ = result self.FocusCone_.weak_highlight_edge(self.FocusEdge_,1) self.update_focus_nodes() def focus_in_focuscone(self, selector): # look for the selector in the edges of the Focus Cone for i in range(len(self.FocusCone_.Edges_)): if self.FocusCone_.Edges_[i].geometry_ == selector: self.FocusCone_.highlight_edge(self.FocusEdge_,0) self.FocusEdge_ = i self.FocusCone_.highlight_edge(self.FocusEdge_,1) return True cones = [] if self.FocusCone_.Parent_ == None: cones.append(self.FocusCone_) else: cones.append(self.FocusCone_.Parent_) for child in self.FocusCone_.ChildrenCones_: cones.append(child) # search for the selector in whole FocusCone while (not len(cones) == 0): current = cones.pop() # when found set highlighted if current.id_ == selector or current.Input_node_ == selector or current.outNode_.geometry_ == selector: # set new focus # self.FocusCone_.highlight_edge(self.FocusEdge_,0) self.FocusCone_.highlight_path(0) self.FocusCone_ = current self.FocusCone_.highlight_path(1) self.FocusEdge_ = -1 self.update_focus_nodes() return True return False def focus_next_edge(self): if not self.FocusCone_.is_leaf(): if not self.FocusEdge_ == -1: self.FocusCone_.highlight_edge(self.FocusEdge_,0) self.FocusEdge_ += 1 if self.FocusEdge_ == len(self.FocusCone_.Edges_): self.FocusEdge_ = 0 self.FocusCone_.highlight_edge(self.FocusEdge_,1) self.update_focus_nodes() # self.update_label() def focus_prev_edge(self): if not self.FocusCone_.is_leaf(): if self.FocusEdge_ == -1: self.FocusEdge_ = len(self.FocusCone_.Edges_) - 1 else: self.FocusCone_.highlight_edge(self.FocusEdge_,0) self.FocusEdge_ -= 1 if self.FocusEdge_ == -1: self.FocusEdge_ = len(self.FocusCone_.Edges_) - 1 self.FocusCone_.highlight_edge(self.FocusEdge_,1) self.update_focus_nodes() # self.update_label() def go_deep_at_focus(self): if not (self.FocusCone_.is_leaf() or self.FocusEdge_ == -1): # reset focusi self.FocusCone_.highlight_path(0) self.FocusCone_.highlight_edge(self.FocusEdge_,0) # set new focus self.FocusCone_ = self.FocusCone_.ChildrenCones_[self.FocusEdge_] self.FocusCone_.highlight_path(1) self.FocusEdge_ = -1 self.update_focus_nodes() # self.update_label() def level_up(self): if not self.FocusCone_.Parent_ == None: self.FocusCone_.highlight_path(0) if not self.FocusEdge_ == -1: self.FocusCone_.highlight_edge(self.FocusEdge_,0) self.FocusEdge_ = -1 self.FocusCone_ = self.FocusCone_.Parent_ self.FocusCone_.highlight_path(1) self.update_focus_nodes()
def process(img, depth_img, K_RGB): def distance_from_rgb(x, y, w, h, standing): # code from LAR laboratory if standing: u1_homogeneous = np.array([x, (y + h) / 2, 1]) u2_homogeneous = np.array([x + w, (y + h) / 2, 1]) else: u1_homogeneous = np.array([(x + w) / 2, y, 1]) u2_homogeneous = np.array([(x + w) / 2, (y + h), 1]) x1 = np.matmul(np.linalg.inv(K_RGB), u1_homogeneous) x2 = np.matmul(np.linalg.inv(K_RGB), u2_homogeneous) cos_alpha = np.dot(x1, x2) / (np.linalg.norm(x1) * np.linalg.norm(x2)) alpha = np.arccos(cos_alpha) return 0.025 / np.sin(alpha / 2) def camera_coord(x, y, z): u_mid_homogeneous = np.array([x, y, 1]) return np.matmul(np.linalg.inv(K_RGB), u_mid_homogeneous * z) def convert_to_robot_coord(coordinates): T = np.array([[0.0, 0.0, 1.0, -0.087], #generated by quaternion_to_rot_matrix.py [-1.0, 0.0, 0.0, 0.013], [0.0, -1.0, 0.0, 0.287], [0, 0, 0, 1]]) coord = np.append(coordinates, [1]) new_coord = np.matmul(T, coord) return Coordinates.Coordinates(new_coord[0], new_coord[1], new_coord[2]) HSV = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) color_min = [(0, 35, 25), (49, 30, 25), (107, 30, 25)] # for R, G, B in format (h_min, s_min, v_min) color_max = [(15, 255, 255), (77, 255, 255), (135, 255, 255)] # for R, G, B in format (h_max, s_max, v_max) cones = [] for i in range(3): color = "Red" if i == 1: color = "Green" elif i == 2: color = "Blue" frame_threshold = cv2.inRange(HSV, color_min[i], color_max[i]) contours, hierarchy = cv2.findContours(frame_threshold, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) for cnt in contours: area = cv2.contourArea(cnt) if area < 500: continue x, y, w, h = cv2.boundingRect(cnt) approx = cv2.approxPolyDP(cnt, 0.02 * cv2.arcLength(cnt, True), True) if len(approx) > 4: # not a square continue if float(h) / w > 8: #standing Cone standing = True elif float(w) / h > 8: # Cone on the floor standing = False else: #invalid ratio continue mid = (int(x + w / 2), int(y + h / 2)) z = depth_img[mid[1], mid[0]] if z is None: z = distance_from_rgb(x, y, w, h, standing) else: z += 0.025 # convert to distance from the middle of the bannsiter pos = camera_coord(mid[0], mid[1], z) # position in camera coordinates pos = convert_to_robot_coord(pos) new = Cone.Cone(color, pos, standing) cones.append(new) return cones