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 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_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 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 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 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 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 strength(self, point, task_params):
"""\
Return the coverage strength for a directional point. Note that since
the L{Camera} object is not internally aware of the scene it inhabits,
occlusion is computed in the L{Model} object instead.
@param point: The (directional) point to test.
@type point: L{Point}
@param task_params: Task parameters.
@type task_params: C{dict}
@return: The coverage strength of the point.
@rtype: C{float}
"""
cp = self.pose.inverse().map(point)
zn, zf = self.zc(task_params['blur_max'][1] * min(self._params['s']))
vfc = Point(0, 0, (zf - zn) / 2.0 + zn)
bn, bf = [self.fov['tah'] * self.fov['tav'] * z ** 2 for z in zn, zf]
# FIXME: this does not return a value in [0, 1]
try:
return vfc.euclidean(cp) / ((zf * bf - zn * bn) / 3.0)
except ZeroDivisionError:
return 0.0
def interpolate_points(points):
"""\
Interpolate the path function (mapping time to L{Point}s) from a set of
waypoints or knot points of the path at even time intervals.
@param points: List of path waypoints (one per unit time).
@type points: C{list} of L{Point}
@return: Smooth interpolated path function over time.
@rtype: C{function}
"""
split = [numpy.array([p[i] for p in points]) for i in range(3)]
tm = [float(i) for i in range(len(points))]
f = [interp1d(tm, p, kind='cubic') for p in split]
return lambda t: Point(*[f[i](t) for i in range(3)])
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 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))
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 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)
def test_point_eq(self):
e = 9e-5
self.assertEqual(self.p, Point(3 + e, 4 - e, 5 + e))
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_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))
default=100,
help='interpolation pitch')
parser.add_argument('-t',
'--threshold',
dest='threshold',
type=float,
default=0.0,
help='hysteresis threshold')
parser.add_argument('pathfile', help='path waypoint file')
args = parser.parse_args()
# Load path waypoints.
print('Loading path waypoints...')
points = []
with open(args.pathfile, 'r') as pf:
for line in pf.readlines():
points.append(Point(*[float(s) for s in line.rstrip().split(',')]))
path = interpolate_points(points)
# Set up displays and load the model.
print('Loading model...')
ex = Experiment(zoom=True)
ex.add_display()
ex.display.autoscale = True
ex.display.userspin = False
ex.display.forward = (0, 0, -1)
ex.display.up = (0, 1, 0)
ex.execute('loadmodel follow.yaml')
ex.execute('loadconfig')
# Compute the vision graph.
if os.path.exists('vgraph.pickle'):
print('Loading vision graph...')
vision_graph = pickle.load(open('vgraph.pickle', 'r'))
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_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_point_euclidean(self):
self.assertEqual(self.p.euclidean(Point(0, 0, 0)), self.p.magnitude())
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_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_cross(self):
self.assertEqual(self.p.cross(Point(1, 2, 1)), Point(-6, 2, 2))
def test_point_dot(self):
self.assertEqual(self.p.dot(Point(2, 1, 3)), 25)