|
13 | 13 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
14 | 14 | See the License for the specific language governing permissions and
|
15 | 15 | limitations under the License.
|
16 |
| -""" |
| 16 | +""" |
17 | 17 |
|
18 |
| -import os |
19 | 18 | import argparse
|
20 | 19 | import configparser
|
| 20 | +import os |
| 21 | + |
21 | 22 | import numpy as np
|
22 |
| -from scipy.spatial.transform import Rotation |
23 | 23 | import yaml
|
| 24 | +from scipy.spatial.transform import Rotation |
24 | 25 |
|
25 | 26 | rez = "FHD"
|
26 | 27 | calibration_file = "calibration.yaml"
|
27 | 28 |
|
28 |
| -parser = argparse.ArgumentParser(description='Get factory ZED calibration') |
29 |
| -required = parser.add_argument_group('required arguments') |
30 |
| -required.add_argument('-s','--serial_number', help='serial number of the ZED camera', required=True) |
31 |
| -required.add_argument('-o','--output', help='output calibration file (default: stereo_calibration.yaml)', default="stereo_calibration.yaml") |
32 |
| -required.add_argument('-r','--resoution', help='resolution setting (default: FHD)', default="FHD") |
| 29 | +parser = argparse.ArgumentParser(description="Get factory ZED calibration") |
| 30 | +required = parser.add_argument_group("required arguments") |
| 31 | +required.add_argument( |
| 32 | + "-s", "--serial_number", help="serial number of the ZED camera", required=True |
| 33 | +) |
| 34 | +required.add_argument( |
| 35 | + "-o", |
| 36 | + "--output", |
| 37 | + help="output calibration file (default: stereo_calibration.yaml)", |
| 38 | + default="stereo_calibration.yaml", |
| 39 | +) |
| 40 | +required.add_argument("-r", "--resolution", help="resolution setting (default: FHD)", default="FHD") |
33 | 41 | args = parser.parse_args()
|
34 | 42 |
|
35 |
| -serial_number=args.serial_number |
36 |
| -rez = args.resoution |
| 43 | +serial_number = args.serial_number |
| 44 | +rez = args.resolution |
37 | 45 | calibration_file = args.output
|
38 | 46 | if rez == "2K":
|
39 |
| - width = 2208 |
40 |
| - height = 1242 |
| 47 | + width = 2208 |
| 48 | + height = 1242 |
41 | 49 | elif rez == "FHD":
|
42 |
| - width = 1920 |
43 |
| - height = 1080 |
| 50 | + width = 1920 |
| 51 | + height = 1080 |
44 | 52 | elif rez == "HD":
|
45 |
| - width = 1280 |
46 |
| - height = 720 |
| 53 | + width = 1280 |
| 54 | + height = 720 |
47 | 55 | elif rez == "VGA":
|
48 |
| - width = 672 |
49 |
| - height = 376 |
| 56 | + width = 672 |
| 57 | + height = 376 |
50 | 58 | else:
|
51 |
| - raise NameError("resolution not supported") |
| 59 | + raise NameError("resolution not supported") |
52 | 60 |
|
53 | 61 |
|
54 |
| -tmp_file = str(serial_number)+".tmp" |
55 |
| -url = "http://calib.stereolabs.com/?SN="+str(serial_number) |
56 |
| -os.system("wget -O"+ tmp_file +" "+url) |
| 62 | +tmp_file = str(serial_number) + ".tmp" |
| 63 | +url = "http://calib.stereolabs.com/?SN=" + str(serial_number) |
| 64 | +os.system("wget -O" + tmp_file + " " + url) |
57 | 65 |
|
58 | 66 | calib = configparser.ConfigParser()
|
59 | 67 | calib.read(tmp_file)
|
60 |
| -M1 = np.zeros([3,3]) |
61 |
| -M1[0,0]=calib['LEFT_CAM_'+rez]['fx'] |
62 |
| -M1[0,2]=calib['LEFT_CAM_'+rez]['cx'] |
63 |
| -M1[1,1]=calib['LEFT_CAM_'+rez]['fy'] |
64 |
| -M1[1,2]=calib['LEFT_CAM_'+rez]['cy'] |
65 |
| -M1[2,2] = 1.0 |
| 68 | +M1 = np.zeros([3, 3]) |
| 69 | +M1[0, 0] = calib["LEFT_CAM_" + rez]["fx"] |
| 70 | +M1[0, 2] = calib["LEFT_CAM_" + rez]["cx"] |
| 71 | +M1[1, 1] = calib["LEFT_CAM_" + rez]["fy"] |
| 72 | +M1[1, 2] = calib["LEFT_CAM_" + rez]["cy"] |
| 73 | +M1[2, 2] = 1.0 |
66 | 74 |
|
67 | 75 | d1 = np.zeros(5)
|
68 |
| -d1[0] = calib['LEFT_CAM_'+rez]['k1'] |
69 |
| -d1[1] = calib['LEFT_CAM_'+rez]['k2'] |
70 |
| -d1[2] = calib['LEFT_CAM_'+rez]['p1'] |
71 |
| -d1[3] = calib['LEFT_CAM_'+rez]['p2'] |
72 |
| -d1[4] = calib['LEFT_CAM_'+rez]['k3'] |
73 |
| - |
74 |
| -M2 = np.zeros([3,3]) |
75 |
| -M2[0,0]=calib['RIGHT_CAM_'+rez]['fx'] |
76 |
| -M2[0,2]=calib['RIGHT_CAM_'+rez]['cx'] |
77 |
| -M2[1,1]=calib['RIGHT_CAM_'+rez]['fy'] |
78 |
| -M2[1,2]=calib['RIGHT_CAM_'+rez]['cy'] |
79 |
| -M2[2,2]=1.0 |
| 76 | +d1[0] = calib["LEFT_CAM_" + rez]["k1"] |
| 77 | +d1[1] = calib["LEFT_CAM_" + rez]["k2"] |
| 78 | +d1[2] = calib["LEFT_CAM_" + rez]["p1"] |
| 79 | +d1[3] = calib["LEFT_CAM_" + rez]["p2"] |
| 80 | +d1[4] = calib["LEFT_CAM_" + rez]["k3"] |
| 81 | + |
| 82 | +M2 = np.zeros([3, 3]) |
| 83 | +M2[0, 0] = calib["RIGHT_CAM_" + rez]["fx"] |
| 84 | +M2[0, 2] = calib["RIGHT_CAM_" + rez]["cx"] |
| 85 | +M2[1, 1] = calib["RIGHT_CAM_" + rez]["fy"] |
| 86 | +M2[1, 2] = calib["RIGHT_CAM_" + rez]["cy"] |
| 87 | +M2[2, 2] = 1.0 |
80 | 88 |
|
81 | 89 | d2 = np.zeros(5)
|
82 |
| -d2[0] = calib['RIGHT_CAM_'+rez]['k1'] |
83 |
| -d2[1] = calib['RIGHT_CAM_'+rez]['k2'] |
84 |
| -d2[2] = calib['RIGHT_CAM_'+rez]['p1'] |
85 |
| -d2[3] = calib['RIGHT_CAM_'+rez]['p2'] |
86 |
| -d2[4] = calib['RIGHT_CAM_'+rez]['k3'] |
| 90 | +d2[0] = calib["RIGHT_CAM_" + rez]["k1"] |
| 91 | +d2[1] = calib["RIGHT_CAM_" + rez]["k2"] |
| 92 | +d2[2] = calib["RIGHT_CAM_" + rez]["p1"] |
| 93 | +d2[3] = calib["RIGHT_CAM_" + rez]["p2"] |
| 94 | +d2[4] = calib["RIGHT_CAM_" + rez]["k3"] |
87 | 95 |
|
88 | 96 | t = np.zeros(3)
|
89 |
| -t[0] = -float(calib['STEREO']['Baseline']) |
90 |
| -t[1] = calib['STEREO']['TY'] |
91 |
| -t[2] = calib['STEREO']['TZ'] |
| 97 | +t[0] = -float(calib["STEREO"]["Baseline"]) |
| 98 | +t[1] = calib["STEREO"]["TY"] |
| 99 | +t[2] = calib["STEREO"]["TZ"] |
92 | 100 |
|
93 | 101 | r = np.zeros(3)
|
94 |
| -r[0] = calib['STEREO']["RX_"+rez] |
95 |
| -r[1] = calib['STEREO']["CV_"+rez] |
96 |
| -r[2] = calib['STEREO']["RZ_"+rez] |
| 102 | +r[0] = calib["STEREO"]["RX_" + rez] |
| 103 | +r[1] = calib["STEREO"]["CV_" + rez] |
| 104 | +r[2] = calib["STEREO"]["RZ_" + rez] |
97 | 105 | R = Rotation.from_rotvec(r).as_matrix()
|
98 | 106 |
|
99 |
| -calibration = dict( |
100 |
| -SN = serial_number, |
101 |
| -M1 = str(M1.flatten().tolist()), |
102 |
| -d1 = str(d1.tolist()), |
103 |
| -M2 = str(M2.flatten().tolist()), |
104 |
| -d2 = str(d2.tolist()), |
105 |
| -t = str(t.tolist()), |
106 |
| -R = str(R.flatten().tolist()), |
107 |
| -width = width, |
108 |
| -height = height |
| 107 | +calibration = dict( |
| 108 | + SN=serial_number, |
| 109 | + M1=str(M1.flatten().tolist()), |
| 110 | + d1=str(d1.tolist()), |
| 111 | + M2=str(M2.flatten().tolist()), |
| 112 | + d2=str(d2.tolist()), |
| 113 | + t=str(t.tolist()), |
| 114 | + R=str(R.flatten().tolist()), |
| 115 | + width=width, |
| 116 | + height=height, |
109 | 117 | )
|
110 | 118 |
|
111 |
| -output_yaml = yaml.dump(calibration, default_style=None, default_flow_style=False, sort_keys=False).replace("'","") |
| 119 | +output_yaml = yaml.dump( |
| 120 | + calibration, default_style=None, default_flow_style=False, sort_keys=False |
| 121 | +).replace("'", "") |
112 | 122 | print("Writing calibration to: " + calibration_file)
|
113 |
| -with open(calibration_file,'w') as f: |
114 |
| - f.write(output_yaml) |
115 |
| - |
116 |
| -os.system("rm "+tmp_file) |
| 123 | +with open(calibration_file, "w") as f: |
| 124 | + f.write(output_yaml) |
117 | 125 |
|
| 126 | +os.system("rm " + tmp_file) |
0 commit comments