Debian Codegraph @

mrcal-from-kalibr

raw
1 #!/usr/bin/env python3
2
3 # Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
4 # Government sponsorship acknowledged. All rights reserved.
5 #
6 # Licensed under the Apache License, Version 2.0 (the "License");
7 # You may obtain a copy of the License at
8 #
9 # http://www.apache.org/licenses/LICENSE-2.0
10
11 r'''Converts a kalibr-formatted camera model to the .cameramodel file format
12
13 SYNOPSIS
14
15 $ cat model.yaml
16
17 cam0:
18 camera_model: pinhole
19 intrinsics: [461.629, 460.152, 362.680, 246.049]
20 distortion_model: radtan
21 distortion_coeffs: [-0.27695497, 0.06712482, 0.00087538, 0.00011556]
22 T_cam_imu:
23 - [0.01779318, 0.99967549,-0.01822936, 0.07008565]
24 - [-0.9998017, 0.01795239, 0.00860714,-0.01771023]
25 - [0.00893160, 0.01807260, 0.99979678, 0.00399246]
26 - [0.0, 0.0, 0.0, 1.0]
27 timeshift_cam_imu: -8.121e-05
28 rostopic: /cam0/image_raw
29 resolution: [752, 480]
30
31
32 $ mrcal-from-kalibr model.yaml
33
34 Wrote model-cam0.cameramodel
35
36
37 $ cat model-cam0.cameramodel
38
39 {
40 'lensmodel': 'LENSMODEL_OPENCV4',
41
42 # intrinsics are fx,fy,cx,cy,distortion0,distortion1,....
43 'intrinsics': [ 461.629, 460.152, 362.68, 246.049, -0.27695497, 0.06712482, 0.00087538, 0.00011556,],
44
45 'rt_cam_ref': [ 0, 0, 0, 0, 0, 0,],
46 'extrinsics': [ 0, 0, 0, 0, 0, 0,], # for compatibility with mrcal < 2.5
47
48 'imagersize': [ 752, 480,],
49 }
50
51 File formats supported by mrcal are described at
52 https://mrcal.secretsauce.net/cameramodels.html#cameramodel-file-formats
53
54 This tool converts the given model(s) to the mrcal-native .cameramodel file
55 format. No changes to the content are made; this is purely a format converter
56 (the mrcal-convert-lensmodel tool fits different lens models instead). Model
57 filenames are given on the commandline. Output is written to the same directory,
58 with the same filename, but annotated with the camera name from the data file,
59 and with a .cameramodel extension.
60
61 If the model is omitted or given as "-", the input is read from standard input,
62 and the output is written to standard output
63
64 Unlike mrcal .cameramodel files where one camera is described by one file, the
65 .yaml files used by kalibr are intended to describe multiple cameras. This
66 format conversion tool will write out multiple .cameramodel file to represent
67 the given data. To select a single camera, pass --camera CAMERA.
68
69 By default we set the extrinsics of camera-0 as they appear in the input: the
70 "previous" camera from camera-0 is used as the reference coordinate system. If
71 we want to force camera-0 to have an identity transform, pass
72 --cam0-at-reference. Usually this will be the case anyway in the input data, but
73 this option makes sure.
74
75 The kalibr format is described at
76 https://github.com/ethz-asl/kalibr/wiki/Yaml-formats
77
78 At this time we only support
79
80 - LENSMODEL_PINHOLE
81 - LENSMODEL_OPENCV4
82 - LENSMODEL_CAHVOR (experimental)
83 - LENSMODEL_CAHVORE (experimental)
84
85 which corresponds to
86
87 camera_model == 'pinhole' and distortion_model in ('radtan','none')
88
89 '''
90
91 import sys
92 import argparse
93 import re
94 import os
95
96 def parse_args():
97
98 parser = \
99 argparse.ArgumentParser(description = __doc__,
100 formatter_class=argparse.RawDescriptionHelpFormatter)
101
102 parser.add_argument('--cam0-at-reference',
103 action='store_true',
104 default=False,
105 help='''By default we set the extrinsics of camera-0 as
106 they appear in the input. If we want to force camera-0
107 to have an identity transform, pass --cam0-at-reference.
108 Usually this will be the case anyway in the input data,
109 but this option makes sure''')
110 parser.add_argument('--camera',
111 help='''Kalibr model files may contain multiple cameras,
112 selected by this argument. If omitted, all available
113 cameras will be read. If given, we read JUST the one
114 requested camera''')
115 parser.add_argument('--force', '-f',
116 action='store_true',
117 default=False,
118 help='''By default existing files are not overwritten. Pass --force to overwrite them
119 without complaint''')
120 parser.add_argument('--outdir',
121 required=False,
122 type=lambda d: d if os.path.isdir(d) else \
123 parser.error("--outdir requires an existing directory as the arg, but got '{}'".format(d)),
124 help='''Directory to write the output models into. If omitted, we write the output
125 models to the same directory as the input models''')
126 parser.add_argument('model',
127 default=['-'],
128 nargs='*',
129 type=str,
130 help='''Input camera model; '-' to read standard input''')
131
132 args = parser.parse_args()
133
134 if len(args.model) > 1:
135 print("ERROR: only one input file may be processed", file=sys.stderr)
136 sys.exit(1)
137 args.model = args.model[0]
138 return args
139
140 args = parse_args()
141
142 # arg-parsing is done before the imports so that --help works without building
143 # stuff, so that I can generate the manpages and README
144
145
146 import mrcal
147 import mrcal.cahvor
148 import yaml
149 import numpy as np
150 import numpysane as nps
151
152
153
154 Rt_camprev_ref = None
155
156 # stolen from mrcal-to-kalibr. Please consolidate
157 def Rt_is_identity(Rt):
158 cos_th = (np.trace(Rt[:3,:]) - 1.) / 2.
159 # cos_th ~ 1 - x^2/2
160 th_sq = (1 - cos_th)*2.
161 norm2_t = nps.norm2(Rt[3,:])
162 return norm2_t < 1e-12 and th_sq < 1e-12
163
164 def convert_one(d,name):
165
166 global Rt_camprev_ref
167
168
169 # I expect at least this structure:
170 #
171 # camera_model: pinhole
172 # intrinsics: [461.629, 460.152, 362.680, 246.049]
173 # distortion_model: radtan
174 # distortion_coeffs: [-0.27695497, 0.06712482, 0.00087538, 0.00011556]
175 # resolution: [752, 480]
176 # T_cn_cnm1:
177 # - [ 0.99998854, 0.00216014, 0.00427195,-0.11003785]
178 # - [-0.00221074, 0.99992702, 0.01187697, 0.00045792]
179 # - [-0.00424598,-0.01188627, 0.99992034,-0.00064487]
180 # - [0.0, 0.0, 0.0, 1.0]
181 #
182 # T_cn_cnm1 may be omitted: I use an identity transform in that case
183 camera_model_known = set(('pinhole',
184 'cahvor','cahvore'),)
185 if not d['camera_model'] in camera_model_known:
186 print(f"ERROR: at this time I only support kalibr camera_model: {camera_model_known}; got camera_model={d['camera_model']}",
187 file=sys.stderr)
188 sys.exit(1)
189
190 if not re.match('cahvor',d['camera_model']):
191
192 # The "usual" path
193 distortion_model_known = set(('radtan','none'),)
194 if not d['distortion_model'] in distortion_model_known:
195 print(f"ERROR: at this time I only support kalibr distortion_model: {distortion_model_known}; got distortion_model={d['distortion_model']}",
196 file=sys.stderr)
197 sys.exit(1)
198
199 if d['distortion_model'] == 'radtan':
200 intrinsics = ('LENSMODEL_OPENCV4',
201 np.array(d['intrinsics'] +
202 d['distortion_coeffs'],
203 dtype=float))
204 else:
205 intrinsics = ('LENSMODEL_PINHOLE',
206 np.array(d['intrinsics'],
207 dtype=float))
208
209 m = mrcal.cameramodel( intrinsics = intrinsics,
210 imagersize = np.array(d['resolution'],
211 dtype=int) )
212
213 else:
214 # cahvor or cahvore. This can only come from the hacked kalibr at jpl
215 if 'distortion_model' in d:
216 print(f"ERROR: camera_model={d['camera_model']}, so no distortion_model should have been given, but we have distortion_model={d['distortion_model']}",
217 file=sys.stderr)
218 sys.exit(1)
219
220 intrinsics = np.array(d['intrinsics'])
221
222 if d['camera_model'] == 'cahvor':
223 if intrinsics.size != 6*3:
224 print(f"ERROR: camera_model='cahvor' should have {6*3} intrinsics values, but got {intrinsics.size}",
225 file=sys.stderr)
226 sys.exit(1)
227
228 C,A,H,V,O,R = intrinsics.reshape(6,3)
229 E = None
230
231 else:
232 # cahvore
233 if intrinsics.size != 7*3:
234 print(f"ERROR: camera_model='cahvor' should have {7*3} intrinsics values, but got {intrinsics.size}",
235 file=sys.stderr)
236 sys.exit(1)
237
238 C,A,H,V,O,R,E = intrinsics.reshape(7,3)
239
240 try:
241 m = mrcal.cahvor._construct_model(C,A,H,V,O,R,E,
242 is_cahvor_or_cahvore = True,
243 is_cahvore = E is not None,
244 cahvore_linearity = d.get('cahvore_linearity',0.37),
245 Dimensions = np.array(d['resolution'],dtype=np.int32),
246 name = name)
247 except Exception as e:
248 print(f"ERROR: couldn't parse cahvor(e) model: {e}",
249 file=sys.stderr)
250 sys.exit(1)
251
252 if not Rt_is_identity(m.Rt_cam_ref()):
253 print(f"ERROR: given cahvor(e) model has non-identity extrinsics. These may conflict with the extrinsics in the rest of the kalibr file",
254 file=sys.stderr)
255 sys.exit(1)
256
257
258 if not 'T_cn_cnm1' in d:
259 Rt_cam_camprev = mrcal.identity_Rt()
260
261 else:
262 T_cn_cnm1 = np.array(d['T_cn_cnm1'])
263 if T_cn_cnm1.shape != (4,4):
264 print(f"ERROR: T_cn_cnm1 must be a 4x4 array; got T_cn_cnm1.shape={T_cn_cnm1.shape}",
265 file=sys.stderr)
266 sys.exit()
267 if np.any(T_cn_cnm1[3,:] - np.array((0,0,0,1))):
268 print(f"ERROR: T_cn_cnm1 must have a last row of (0,0,0,1); got {T_cn_cnm1[3,:]}",
269 file=sys.stderr)
270 sys.exit()
271
272 # The documentation is here:
273 #
274 # https://github.com/ethz-asl/kalibr/wiki/Yaml-formats
275 #
276 # It says:
277 #
278 # T_cn_cnm1 camera extrinsic transformation, always with respect to the
279 # last camera in the chain (e.g. cam1: T_cn_cnm1 = T_c1_c0, takes cam0 to
280 # cam1 coordinates)
281 #
282 # This isn't clear. I assume "last" means "previous"
283 R_cam_camprev = T_cn_cnm1[:3,:3]
284 if nps.norm2((nps.matmult(R_cam_camprev, R_cam_camprev.T) - np.eye(3)).ravel()) > 1e-12:
285 print(f"ERROR: T_cn_cnm1[:3,:3] must be a valid rotation. Instead it is {T_cn_cnm1[:3,:3]}",
286 file=sys.stderr)
287 sys.exit(1)
288 t_cam_camprev = T_cn_cnm1[:3, 3]
289 Rt_cam_camprev = nps.glue(R_cam_camprev,t_cam_camprev, axis=-2)
290
291 if Rt_camprev_ref is not None:
292 Rt_cam_ref = mrcal.compose_Rt(Rt_cam_camprev, Rt_camprev_ref)
293 else:
294 # This is the first camera. What do we use as our reference?
295 if args.cam0_at_reference:
296 Rt_cam_ref = mrcal.identity_Rt()
297 else:
298 # By default the "prev cam" from cam0 is the reference
299 Rt_cam_ref = Rt_cam_camprev
300
301
302 Rt_camprev_ref = Rt_cam_ref # for the next one
303
304 m.Rt_cam_ref(Rt_cam_ref)
305 return m
306
307
308 def convert_all(f, which):
309
310 # which is None = "all cameras"
311
312 try:
313 D = yaml.safe_load(f)
314 except yaml.scanner.ScannerError as e:
315 print("Error parsing YAML:\n\n")
316 print(e)
317 sys.exit(1)
318
319 if len(D) == 0:
320 print("Zero models read from input",
321 file=sys.stderr)
322 sys.exit(1)
323
324 if 'camera_model' in D:
325 # one camera; stored inline
326 d = D
327 if which is not None:
328 print(f"Error: a single in-line model found in the file, but a specific camera is requested: '{which}'. Omit --camera",
329 file=sys.stderr)
330 sys.exit(1)
331 try:
332 return ( \
333 (None, convert_one(d,'camera')),
334 )
335 except KeyError as e:
336 print(f"Error parsing input; missing key: {e}", file=sys.stderr)
337 sys.exit(1)
338
339 if which is not None and \
340 not which in D:
341 print(f"Error: asked for camera '{which}', but the given file contains only these cameras: {tuple(D.keys())}",
342 file=sys.stderr)
343 sys.exit(1)
344
345
346 # The extrinsics are specified as a serial chain. So I need to parse all the
347 # cameras, even if I only need to save one
348 models = dict()
349 for name,d in D.items():
350 try:
351 models[name] = convert_one(d,name)
352 except KeyError as e:
353 print(f"Error parsing input; missing key: {e}", file=sys.stderr)
354 sys.exit(1)
355
356 if which is None:
357 # all cameras requested
358 return [ (name, models[name]) \
359 for name in models.keys() ]
360
361 return ( \
362 (which, models[which]),
363 )
364
365
366
367 if args.model == '-':
368 if sys.stdin.isatty():
369 # This isn't an error per-se. But most likely the user ran this
370 # without redirecting any data into it. Without this check the
371 # program will sit there, waiting for input. Which will look strange
372 # to an unsuspecting user
373 print("Trying to read a model from standard input, but no file is being redirected into it",
374 file=sys.stderr)
375 sys.exit(1)
376
377 names_models = convert_all(sys.stdin, which = args.camera)
378 if len(names_models) > 1:
379 print(f"Reading from standard input may only produce ONE model on stdout, but got more here. Pass --camera to select the camera you want; available cameras: {list(n for n,m in names_models)}",
380 file=sys.stderr)
381 sys.exit(1)
382
383 names_models[0][1].write(sys.stdout)
384
385 else:
386 base,extension = os.path.splitext(args.model)
387 if extension.lower() == '.cameramodel':
388 print("Input file is already in the cameramodel format (judging from the filename). Doing nothing",
389 file=sys.stderr)
390 sys.exit(0)
391
392 if args.outdir is not None:
393 base = args.outdir + '/' + os.path.split(base)[1]
394
395 with open(args.model, "r") as f:
396 names_models = convert_all(f, which = args.camera)
397
398 for name,model in names_models:
399
400 if name is None: name = ''
401 else: name = f"-{name}"
402 filename_out = f"{base}{name}.cameramodel"
403 if not args.force and os.path.exists(filename_out):
404 print(f"Target model '{filename_out}' already exists. Doing nothing with this model. Pass -f to overwrite",
405 file=sys.stderr)
406 continue
407
408 model.write(filename_out)
409 print("Wrote " + filename_out)