def _flatten_pose(self): du = self.pose.map(Point(0, 0, 1)) - self.pose.T R = \ Rotation.from_axis_angle(Angle(atan2(du.y, du.x) - pi / 2.0), Point(0, 0, 1)) + \ Rotation.from_axis_angle(-pi / 2.0, Point(1, 0, 0)) flat_pose = Pose(T=Point(self.pose.T.x, self.pose.T.y, 50), R=R) self.set_absolute_pose(flat_pose) self._flatten_pose = lambda: None
def gen_view_points(ex, camera, task_params, scene_points): """\ Generate the solution space as the list of viewpoints. @param camera: The camera name in the model. @type camera: C{str} @param task_params: Task parameters. @type task_params: C{dict} @param scene_points: A list of scene points in the task model. @type scene_points: C{list} of L{adolphus.geometry.DirectionalPoint} @return: A pair of lists containing the viewpoints and their poses. @rtype: C{list} """ view_point_poses = [] hull = ex.model[camera].gen_frustum_hull(task_params) standoff = (hull[0][2] + hull[-1][2]) / 2.0 for point in scene_points: x = point.x + (standoff * sin(point.rho) * cos(point.eta)) y = point.y + (standoff * sin(point.rho) * sin(point.eta)) z = point.z + (standoff * cos(point.rho)) rho = point.rho + pi eta = point.eta point = DirectionalPoint(x, y, z, rho, eta) normal = point.direction_unit() angle = Point(0, 0, 1).angle(normal) axis = Point(0, 0, 1).cross(normal) if abs(angle) < 1e-4 and axis == Point(0,0,0): pose = Pose() else: pose = Pose(Point(x, y, z), Rotation.from_axis_angle(angle, axis)) view_point_poses.append(pose) return view_point_poses
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 pose_from_dp(x, y, z, rho, eta): point = DirectionalPoint(x, y, z, rho, eta) normal = point.direction_unit() angle = Point(0, 0, 1).angle(normal) axis = Point(0, 0, 1).cross(normal) if angle < 1e-4: return Pose() else: return Pose(Point(x, y, z), Rotation.from_axis_angle(angle, axis))
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_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 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 gen_view_points(self, scene_points, mode='flat'): """\ Generate the solution space as the list of viewpoints. @param scene_points: The list of scene points in the task model. @type scene_points: C{list} @param mode: Select whether to position all cameras flat, vertical, or alternate between flat and vertical. Options: C{flat}, C{alternate}, C{vertical} @type mode: C{str} @return: A pair of lists containing the viewpoints and their poses. @rtype: C{list} """ view_points = [] view_point_poses = [] flat = True # Standoff calculation as described in Scott 2009, Section 4.4.3. f_d = 1.02 R_o = max(self.cam.zres(self.task_par['res_max'][1]), \ self.cam.zc(self.task_par['blur_max'][1] * \ min(self.cam.getparam('s')))[0]) e = 1e9 for zS in self.lens_dict['zS']: if abs(zS - (f_d * R_o)) < e: e = abs(zS - (f_d * R_o)) index = self.lens_dict['zS'].index(zS) # Updating the camera after computing the camera standoff distance simulates # the action of focusing the lens. self.update_camera(index) # This implementation assumes that the camera is mounted on the laser # and it is in fact the laser that moves. z_lim = [max(self.cam.zres(self.task_par['res_max'][1]), self.cam.zc(self.task_par['blur_max'][1] * \ min(self.cam_par['s']))[0]), min(self.cam.zres(self.task_par['res_min'][1]), self.cam.zc(self.task_par['blur_max'][1] * \ min(self.cam_par['s']))[1])] cam_standoff = z_lim[1] * 0.9 self.laser.mount = self.cam T = Point(0, cam_standoff * tan(0.7853), 0) R = Rotation.from_euler('zyx', (Angle(-0.7853), Angle(0), Angle(0))) self.laser.set_relative_pose(Pose(T, R)) # The camera standoff. standoff = cam_standoff for point in scene_points: x = point.x + (standoff * sin(point.rho) * cos(point.eta)) y = point.y + (standoff * sin(point.rho) * sin(point.eta)) z = point.z + (standoff * cos(point.rho)) rho = point.rho + pi eta = point.eta point = DirectionalPoint(x, y, z, rho, eta) view_points.append(point) normal = point.direction_unit() angle = Point(0, 0, 1).angle(normal) axis = Point(0, 0, 1).cross(normal) pose = Pose(Point(x, y, z), Rotation.from_axis_angle(angle, axis)) angles = pose.R.to_euler_zyx() if mode == 'flat': if y >= 0: new_angles = (angles[0], angles[1], Angle(pi)) elif y < 0: new_angles = (angles[0], angles[1], Angle(0)) elif mode == 'vertical': new_angles = (angles[0], angles[1], Angle(0)) elif mode == 'alternate': if flat: new_angles = (angles[0], angles[1], Angle(1.5707)) flat = False elif not flat: new_angles = (angles[0], angles[1], Angle(0)) flat = True new_pose = Pose(Point(x,y,z), Rotation.from_euler('zyx', new_angles)) view_point_poses.append(new_pose) return view_points, view_point_poses
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)
task_points[DirectionalPoint(-32.5, 110, 1660, 1.570796327, 1.570796327)] = 1 task_points[DirectionalPoint(32.5, 110, 1660, 1.570796327, 1.570796327)] = 1 task_points[Point(-225, 0, 1500)] = 1 task_points[Point(225, 0, 1500)] = 1 task_points[Point(0, 225, 1500)] = 1 ex.tasks['T'] = Task(task_par, task_points, mount=ex.model['human']) # Run the demo. line = "" pitch = float(args.interpolate) i = 0 for t in range(1, args.interpolate * (len(points) - 1)): if ex.exit: break # Set target pose. normal = -(path(t / pitch) - path((t - 1) / pitch)).unit() angle = Point(0, -1, 0).angle(normal) axis = Point(0, -1, 0).cross(normal) R = Rotation.from_axis_angle(angle, axis) ex.model['human'].pose = Pose(T=path(t / pitch), R=R) line += str(i) + '\t' + str(ex.model.performance(ex.tasks['T'])) + '\n' i += 1 sleep(.01) if args.vis: ex.model['human'].update_visualization() with open('result.txt', 'w') as file: file.write(line) print "Done.\n"
for occ_t in occ_triangles: if occ_t.intersection(t, p, True): occluded = True break if occluded: continue vector = p - t if abs(vector.magnitude() - half_hull) > ((hull[-1][2] - hull[0][2]) * .1): continue normal = vector.unit() angle = Point(0, 0, 1).angle(normal) axis = Point(0, 0, 1).cross(normal) if angle < 1e-4: pose = Pose() else: pose = Pose(Point(t.x, t.y, t.z), Rotation.from_axis_angle(angle, axis)) view_point_poses.append(pose) print "Size of the solution space:", len(view_point_poses) # Deploy camera network. # Generate the measurability matrix. visual_m = gen_visual_matrix(ex, view_point_poses, scene_points, [cam_par, task_par]) # Generate overlap graph. o_graph = gen_overlap_graph(visual_m, 0.05) # Select viewpoints. indices = select_view_points(visual_m, o_graph) selection = [] for i in indices:
with open('vgraph.pickle', 'w') as vgf: pickle.dump(vision_graph, vgf) 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)