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 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 get_bounds(model, task, cameras, lut):
# compute bounds on x and h
xmin, xmax = float('inf'), -float('inf')
zmin, zmax = float('inf'), -float('inf')
for point in task.mapped:
lp = model[model.active_laser].triangle.intersection(point,
point + Point(0, 1, 0), False)
if lp:
if lp.x < xmin:
xmin = lp.x
if lp.x > xmax:
xmax = lp.x
if lp.z < zmin:
zmin = lp.z
if lp.z > zmax:
zmax = lp.z
# compute bounds on d
dbounds = []
Ra = task.getparam('res_min')[1]
Ha = task.getparam('hres_min')[1]
for c, camera in enumerate(cameras):
modify_camera(model, camera, lut[c], 0, 0, lut[c].bounds[1],
task.getparam('angle_max')[1])
p = (-model[camera].pose).map(DirectionalPoint(0, 0, 0, 0, 0))
angle = p.direction_unit().angle(-p)
du = min(model[camera].zres(Ra),
model[camera].zhres(Ha, angle))
dbounds.append((lut[c].bounds[0], min(lut[c].bounds[1], du)))
# return bounds
bounds = []
for i in range(len(cameras)):
bounds += [(xmin, xmax), (zmin, zmax), dbounds[i],
(0, task.getparam('angle_max')[1])]
return bounds
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 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_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_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_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 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]))