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