1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
| import pandas as pd
| import numpy as np
| import cv2
|
|
|
| def Joint():
|
| matrix12 = np.matrix([[ 0.999994, -2.30179e-005, -0.00338715, 22.9331],
| [2.30258e-005, 1.0, 2.28675e-006, 47.9336],
| [ 0.00338715, -2.36473e-006, 0.999994, -30.3661],
| [ 0.0, 0.0, 0.0, 1.0]])
| matrix13 = np.matrix([[ 0.999988, -1.16345e-005, -0.00490587, 48.7739],
| [1.17334e-005, 1.0, 2.01129e-005, 91.5852],
| [ 0.00490587, -2.01702e-005, 0.999988, -71.2706],
| [ 0.0, 0.0, 0.0, 1.0]])
| matrix14 = np.matrix([[ 0.999985, -2.39867e-005, -0.00554939, 74.1329],
| [2.41246e-005, 1.0, 2.47831e-005, 142.206],
| [ 0.00554939, -2.49166e-005, 0.999985, -113.294],
| [ 0.0, 0.0, 0.0, 1.0]])
| matrix15 = np.matrix([[ 0.999981, -4.08954e-005, -0.00621533, 98.6007],
| [4.11151e-005, 1.0, 3.52216e-005, 192.034],
| [ 0.00621533, -3.54764e-005, 0.999981, -163.256],
| [ 0.0, 0.0, 0.0, 1.0]])
| matrix16 = np.matrix([[ 0.999975, -6.00843e-005, -0.00709497, 129.069],
| [6.03902e-005, 1.0, 4.28944e-005, 236.579],
| [ 0.00709497, -4.33218e-005, 0.999975, -226.242],
| [ 0.0, 0.0, 0.0, 1.0]])
| matrix17 = np.matrix([[ 0.999971, -7.82354e-005, -0.00755777, 154.855],
| [7.86353e-005, 1.0, 5.26223e-005, 282.774],
| [ 0.00755777, -5.32151e-005, 0.999971, -279.706],
| [ 0.0, 0.0, 0.0, 1.0]])
| matrix18 = np.matrix([[ 0.999965, -0.000100226, -0.00832648, 184.996],
| [ 0.000100805, 1.0, 6.90642e-005, 323.342],
| [ 0.00832648, -6.99012e-005, 0.999965, -354.304],
| [ 0.0, 0.0, 0.0, 1.0]])
| matrix22 = np.matrix([[ 0.872123, 0.000773, 0.489285, 4942.74],
| [-0.000745115, 1.0, -0.000251729, -2.8314],
| [ -0.489285, -0.000145035, 0.872124, -598.535],
| [ 0.0, 0.0, 0.0, 1.0]])
| matrix32 = np.matrix([[ -0.869019, -0.00114372, -0.494777, 64430.3],
| [-0.000670692, -0.999994, 0.00348957, 186874.4],
| [ -0.494778, 0.00336434, 0.869013, -325.194],
| [ 0.0, 0.0, 0.0, 1.0]])
|
| data12 = pd.read_csv("RawData\\12.csv");
| print("data12 load completed!")
| data13 = pd.read_csv("RawData\\13.csv");
| print("data13 load completed!")
| data14 = pd.read_csv("RawData\\14.csv");
| print("data14 load completed!")
| data15 = pd.read_csv("RawData\\15.csv");
| print("data15 load completed!")
| data16 = pd.read_csv("RawData\\16.csv");
| print("data16 load completed!")
| data17 = pd.read_csv("RawData\\17.csv");
| print("data17 load completed!")
| data18 = pd.read_csv("RawData\\18.csv");
| print("data18 load completed!")
| data22 = pd.read_csv("RawData\\22.csv");
| print("data22 load completed!")
| data32 = pd.read_csv("RawData\\32.csv");
| print("data32 load completed!")
|
| dataArray12 = np.c_[np.array(data12),np.ones(np.array(data12).shape[0])]
| dataArray13 = np.c_[np.array(data13),np.ones(np.array(data13).shape[0])]
| dataArray14 = np.c_[np.array(data14),np.ones(np.array(data14).shape[0])]
| dataArray15 = np.c_[np.array(data15),np.ones(np.array(data15).shape[0])]
| dataArray16 = np.c_[np.array(data16),np.ones(np.array(data16).shape[0])]
| dataArray17 = np.c_[np.array(data17),np.ones(np.array(data17).shape[0])]
| dataArray18 = np.c_[np.array(data18),np.ones(np.array(data18).shape[0])]
| dataArray22 = np.c_[np.array(data22),np.ones(np.array(data22).shape[0])]
| dataArray32 = np.c_[np.array(data32),np.ones(np.array(data32).shape[0])]
|
|
|
| dataTrans12 = matrix12.dot(dataArray12.transpose())
| dataTrans13 = matrix13.dot(dataArray13.transpose())
| dataTrans14 = matrix14.dot(dataArray14.transpose())
| dataTrans15 = matrix15.dot(dataArray15.transpose())
| dataTrans16 = matrix16.dot(dataArray16.transpose())
| dataTrans17 = matrix17.dot(dataArray17.transpose())
| dataTrans18 = matrix18.dot(dataArray18.transpose())
| dataTrans22 = matrix22.dot(dataArray22.transpose())
| dataTrans32 = matrix32.dot(dataArray32.transpose())
|
| dataTrans = np.r_[dataTrans12.transpose(),
| dataTrans13.transpose(),
| dataTrans14.transpose(),
| dataTrans15.transpose(),
| dataTrans16.transpose(),
| dataTrans17.transpose(),
| dataTrans18.transpose(),
| dataTrans22.transpose(),
| dataTrans32.transpose()]
|
| return dataTrans
|
|
