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 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 parse_external(filename):
"""\
Parse external calibration parameters from a HALCON external calibration
output file.
@param filename: The calibration output file to load from.
@type filename: C{str}
"""
with open(filename, 'r') as ecfile:
for line in ecfile:
line = line.rstrip()
if line.startswith('t'):
T = Point([1e3 * float(value) for value in line.split(' ')[1:]])
elif line.startswith('r'):
R = Rotation.from_euler('zyx', [(360. - float(value)) * pi \
/ 180.0 for value in line.split(' ')[1:]])
return Pose(T, R)
def parse_pose_string(pose_string):
"""\
Parse a pose string from HALCON (comma-delimited concantenation of a
homogeneous 3D transformation matrix).
@param pose_string: The pose string from HALCON.
@type pose_string: C{str}
@return: The parsed pose.
@rtype: L{Pose}
"""
pose = pose_string.split(',')
T = Point([1e3 * float(s) for s in [pose[4 * i + 3] for i in range(3)]])
rot = [pose[4 * i:4 * i + 3] for i in range(3)]
for i in range(3):
for j in range(3):
rot[i][j] = float(rot[i][j])
R = Rotation.from_rotation_matrix(rot)
return Pose(T, R)
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 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 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 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 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)
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)
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)
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:
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
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"
