def test_not_a_loop_too_far_horizontally(self):
forward = euler_to_quaternion(0, 0, 0)
trajectory = [((0, 0, 0), forward),
((2, 0, 0), forward),
((2, 2, 0), forward),
((0, 2, 0), forward)]
detector = LoopDetector(min_loop_length=5)
detector.add_all(trajectory)
loop = detector.loop()
self.assertIsNone(loop)
def test_perfect_loop(self):
forward = euler_to_quaternion(0, 0, 0)
trajectory = [((0, 0, 0), forward),
((2, 0, 0), forward),
((2, 2, 0), forward),
((0, 2, 0), forward),
((0, 0, 0), forward)]
detector = LoopDetector(min_loop_length=5)
detector.add_all(trajectory)
loop = detector.loop()
self.assertEqual(loop, trajectory)
def test_loop_with_small_rotation(self):
forward = euler_to_quaternion(0, 0, 0)
almost_forward = euler_to_quaternion(math.radians(10), 0, 0)
trajectory = [((0, 0, 0), forward),
((2, 0, 0), forward),
((2, 2, 0), forward),
((0, 2, 0), forward),
((0, 0, 0), almost_forward)]
detector = LoopDetector(min_loop_length=5)
detector.add_all(trajectory)
loop = detector.loop()
self.assertEqual(loop, trajectory)
def test_not_a_loop_different_direction(self):
forward = euler_to_quaternion(0, 0, 0)
left = euler_to_quaternion(math.pi/2, 0, 0)
trajectory = [((0, 0, 0), forward),
((2, 0, 0), forward),
((2, 2, 0), forward),
((0, 2, 0), forward),
((0, 0, 0), left)]
detector = LoopDetector(min_loop_length=5)
detector.add_all(trajectory)
loop = detector.loop()
self.assertIsNone(loop)
def test_granularity(self):
forward = euler_to_quaternion(0, 0, 0)
trajectory = [((0, 0, 0), forward),
((1, 0, 0), forward),
((2, 0, 0), forward),
((2, 1, 0), forward),
((2, 2, 0), forward),
((1, 2, 0), forward),
((0, 2, 0), forward),
((0, 1, 0), forward),
((0, 0, 0), forward)]
detector = LoopDetector(min_loop_length=5, granularity=2.0)
detector.add_all(trajectory)
loop = detector.loop()
self.assertEqual(loop, trajectory[::2])