-
Notifications
You must be signed in to change notification settings - Fork 1
/
reconstr.py
153 lines (134 loc) · 7.77 KB
/
reconstr.py
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
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
import cv2
import numpy as np
from feature_test import match
from pprint import pprint
def getPts():
imgPts1 = np.array([[774.17749023, 1141.49047852, 548.95800781, 781.43304443, 687.6373291,
776.95220947, 782.07891846, 780.11968994, 762.12304688,
730.68682861, 765.61199951, 793.07611084, 553.9196167, 854.16015625,
533.69543457, 786.14648438, 561.7253418, 810.68432617,
736.19763184, 580.72192383, 698.19104004, 852.4543457, 602.23754883,
629.24688721, 774.89276123, 755.18804932, 641.8661499, 625.20159912,
614.47088623, 692.92315674, 718.87054443, 603.64611816,
770.22924805, 806.85980225, 893.60809326, 875.58215332,
197.52639771, 618.43939209, 224.81550598, 879.64703369,
338.98193359, 876.91516113, 331.44839478, 301.98596191,
886.31951904, 869.93255615, 1141.86987305, 390.36938477, 847.5055542,
207.1446991, 875.07226562, 1142.85046387, 199.10145569,
847.01635742, 901.5279541, 1165.65783691, 1149.50085449,
1156.85253906, 1009.06604004, 1162.39904785, 1014.30609131,
1024.16552734, 904.63885498, 1019.58081055, 177.49160767,
1184.46643066, 1097.89111328, 322.34765625, 465.21520996],
[1308.36657715, 1114.28869629, 728.38775635, 1322.55175781,
884.29730225, 1169.9765625, 767.29217529, 1518.28295898,
1639.99279785, 1427.20043945, 1445.78015137, 693.39483643,
1510.30407715, 1037.76135254, 1644.29711914, 1008.89544678,
560.67266846, 604.52557373, 1728.44677734, 1147.79602051,
700.50726318, 582.24517822, 813.7633667, 1305.99633789,
1330.10595703, 1632.31945801, 721.7857666, 687.74969482,
793.75256348, 1749.80737305, 678.00378418, 1716.55383301,
1185.83679199, 680.65960693, 1033.24487305, 610.5178833, 1526.48962402,
982.43896484, 1503.19506836, 689.5112915, 1024.90234375,
689.28710938, 1088.36010742, 711.03424072, 835.16906738,
914.97991943, 737.91882324, 588.55670166, 1620.31555176,
1519.56677246, 694.54217529, 1534.90405273, 1524.88879395,
1654.16369629, 1278.14135742, 1557.6998291, 1541.97387695,
1523.40734863, 748.75164795, 764.21136475, 2570.34912109,
1388.36853027, 720.50292969, 838.77923584, 1474.72509766,
1547.44433594, 746.6887207, 711.40222168, 823.76916504]])
imgPts2 = np.array([[640.84942627, 951.25042725, 509.85656738, 644.82995605,
598.82629395, 650.41271973, 598.82629395, 636.00073242,
616.41882324, 688.89416504, 627.79876709, 689.89849854,
473.61016846, 509.85656738, 456.75866699, 665.8614502, 440.55587769,
712.35089111, 600.32458496, 505.32192993, 614.64227295,
756.94659424, 539.44281006, 667.996521, 641.57244873,
621.07824707, 506.14151001, 560.54187012, 544.30499268,
592.10217285, 701.52166748, 505.95126343, 645.37713623,
701.52166748, 325.21310425, 796.30993652, 1041.34960938,
539.44281006, 1041.34960938, 409.94830322, 987.21398926,
409.94830322, 987.21398926, 1041.34960938, 173.11088562,
409.94830322, 1041.34960938, 443.53613281, 486.55899048,
1041.34960938, 409.94830322, 1041.34960938, 310.30300903,
486.55899048, 836.94854736, 1041.34960938, 1041.34960938,
1025.74707031, 431.41830444, 294.55352783, 1291.92895508,
405.34124756, 817.0010376, 919.97149658, 294.55352783,
1061.61743164, 361.95922852, 316.06356812, 877.86737061],
[1431.89050293, 1201.2520752, 848.03479004, 1446.1192627, 957.09771729,
1298.11499023, 957.09771729, 1629.02294922, 1738.41748047,
1205.03552246, 1562.47375488, 806.35162354, 1621.80444336,
848.03479004, 1739.70922852, 1135.79663086, 1919.01916504,
704.34954834, 1816.36254883, 1289.70166016, 829.11151123,
656.94995117, 1127.60693359, 1493.75927734, 1452.19604492,
1745.92541504, 1744.02514648, 827.25897217, 938.88665771,
1811.73962402, 788.09698486, 1803.0020752, 1313.25183105,
788.09698486, 1608.04138184, 668.12542725, 1635.58215332,
1127.60693359, 1635.58215332, 1482.93347168, 1119.72192383,
1482.93347168, 1119.72192383, 1635.58215332, 2168.70581055,
1482.93347168, 1635.58215332, 733.61431885, 1929.73522949,
1635.58215332, 1482.93347168, 1635.58215332, 1611.77368164,
1929.73522949, 1120.79101562, 1635.58215332, 1635.58215332,
1579.10461426, 1656.9197998, 1578.91918945, 2119.06616211,
1625.73730469, 1500.64074707, 1497.93652344, 1578.91918945,
1619.22814941, 1602.33435059, 1587.79626465, 769.91656494]])
return imgPts1, imgPts2
def getGoodPts():
im1 = np.array([[774.17749023, 1308.36657715], [781.43304443, 1322.55175781], [776.95220947, 1169.9765625], [780.11968994, 1518.28295898], [762.12304688, 1639.99279785], [765.61199951, 1445.78015137], [793.07611084, 693.39483643], [553.9196167, 1510.30407715], [533.69543457, 1644.29711914], [786.14648438, 1008.89544678], [810.68432617, 604.52557373], [736.19763184, 1728.44677734], [580.72192383, 1147.79602051], [698.19104004, 700.50726318], [852.4543457, 582.24517822], [774.89276123, 1330.10595703], [755.18804932, 1632.31945801], [625.20159912, 687.74969482], [614.47088623, 793.75256348], [603.64611816, 1716.55383301], [770.22924805, 1185.83679199], [806.85980225, 680.65960693], [618.43939209, 982.43896484]])
im2 = np.array([[640.84942627, 1431.89050293], [644.82995605, 1446.1192627], [650.41271973, 1298.11499023], [636.00073242, 1629.02294922], [616.41882324, 1738.41748047], [627.79876709, 1562.47375488], [689.89849854, 806.35162354], [473.61016846, 1621.80444336], [456.75866699, 1739.70922852], [665.8614502, 1135.79663086], [712.35089111, 704.34954834], [600.32458496, 1816.36254883], [505.32192993, 1289.70166016], [614.64227295, 829.11151123], [756.94659424, 656.94995117], [641.57244873, 1452.19604492], [621.07824707, 1745.92541504], [560.54187012, 827.25897217], [544.30499268, 938.88665771], [505.95126343, 1803.0020752], [645.37713623, 1313.25183105], [701.52166748, 788.09698486], [539.44281006, 1127.60693359]])
return im1.T, im2.T
if __name__ == "__main__":
img1_o = cv2.imread("imgs/WP_20160713_002.jpg")
img2_o = cv2.imread("imgs/WP_20160713_004.jpg")
img1 = cv2.pyrDown(img1_o)
img1 = cv2.pyrDown(img1)
img2 = cv2.pyrDown(img2_o)
img2 = cv2.pyrDown(img2)
# good = match("imgs/WP_20160713_002.jpg", "imgs/WP_20160713_004.jpg")
#
# imgPts1 = [np.array(m[0].pt) for m in good]
# imgPts2 = [np.array(m[1].pt) for m in good]
# imgPts1 = np.array(imgPts1).T
# imgPts2 = np.array(imgPts2).T
imgPts1, imgPts2 = getGoodPts()
numPts = imgPts1.shape[1]
# good = []
# for i in range(numPts):
# print(imgPts1[:,i])
# print(imgPts2[:,i])
# p1 = tuple(np.int32(imgPts1[:,i]))
# p2 = tuple(np.int32(imgPts2[:,i]))
#
# width = img1.shape[1]
#
# out = np.zeros((img1.shape[0], width * 2, 3), np.uint8)
# out[:,:width,:] = img1
# out[:,width:,:] = img2
#
# cv2.circle(out, (p1[0]/4, p1[1]/4), 10, (0, 0, 255), 3)
# cv2.circle(out, (p2[0]/4 + width, p2[1]/4), 10, (0, 0, 255), 3)
# cv2.imshow("asd", out)
#
# c = cv2.waitKey()
# if ord('y') == c:
# print("yes")
# good.append((p1, p2))
projMat1 = np.array(
[[ 2.47167840e+03, -4.80618969e+02, 6.01855176e+02, 2.65640522e+04],
[ 2.92673138e+02, 1.63612863e+03, 1.67056274e+03, 4.00796099e+04],
[ 2.25032587e-01, -4.68121905e-01, 8.54530407e-01, 3.65991619e+01]])
projMat2 = np.array(
[[ 1.73156281e+03, -3.45091058e+02, 1.89341510e+03, 2.64188071e+04],
[ -4.89842184e+02, 1.90796353e+03, 1.29347754e+03, 4.62158335e+04],
[ -3.00352985e-01, -3.07844908e-01, 9.02784358e-01, 3.86938810e+01]])
p4d = cv2.triangulatePoints(projMat1, projMat2, imgPts1, imgPts2)
for i in range(0, p4d.shape[1]):
p4d[0][i] /= p4d[3][i]
p4d[1][i] /= p4d[3][i]
p4d[2][i] /= p4d[3][i]
p4d[3][i] /= p4d[3][i]
pprint(p4d)
pprint(imgPts1.T)
for i in range(p4d.shape[1]):
p1 = tuple(np.int32(imgPts1[:,i]))
cv2.circle(img1_o, p1, 5, (0, 0, 255), 2)
cv2.putText(img1_o, str(np.round(p4d[:3,i], 1)), (p1[0] + 5, p1[1] + 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
cv2.imwrite("imgs/out.jpg", img1_o)