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) ----'