|
1
|
|
|
import numpy as np |
|
2
|
|
|
from patty.registration import find_rotation_xy |
|
3
|
|
|
import pcl |
|
4
|
|
|
|
|
5
|
|
|
from numpy.testing import assert_almost_equal |
|
6
|
|
|
import unittest |
|
7
|
|
|
|
|
8
|
|
|
|
|
9
|
|
|
class TestFindRotation(unittest.TestCase): |
|
10
|
|
|
'''Test rotation TestCase''' |
|
11
|
|
|
|
|
12
|
|
|
def setUp(self): |
|
13
|
|
|
'''Setup test''' |
|
14
|
|
|
angle = 0.3 |
|
15
|
|
|
self.rotation = np.array([[np.cos(angle), np.sin(angle), 0], |
|
16
|
|
|
[-np.sin(angle), np.cos(angle), 0], |
|
17
|
|
|
[0, 0, 1]]) |
|
18
|
|
|
|
|
19
|
|
|
data = np.random.random_sample( |
|
20
|
|
|
[200, 3]) * [5.0, 10.0, 1.0] - [2.5, 5, 0.5] |
|
21
|
|
|
self.pc1 = pcl.PointCloudXYZRGB(data.astype(np.float32)) |
|
22
|
|
|
|
|
23
|
|
|
self.pc2 = pcl.PointCloudXYZRGB(data.astype(np.float32)) |
|
24
|
|
|
self.pc2.rotate(self.rotation) |
|
25
|
|
|
|
|
26
|
|
|
def test_perfectly_aligned(self): |
|
27
|
|
|
'''Test find_rotation_xy function''' |
|
28
|
|
|
rotation = find_rotation_xy(self.pc1, self.pc1) |
|
29
|
|
|
assert_almost_equal(rotation, np.eye(3)) |
|
30
|
|
|
|
|
31
|
|
|
def test_rotated(self): |
|
32
|
|
|
'''Test find_rotation_xy function''' |
|
33
|
|
|
rotation = find_rotation_xy(self.pc1, self.pc2) |
|
34
|
|
|
w1, v1 = np.linalg.eig(rotation[0:2, 0:2]) |
|
35
|
|
|
w2, v2 = np.linalg.eig(self.rotation[0:2, 0:2]) |
|
36
|
|
|
# assert_almost_equal( rotation, self.rotation ) |
|
37
|
|
|
|
NLeSC /
PattyAnalytics
