def test_strength(self):
p1 = Point(0, 0, 1000)
p2 = Point(0, 0, 1200)
self.assertEqual(self.model.strength(p1, self.tasks['R1'].params), self.model['C'].strength(p1, self.tasks['R1'].params))
self.assertTrue(self.model.strength(p1, self.tasks['R1'].params))
self.assertFalse(self.model.strength(p2, self.tasks['R1'].params))
self.model['C'].set_absolute_pose(Pose(T=Point(1000, 0, 0)))
self.assertFalse(self.model.strength(p1, self.tasks['R1'].params))
self.model['C'].set_absolute_pose(Pose(R=Rotation.from_axis_angle(pi, Point(1, 0, 0))))
self.assertFalse(self.model.strength(p1, self.tasks['R1'].params))
def test_set_pose(self):
self.model['Plate'].set_absolute_pose(Pose(T=Point(25, 0, 0)))
self.assertEqual(self.model['Plate'].pose, Pose(T=Point(25, 0, 0)))
self.model['Plate'].pose = Pose(T=Point(50, 0, 0))
self.assertEqual(self.model['Plate'].pose, Pose(T=Point(50, 0, 0)))
self.assertEqual(self.model['Block'].get_relative_pose(), Pose(T=Point(32, 8, 0)))
self.assertEqual(self.model['Block'].pose, Pose(T=Point(82, 8, 3.2)))
self.model['Block'].set_relative_pose(Pose(T=Point(16, 8, 0)))
self.assertEqual(self.model['Plate'].pose, Pose(T=Point(50, 0, 0)))
self.assertEqual(self.model['Block'].pose, Pose(T=Point(66, 8, 3.2)))
def test_mount(self):
self.assertEqual(self.model['Plate'].mount_pose(), Pose(T=Point(0, 0, 3.2)))
self.assertEqual(self.model['Block'].mount, self.model['Plate'])
self.assertTrue(self.model['Block'] in self.model['Plate'].children)
self.model['Block'].mount = None
self.assertEqual(self.model['Block'].mount, None)
self.assertFalse(self.model['Block'] in self.model['Plate'].children)
self.assertEqual(self.model['Block'].get_absolute_pose(), Pose(T=Point(32, 8, 0)))
self.model['Plate'].set_absolute_pose(Pose(T=Point(25, 0, 0)))
self.assertEqual(self.model['Block'].get_absolute_pose(), Pose(T=Point(32, 8, 0)))
self.model['Block'].mount = self.model['Plate']
self.assertEqual(self.model['Block'].mount, self.model['Plate'])
self.assertTrue(self.model['Block'] in self.model['Plate'].children)
self.assertEqual(self.model['Block'].get_absolute_pose(), Pose(T=Point(57, 8, 3.2)))
def test_hook(self):
def callback():
self.value += 1
self.model['Block'].posecallbacks['test'] = callback
self.model['Block'].set_relative_pose(Pose(T=Point(16, 8, 0)))
self.assertEqual(self.value, 1)
self.model['Plate'].set_absolute_pose(Pose(T=Point(50, 0, 0)))
self.assertEqual(self.value, 2)
self.model['Block'].mount = None
self.assertEqual(self.value, 3)
self.model['Plate'].set_absolute_pose(Pose(T=Point(100, 0, 0)))
self.assertEqual(self.value, 3)
del self.model['Block'].posecallbacks['test']
self.model['Block'].set_absolute_pose(Pose())
self.assertEqual(self.value, 3)
def test_occlusion_cache(self):
key = self.model._update_occlusion_cache(self.tasks['R1'].params)
self.assertTrue(all([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P1'].triangles]))
self.assertFalse(any([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P2'].triangles]))
self.model['C'].set_absolute_pose(Pose(R=Rotation.from_axis_angle(-pi / 2.0, Point(1, 0, 0))))
key = self.model._update_occlusion_cache(self.tasks['R1'].params)
self.assertFalse(any([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P1'].triangles]))
self.assertTrue(all([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P2'].triangles]))
self.model['C'].set_absolute_pose(Pose(R=Rotation.from_axis_angle(pi, Point(1, 0, 0))))
key = self.model._update_occlusion_cache(self.tasks['R1'].params)
self.assertFalse(any([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P1'].triangles]))
self.assertFalse(any([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P2'].triangles]))
self.model['C'].set_absolute_pose(Pose())
key = self.model._update_occlusion_cache(self.tasks['R1'].params)
self.assertTrue(all([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P1'].triangles]))
self.model['C'].setparam('zS', 600.0)
key = self.model._update_occlusion_cache(self.tasks['R1'].params)
self.assertFalse(any([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P1'].triangles]))
self.assertFalse(any([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P2'].triangles]))
def test_get_pose(self):
self.assertEqual(self.model['Plate'].get_absolute_pose(), Pose())
self.assertEqual(self.model['Plate'].get_relative_pose(), Pose())
self.assertEqual(self.model['Plate'].pose, Pose())
self.assertEqual(self.model['Block'].get_absolute_pose(), Pose(T=Point(32, 8, 3.2)))
self.assertEqual(self.model['Block'].get_relative_pose(), Pose(T=Point(32, 8, 0)))
self.assertEqual(self.model['Block'].pose, Pose(T=Point(32, 8, 3.2)))
def find_convex_group(ex, sceneobj, vertex):
"""\
Find the set of faces that form a convex set with respect to a given vertex.
@param sceneobj: The name of the object in the scene.
@type sceneobj: C{str}
@param vertex: The vertex.
@type vertex: L{adolphus.geometry.Point}
@return: List of indices of the list of faces of the scene object.
@rtype: C{list} of C{int}
"""
solid = ex.model[sceneobj]
faces = solid.faces_of_vertex(vertex)
normals_v = [solid.normals[face] for face in faces]
direction = avg_points(normals_v)
angle = Point(0, 0, 1).angle(direction)
axis = Point(0, 0, 1).cross(direction)
pose = Pose(vertex, Rotation.from_axis_angle(angle, axis)).inverse()
vertices = solid.vertex_neighbors(vertex)
v_map = pose.map(vertex)
mapped = []
for point in vertices:
mapped.append(pose.map(point))
convex_indices = [solid.vertices.index(vertex)]
for i in range(len(mapped)):
if mapped[i].z >= v_map.z:
convex_indices.append(solid.vertices.index(vertices[i]))
candidates = combinations(convex_indices, 3)
convex_set = []
for item in candidates:
tri = solid.flook(solid.vertices[item[0]], \
solid.vertices[item[1]], \
solid.vertices[item[2]])
if tri != None:
convex_set.append(tri)
return convex_set
def modify_camera(model, camera, lut, x, h, d, beta):
if model[camera].negative_side:
y = -d * sin(beta) + model[model.active_laser].pose.T.y
z = d * cos(beta) + h
R = Rotation.from_axis_angle(pi + beta, Point(1, 0, 0))
else:
y = d * sin(beta) + model[model.active_laser].pose.T.y
z = d * cos(beta) + h
R = Rotation.from_axis_angle(pi, Point(0, 1, 0)) + \
Rotation.from_axis_angle(-beta, Point(-1, 0, 0))
T = Point(x, y, z)
model[camera].set_absolute_pose(Pose(T, R))
f, ou, ov, A = lut.parameters(d)
model[camera].setparam('zS', d)
model[camera].setparam('f', f)
model[camera].setparam('o', [ou, ov])
model[camera].setparam('A', A)
def parse_from_halcon(hstring):
"""\
Convert tuple data in string format from HALCON into the camera ID and
target pose.
@param hstring: the string data from HALCON.
@type hstring: C{str}
@return: Camera ID and target pose.
@rtype: C{str}, L{Pose}
"""
frame_markers = {}
try:
for pair in hstring.split(';'):
pose = pair.split(':')[1].split(',')
weight = float(pose[len(pose) - 1].split('|').pop())
pose[len(pose) - 1] = pose[len(pose) - 1].split('|')[0]
for i in range(len(pose)):
pose[i] = float(pose[i])
frame_markers.update({int(pair.split(':')[0]): \
{'pose':pose, 'weight':weight}})
for marker in frame_markers:
trans_vec = [frame_markers[marker]['pose'][3],frame_markers[marker]['pose'][7],\
frame_markers[marker]['pose'][11]]
rot_mat = [[] for x in range(3)]
for i in range(3):
rot_mat[i] = [
frame_markers[marker]['pose'][4 * i],
frame_markers[marker]['pose'][4 * i + 1],
frame_markers[marker]['pose'][4 * i + 2]
]
t = Point(trans_vec)
r = Rotation.from_rotation_matrix(rot_mat)
pose = Pose(t, r)
frame_markers[marker]['pose'] = pose
except:
pass
return frame_markers
def gaussian_yz_pose_error(pose, tsigma, rsigma):
T, R = pose.T, pose.R
T = Point(gauss(T.x, tsigma), gauss(T.y, tsigma), gauss(T.z, tsigma))
R += Rotation.from_axis_angle(Angle(gauss(0, rsigma)), Point(1, 0, 0))
return Pose(T=T, R=R)
except ImportError:
vision_graph = None
# Run demo.
best = None
ex.start()
for t in range(1, args.interpolate * (len(points) - 1)):
if ex.exit:
break
# Set target pose.
normal = -(path(t / float(args.interpolate)) - path((t - 1) \
/ float(args.interpolate))).unit()
angle = Point(0, -1, 0).angle(normal)
axis = Point(0, -1, 0).cross(normal)
R = Rotation.from_axis_angle(angle, axis)
ex.model['Person'].set_absolute_pose(\
Pose(T=path(t / float(args.interpolate)), R=R))
ex.model['Person'].update_visualization()
# Compute best view.
current = best
best, score = ex.model.best_view(
ex.tasks['target'],
current=(current and frozenset([current]) or None),
threshold=args.threshold)
best = set(best).pop()
if current != best:
ex.execute('select %s' % best)
ex.altdisplays[0].camera_view(ex.model[best])
try:
ex.execute('guide %s target' % current)
except:
pass
def setUp(self):
self.p = Point(3, 4, 5)
self.dp = DirectionalPoint(-7, 1, 9, 1.3, 0.2)
self.R = Rotation.from_euler('zyx', (pi, 0, 0))
self.P1 = Pose(R=self.R)
self.P2 = Pose(T=Point(3, 2, 1), R=self.R)
class TestGeometry(unittest.TestCase):
"""\
Tests for the geometry module.
"""
def setUp(self):
self.p = Point(3, 4, 5)
self.dp = DirectionalPoint(-7, 1, 9, 1.3, 0.2)
self.R = Rotation.from_euler('zyx', (pi, 0, 0))
self.P1 = Pose(R=self.R)
self.P2 = Pose(T=Point(3, 2, 1), R=self.R)
def test_angle(self):
a = Angle(0.3)
self.assertTrue(abs(a - Angle(0.3 + 2 * pi)) < 1e-4)
b = a + Angle(6.0)
self.assertTrue(b < a)
b = -a
self.assertTrue(b > a)
def test_point_eq(self):
e = 9e-5
self.assertEqual(self.p, Point(3 + e, 4 - e, 5 + e))
def test_point_add_sub(self):
self.assertEqual(self.p + self.dp, Point(-4, 5, 14))
self.assertEqual(self.dp + self.p, DirectionalPoint(-4, 5, 14, 1.3, 0.2))
self.assertEqual(self.p - self.dp, Point(10, 3, -4))
def test_point_mul_div(self):
self.assertEqual(self.p * 1.5, Point(4.5, 6, 7.5))
self.assertEqual(self.p / 2, Point(1.5, 2, 2.5))
def test_point_dot(self):
self.assertEqual(self.p.dot(Point(2, 1, 3)), 25)
def test_point_cross(self):
self.assertEqual(self.p.cross(Point(1, 2, 1)), Point(-6, 2, 2))
def test_point_neg(self):
self.assertEqual(-self.p, Point(-3, -4, -5))
self.assertEqual(-self.dp, DirectionalPoint(7, -1, -9, 1.3 + pi, 0.2))
def test_point_magnitude(self):
self.assertEqual(self.p.magnitude(), sqrt(sum([self.p[i] ** 2 for i in range(3)])))
def test_point_unit(self):
m = self.p.magnitude()
self.assertEqual(self.p.unit(), Point(*[self.p[i] / m for i in range(3)]))
def test_point_euclidean(self):
self.assertEqual(self.p.euclidean(Point(0, 0, 0)), self.p.magnitude())
def test_point_angle(self):
self.assertTrue(abs(float(self.p.angle(-self.p)) - pi) < 1e-4)
def test_point_direction_unit(self):
rho, eta = self.dp.rho, self.dp.eta
self.assertEqual(self.dp.direction_unit(), Point(sin(rho) * cos(eta), sin(rho) * sin(eta), cos(rho)))
def test_rotation_rotate_point(self):
r = Point(3, -4, -5)
self.assertEqual(r, self.R.rotate(self.p))
r = DirectionalPoint(-7, -1, -9, pi - 1.3, 2 * pi - 0.2)
self.assertEqual(r, self.P1.map(self.dp))
def test_pose_map(self):
m = Point(6, -2, -4)
self.assertEqual(m, self.P2.map(self.p))
m = DirectionalPoint(-4, 1, -8, pi - 1.3, 2 * pi - 0.2)
self.assertEqual(m, self.P2.map(self.dp))
def test_triangle_intersection(self):
triangle = Triangle(Point(-3, -3, 0), Point(-3, 2, 0), Point(4, 1, 0))
self.assertTrue(triangle.intersection(Point(-1, -1, 3), Point(-1, -1, -3), True))
self.assertTrue(triangle.intersection(Point(-1, -1, -3), Point(-1, -1, 3), True))
self.assertFalse(triangle.intersection(Point(5, 5, 3), Point(5, 5, -3), True))
self.assertFalse(triangle.intersection(Point(5, 5, 3), Point(5, 5, 1), True))
def test_triangle_overlap(self):
triangles = [Triangle(Point(0, 0, 0), Point(10, 2, 0), Point(8, 0, 6)),
Triangle(Point(0, 2, 1), Point(4, -7, 2), Point(7, 3, 3)),
Triangle(Point(-1, -1, -1), Point(-1, -2, 2), Point(-5, -1, -1))]
self.assertTrue(triangles[0].overlap(triangles[1]))
self.assertTrue(triangles[1].overlap(triangles[0]))
self.assertFalse(triangles[0].overlap(triangles[2]))
self.assertFalse(triangles[2].overlap(triangles[0]))
self.assertFalse(triangles[1].overlap(triangles[2]))
self.assertFalse(triangles[2].overlap(triangles[1]))
def main():
# parse command line arguments for reference marker id
parser = argparse.ArgumentParser(description='Choose reference marker.')
parser.add_argument('ref_id', type=int, help='ID of the reference marker')
ui = parser.parse_args()
# set up network socket
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.bind(('localhost', 5678))
sock.listen(20)
# accept incoming connection requests
channel, details = sock.accept()
#channel.settimeout(20)
# create a text file for poses
filename = 'ourposes.txt'
FILE = open(filename, 'w')
try:
# acceptable forms of 'yes' and 'no' for quiting prompts
end = ''
yes_set = set(['y', 'Y', 'yes', 'Yes', 'YES'])
no_set = set(['n', 'N', 'no', 'No', 'NO'])
'''-------------------'''
''' MAP-BUILDING MODE '''
'''-------------------'''
print '*************** MAP-BUILDING MODE ***************\n'
# initialize empty graph and the global relative poses (GRPs)
graph = Graph()
global_relposes = {}
while (True):
# if a key is pressed
if kbhit():
# catch the key
key = ord(getch())
# calculate the current shortest path to the reference using dijkstra's algorithm
curr_prev = dijkstra(graph, ui.ref_id)
# find the unconnected vertices (markers)
uc_verts = [
um for um in curr_prev
if um != ui.ref_id and curr_prev[um] == None
]
# if 's' is pressed on the keyboard, update user on all current registered and unconnected markers
if key == 115:
print len(graph.vertices), 'Registered marker(s):', list(
graph.vertices)
print len(
uc_verts), 'Unconnected marker(s):', uc_verts, '\n'
# if 'q' is pressed on the keyboard, notify user of all registered markers and also if...
# ... (i) reference marker is not registered or (ii) not all registered markers have a ...
# ... route back to the reference marker
# give user the option to exit
elif key == 113:
# if the reference marker is not registered as a vertex in the graph, tell user they ...
# ... cannot go into online mode without it
# give them option to end the program
if ui.ref_id not in graph.vertices:
print 'The reference marker has not been registered. You cannot continue to'
print 'online mode without it.'
end = raw_input(
'Are you sure you want to end the program? (y/n): '
)
if end in yes_set:
return
else:
pass
else:
try:
# show the number of registered markers
print '- There are currently', len(
graph.vertices), 'registered markers:', list(
graph.vertices)
# warn user of any disconnections in the graph
if not connected(graph):
print '- The following have no route to the reference marker:', uc_verts
else:
print '- All markers have a route to the reference marker.'
pass
finally:
# ask the user if they want to end map-building mode despite the warning
while (end not in (yes_set | no_set)):
end = raw_input(
'End map-building mode? (y/n): ')
# end or continue map-building mode depending on user's choice
if end in yes_set:
break
else:
pass
# parse the incoming data from halcon
frame_markers = {}
hstring = channel.recv(65536)
if not hstring or hstring == 'no markers found':
pass
# if socket is closed from the other end (halcon), stop the program
# elif hstring == 'connection closed':
# print '---- CONNECTION CLOSED (HALCON) ----'
# return
else:
frame_markers = parse_from_halcon(hstring)
# update the set of vertices in the graph with the local markers
graph.vertices.update(frame_markers)
# find the local relative poses (LRPs) of the markers w.r.t. one another and use them ...
# ... to update the GRP and the graph
local_relposes = {}
for marker in frame_markers:
marker_a = frame_markers[marker]
for other in frame_markers:
try:
assert other != marker
assert (marker, other) not in local_relposes
except AssertionError:
continue
marker_b = frame_markers[other]
pose_ab = marker_a['pose'] - marker_b['pose']
pose_ba = -pose_ab
weight_ab = marker_a['weight'] + marker_b['weight']
weight_ba = weight_ab
local_relposes[(marker, other)] = {
'pose': pose_ab,
'weight': weight_ab
}
local_relposes[(other, marker)] = {
'pose': pose_ba,
'weight': weight_ba
}
# if the edge joining the two markers does not exist in neither the GRPs nor the graph, ...
# ... add it to both from the LRPs
if (marker, other) not in global_relposes:
global_relposes[(marker,
other)] = local_relposes[(marker,
other)]
global_relposes[(other,
marker)] = local_relposes[(other,
marker)]
graph.add_edge(
Edge((marker, other)),
local_relposes[(marker, other)]['weight'])
# however, if it already exists and the weight of the new one is less than the existing one, ...
# ... replace the existing ones in the GRPs and graph with the new one
else:
if weight_ab < global_relposes[(marker,
other)]['weight']:
global_relposes[(marker,
other)] = local_relposes[(marker,
other)]
global_relposes[(other, marker)] = local_relposes[(
other, marker)]
graph.weights[Edge((marker, other))] = weight_ab
# send message back to halcon telling it python prog. is done processing frame data and ready for more
#channel.send('d')
# write the graph information to file
FILE.write('------ GRAPH ------\n\n')
FILE.write(str(graph) + '\n\n')
# obtain the "previous array" of the shortest path as determined by dijkstra's algorithm
prev = dijkstra(graph, ui.ref_id)
# write title of section (marker poses) to file
FILE.write('------ MARKER POSES ------\n\n')
# find the global pose (and its associated aggregate weight) of each marker in the map w.r.t. the reference marker
gmarkposes = {}
for marker in prev:
pose_comp = Pose()
agg_weight = 0.0
curr_i = marker
prev_i = prev[curr_i]
# skip markers that are disconnected from the reference
if not prev_i and curr_i != ui.ref_id:
continue
# loop until reference is reached
while prev_i:
edge_i = (curr_i, prev_i)
pose_comp += global_relposes[edge_i]['pose']
agg_weight += global_relposes[edge_i]['weight']
curr_i = prev_i
prev_i = prev[curr_i]
# store the global pose and weight of the marker
gmarkposes[marker] = {'pose': pose_comp, 'weight': agg_weight}
# write the global pose to the file
FILE.write(str(marker) + ': ' + str(gmarkposes[marker]) + '\n\n')
FILE.write(
'------------------------------------------------------\n\n')
'''-------------'''
''' ONLINE MODE '''
'''-------------'''
print '\n\n*************** ONLINE MODE ***************\n'
# write title of section (marker poses) to file
FILE.write('------ ONLINE MODE ------\n\n')
# data is initially not recorded to file
rec = 0
while (True):
# if a key is pressed
if kbhit():
# catch the key
key = ord(getch())
# if 'q' is pressed on the keyboard, give user the option to exit
if key == 113:
end = raw_input(
'Are you sure you want to end the program? (y/n): ')
if end in yes_set:
return
else:
pass
# if key is 'r', toggle recording of data to file
if key == 114:
rec = rec ^ 1
# parse the incoming data from halcon
frame_markers = {}
hstring = channel.recv(65536)
if not hstring or hstring == 'no markers found':
pass
# if socket is closed from the other end (halcon), stop the program
# elif hstring == 'connection closed':
# print '---- CONNECTION CLOSED (HALCON) ----'
# return
else:
frame_markers = parse_from_halcon(hstring)
# find the camera pose that yields the minimum aggregate weight of all markers in the frame
bestmarker = None
bestpose = None
minweight = float('inf')
for marker in frame_markers:
if marker in gmarkposes:
marker_i = frame_markers[marker]
gcampose = -marker_i['pose'] + gmarkposes[marker]['pose']
gcamweight = marker_i['weight'] + gmarkposes[marker][
'weight']
if gcamweight < minweight:
bestmarker = marker
bestpose = gcampose
minweight = gcamweight
else:
pass
print '----------------------------'
print 'through marker: ', bestmarker
print 'pose: ', bestpose
print 'weight: ', minweight
print '----------------------------'
# if record toggle is on, record the poses to file
if rec == 1:
if (bestpose):
temp = str(bestpose.T).split(',')
bestpose_str = temp[0][1:] + ' ' + temp[1][1:]
FILE.write(bestpose_str + '\n')
# send message back to halcon telling it python prog. is done processing frame data and ready for more
#channel.send('d')
finally:
FILE.close()
channel.close()
sock.close()
print '---- CONNECTION CLOSED (PYTHON) ----'