Debian Codegraph @

analyses/mrcal-convert-lensmodel-from-kalibr-fov

raw
1 #!/usr/bin/env python3
2
3 r'''Convert a kalibr 1-parameter distortion model to something more standard
4
5 SYNOPSIS
6
7 $ analyses/mrcal-convert-lensmodel-from-kalibr-fov \
8 --num-trials 10 \
9 --radius 0 \
10 --viz LENSMODEL_OPENCV8 \
11 640 480 251.1 249.4 325.4 238.1 0.91
12
13 RMS error of this solution: 0.038970851989073026 pixels.
14 RMS error of this solution: 0.312567345983669 pixels.
15 RMS error of this solution: 0.03897085198907192 pixels.
16 RMS error of this solution: 0.31102107617891206 pixels.
17 RMS error of this solution: 0.03897085198907148 pixels.
18 RMS error of this solution: 0.31024552395361005 pixels.
19 RMS error of this solution: 0.038970851989072686 pixels.
20 RMS error of this solution: 0.03897085198907228 pixels.
21 RMS error of this solution: 0.31256734598368013 pixels.
22 RMS error of this solution: 232.49658418723286 pixels.
23 RMS error of the BEST solution: 0.03897085198907148 pixels.
24 # generated on 2025-11-10 23:53:53 with ./mrcal-convert-lensmodel-from-kalibr-fov --num-trials 10 --radius 0 --viz LENSMODEL_OPENCV8 640 480 251.1 249.4 325.4 238.1 0.91
25 {
26 'lensmodel': 'LENSMODEL_OPENCV8',
27
28 # intrinsics are fx,fy,cx,cy,distortion0,distortion1,....
29 'intrinsics': [ 269.7069092, 267.8824348, 325.3977834, 238.1017166, 0.4354014227, 0.02216823609, -1.263764376e-06, 1.317529587e-06, 6.778953407e-05, 0.7476900385, 0.09048943655, 0.001241101467,],
30
31 'rt_cam_ref': [ 0, 0, 0, 0, 0, 0,],
32 'extrinsics': [ 0, 0, 0, 0, 0, 0,], # for compatibility with mrcal < 2.5
33
34 'imagersize': [ 640, 480,],
35 }
36
37 This is a cut down 'mrcal-convert-lensmodel --sampled' for the kalibr "fov"
38 model specifically. This model isn't supported by mrcal, so that tool couldn't
39 be used directly. This is a quick hack to interoperate with tools that use this
40 model.
41
42 The kalibr models are documented here:
43
44 https://github.com/ethz-asl/kalibr/wiki/supported-models
45
46 See the docs for mrcal-convert-lensmodel for usage details
47
48 '''
49
50
51
52 import sys
53 import argparse
54 import re
55 import os
56
57 def parse_args():
58
59 parser = \
60 argparse.ArgumentParser(description = __doc__,
61 formatter_class=argparse.RawDescriptionHelpFormatter)
62
63 parser.add_argument('--gridn',
64 type=int,
65 default = (30,20),
66 nargs = 2,
67 help='''How densely we should sample the imager. By default we use
68 a 30x20 grid''')
69 parser.add_argument('--where',
70 type=float,
71 nargs=2,
72 help='''Used with or without --sampled. I use a subset
73 of the imager to compute the fit. The active region is a
74 circle centered on this point. If omitted, we will focus
75 on the center of the imager''')
76 parser.add_argument('--radius',
77 type=float,
78 help='''Used with or without --sampled. I use a subset
79 of the imager to compute the fit. The active region is a
80 circle with a radius given by this parameter. If radius
81 == 0, I'll use the whole imager for the fit. If radius <
82 0, this parameter specifies the width of the region at
83 the corners that I should ignore: I will use
84 sqrt(width^2 + height^2)/2. - abs(radius). This is valid
85 ONLY if we're focusing at the center of the imager. By
86 default I ignore a large-ish chunk area at the corners.''')
87
88 parser.add_argument('--viz',
89 action='store_true',
90 help='''Visualize the differences between the input and
91 output models''')
92 parser.add_argument('--cbmax',
93 type=float,
94 default=4,
95 help='''Maximum range of the colorbar''')
96 parser.add_argument('--title',
97 type=str,
98 default = None,
99 help='''Used if --viz. Title string for the diff plot.
100 Overrides the default title. Exclusive with
101 --extratitle''')
102 parser.add_argument('--extratitle',
103 type=str,
104 default = None,
105 help='''Used if --viz. Additional string for the plot to
106 append to the default title. Exclusive with --title''')
107 parser.add_argument('--hardcopy',
108 type=str,
109 help='''Used if --viz. Write the diff output to disk,
110 instead of making an interactive plot''')
111 parser.add_argument('--terminal',
112 type=str,
113 help=r'''Used if --viz. gnuplotlib terminal. The default
114 is good almost always, so most people don't need this
115 option''')
116 parser.add_argument('--set',
117 type=str,
118 action='append',
119 help='''Used if --viz. Extra 'set' directives to
120 gnuplotlib. Can be given multiple times''')
121 parser.add_argument('--unset',
122 type=str,
123 action='append',
124 help='''Used if --viz. Extra 'unset' directives to
125 gnuplotlib. Can be given multiple times''')
126
127 parser.add_argument('--force', '-f',
128 action='store_true',
129 default=False,
130 help='''By default existing models on disk are not
131 overwritten. Pass --force to overwrite them without
132 complaint''')
133 parser.add_argument('--outdir',
134 type=lambda d: d if os.path.isdir(d) else \
135 parser.error(f"--outdir requires an existing directory as the arg, but got '{d}'"),
136 help='''Directory to write the output into. If omitted,
137 we use the directory of the input model''')
138 parser.add_argument('--num-trials',
139 type = int,
140 default = 1,
141 help='''If given, run the solve more than once. Useful
142 in case random initialization produces noticeably
143 different results. By default we run just one trial''')
144 parser.add_argument('to',
145 type=str,
146 help='The target lens model')
147
148 parser.add_argument('w_h_fx_fy_cx_cy_s',
149 type=float,
150 nargs=7,
151 help='''Input camera model. This is "kalibr fov"
152 parameters. mrcal doesn't support this, so I use these
153 manually''')
154
155 args = parser.parse_args()
156
157 if args.title is not None and \
158 args.extratitle is not None:
159 print("Error: --title and --extratitle are exclusive", file=sys.stderr)
160 sys.exit(1)
161
162 return args
163
164 args = parse_args()
165
166 # arg-parsing is done before the imports so that --help works without building
167 # stuff, so that I can generate the manpages and README
168
169
170
171 # I import the LOCAL mrcal
172 sys.path[:0] = f"{os.path.dirname(os.path.realpath(__file__))}/..",
173
174 import numpy as np
175 import numpysane as nps
176 import time
177 import copy
178 import mrcal
179
180
181
182 def fov_unproject_normalized( q, fxy, cxy, s ):
183
184 # The reference implementation is here:
185 # https://github.com/ethz-asl/kalibr/blob/1f60227442d25e36365ef5f72cd80b9666d73467/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/FovDistortion.hpp#L89
186
187 # kMaxValidAngle is defined here:
188 # https://github.com/ethz-asl/kalibr/blob/1f60227442d25e36365ef5f72cd80b9666d73467/aslam_cv/aslam_cameras/include/aslam/cameras/FovDistortion.hpp#L146
189
190
191 # q has shape (..., 2)
192
193 # shape (..., 2)
194 xy = (q - cxy)/fxy
195
196 # shape (...)
197 r_d = nps.mag(xy)
198
199 i_invalid = r_d < 1e-6
200 r_d[i_invalid] = 1. # to not /0. Will do the "right" thing in a bit
201
202 mul2tanwby2 = 2.0 * np.tan(s / 2.0)
203
204 # shape (...)
205 r_u = np.tan(r_d * s) / mul2tanwby2 / r_d
206
207 # if r_d is ~0 then
208 # np.tan(r_d * s) / mul2tanwby2 / r_d ~
209 # np.tan(r_d * s) / r_d / mul2tanwby2 ~
210 # np.tan(r_d * s) / (r_d*s) * s / mul2tanwby2 ~
211 # s / mul2tanwby2 ~
212 r_u[i_invalid] = s / mul2tanwby2
213
214 # shape (...,2)
215 xy = \
216 nps.dummy(r_u, axis = -1) * \
217 xy
218
219 v = nps.glue(xy, np.ones(xy.shape[:-1] + (1,)),
220 axis = -1)
221
222 # normalize
223 v /= nps.dummy(nps.mag(v), axis=-1)
224
225 return v
226
227
228
229
230
231
232 lensmodel_to = args.to
233
234 try:
235 meta = mrcal.lensmodel_metadata_and_config(lensmodel_to)
236 except Exception as e:
237 print(f"Invalid lens model '{lensmodel_to}': couldn't get the metadata: {e}",
238 file=sys.stderr)
239 sys.exit(1)
240 if not meta['has_gradients']:
241 print(f"lens model {lensmodel_to} is not supported at this time: its gradients aren't implemented",
242 file=sys.stderr)
243 sys.exit(1)
244
245 try:
246 Ndistortions = mrcal.lensmodel_num_params(lensmodel_to) - 4
247 except:
248 print(f"Unknown lens model: '{lensmodel_to}'", file=sys.stderr)
249 sys.exit(1)
250
251
252
253 file_output = sys.stdout
254
255 dims = np.array(args.w_h_fx_fy_cx_cy_s[:2],
256 dtype = np.int32)
257
258 if args.radius is None:
259 # By default use 1/4 of the smallest dimension
260 args.radius = -np.min(dims) // 4
261 print(f"Default radius: {args.radius}. We're ignoring the regions {-args.radius} pixels from each corner",
262 file=sys.stderr)
263 if args.where is not None and \
264 nps.norm2(args.where - (dims - 1.) / 2) > 1e-3:
265 print("A radius <0 is only implemented if we're focusing on the imager center: use an explicit --radius, or omit --where",
266 file=sys.stderr)
267 sys.exit(1)
268
269
270 # Alrighty. Let's actually do the work. I do this:
271 #
272 # 1. Sample the imager space with the known model
273 # 2. Unproject to get the 3d observation vectors
274 # 3. Solve a new model that fits those vectors to the known observations, but
275 # using the new model
276
277 ### I sample the pixels in an NxN grid
278 Nx,Ny = args.gridn
279
280 qx = np.linspace(0, dims[0]-1, Nx)
281 qy = np.linspace(0, dims[1]-1, Ny)
282
283 # q is (Ny*Nx, 2). Each slice of q[:] is an (x,y) pixel coord
284 q = np.ascontiguousarray( nps.transpose(nps.clump( nps.cat(*np.meshgrid(qx,qy)), n=-2)) )
285 if args.radius != 0:
286 # we use a subset of the input data for the fit
287 if args.where is None:
288 focus_center = (dims - 1.) / 2.
289 else:
290 focus_center = args.where
291
292 if args.radius > 0:
293 r = args.radius
294 else:
295 if nps.norm2(focus_center - (dims - 1.) / 2) > 1e-3:
296 print("A radius <0 is only implemented if we're focusing on the imager center",
297 file=sys.stderr)
298 sys.exit(1)
299 r = nps.mag(dims)/2. + args.radius
300
301 grid_off_center = q - focus_center
302 i = nps.norm2(grid_off_center) < r*r
303 q = q[i, ...]
304
305
306 # To visualize the sample grid:
307 # import gnuplotlib as gp
308 # gp.plot(q[:,0], q[:,1], _with='points pt 7 ps 2', xrange=[0,3904],yrange=[3904,0], wait=1, square=1)
309 # sys.exit()
310
311 ### I unproject this, with broadcasting
312 # shape (Ny*Nx, 3)
313 p = fov_unproject_normalized( q,
314 np.array(args.w_h_fx_fy_cx_cy_s[2:4]),
315 np.array(args.w_h_fx_fy_cx_cy_s[4:6]),
316 args.w_h_fx_fy_cx_cy_s[6] )
317
318 # Ignore any failed unprojections
319 i_finite = np.isfinite(p[:,0])
320 p = p[i_finite]
321 q = q[i_finite]
322 Npoints = len(q)
323 weights = np.ones((Npoints,), dtype=float)
324
325 ### Solve!
326
327 ### I solve the optimization a number of times with different random seed
328 ### values, taking the best-fitting results. This is required for the richer
329 ### models such as LENSMODEL_OPENCV8
330 err_rms_best = 1e10
331 intrinsics_data_best = None
332 rt_cam_ref_best = None
333
334 for i in range(args.num_trials):
335
336 # random seed for the new intrinsics
337 intrinsics_core = np.array(args.w_h_fx_fy_cx_cy_s[2:6])
338 distortions = (np.random.rand(Ndistortions) - 0.5) * 1e-3 # random initial seed
339 intrinsics_to_values = nps.dummy(nps.glue(intrinsics_core, distortions, axis=-1),
340 axis=-2)
341 # each point has weight 1.0
342 observations_points = nps.glue(q, nps.transpose(weights), axis=-1)
343 observations_points = np.ascontiguousarray(observations_points) # must be contiguous. mrcal.optimize() should really be more lax here
344
345 # Which points we're observing. This is dense and kinda silly for this
346 # application. Each slice is (i_point,i_camera,i_camera-1). Initially O
347 # do everything in camera-0 coordinates, and I do not move the
348 # extrinsics
349 indices_point_camintrinsics_camextrinsics = np.zeros((Npoints,3), dtype=np.int32)
350 indices_point_camintrinsics_camextrinsics[:,0] = \
351 np.arange(Npoints, dtype=np.int32)
352 indices_point_camintrinsics_camextrinsics[:,1] = 0
353 indices_point_camintrinsics_camextrinsics[:,2] = -1
354
355 optimization_inputs = \
356 dict(intrinsics = intrinsics_to_values,
357 rt_cam_ref = None,
358 rt_ref_frame = None, # no frames. Just points
359 points = p,
360 observations_board = None, # no board observations
361 indices_frame_camintrinsics_camextrinsics = None, # no board observations
362 observations_point = observations_points,
363 indices_point_camintrinsics_camextrinsics = indices_point_camintrinsics_camextrinsics,
364 lensmodel = lensmodel_to,
365
366 imagersizes = nps.atleast_dims(dims, -2),
367
368 # I'm not optimizing the point positions (frames), so these
369 # need to be set to be inactive, and to include the ranges I do
370 # have
371 point_min_range = 1e-3,
372 point_max_range = 1e3,
373
374 # I optimize the lens parameters. That's the whole point
375 do_optimize_intrinsics_core = True,
376 do_optimize_intrinsics_distortions = True,
377
378 do_optimize_extrinsics = False,
379
380 # NOT optimizing the observed point positions
381 do_optimize_frames = False )
382
383 if re.match("LENSMODEL_SPLINED_STEREOGRAPHIC_", lensmodel_to):
384 # splined models have a core, but those variables are largely redundant
385 # with the spline parameters. So I lock down the core when targetting
386 # splined models
387 optimization_inputs['do_optimize_intrinsics_core'] = False
388
389 stats = mrcal.optimize(**optimization_inputs,
390 # No outliers. I have the points that I have
391 do_apply_outlier_rejection = False,
392 verbose = False)
393
394 err_rms = stats['rms_reproj_error__pixels']
395 print(f"RMS error of this solution: {err_rms} pixels.",
396 file=sys.stderr)
397 if err_rms < err_rms_best:
398 err_rms_best = err_rms
399 intrinsics_data_best = optimization_inputs['intrinsics'][0,:].copy()
400
401 if intrinsics_data_best is None:
402 print("No valid intrinsics found!", file=sys.stderr)
403 sys.exit(1)
404
405 if args.num_trials > 1:
406 print(f"RMS error of the BEST solution: {err_rms_best} pixels.",
407 file=sys.stderr)
408
409
410 m_to = mrcal.cameramodel( intrinsics = (lensmodel_to, intrinsics_data_best.ravel()),
411 imagersize = dims )
412
413 note = \
414 "generated on {} with {}\n". \
415 format(time.strftime("%Y-%m-%d %H:%M:%S"),
416 ' '.join(mrcal.shellquote(s) for s in sys.argv))
417
418 m_to.write(file_output, note=note)
419
420
421
422
423
424
425
426 if isinstance(file_output, str):
427 print(f"Wrote '{file_output}'",
428 file=sys.stderr)
429
430
431 if args.viz:
432
433 plotkwargs_extra = {}
434 if args.set is not None:
435 plotkwargs_extra['set'] = args.set
436 if args.unset is not None:
437 plotkwargs_extra['unset'] = args.unset
438
439 if args.title is not None:
440 plotkwargs_extra['title'] = args.title
441
442
443 # I compute the reprojections again. Similar to the code used in the solve,
444 # but the spacing might be different AND I do not ignore corners
445 gridn_width,gridn_height = 80,50
446
447 qx = np.linspace(0, dims[0]-1, gridn_width)
448 qy = np.linspace(0, dims[1]-1, gridn_height)
449
450 # shape (Ny,Nx,2)
451 q = np.ascontiguousarray( nps.mv( nps.cat(*np.meshgrid(qx,qy)),
452 0,-1))
453
454 p = fov_unproject_normalized( q,
455 np.array(args.w_h_fx_fy_cx_cy_s[2:4]),
456 np.array(args.w_h_fx_fy_cx_cy_s[4:6]),
457 args.w_h_fx_fy_cx_cy_s[6] )
458 q_to = mrcal.project(p, *m_to.intrinsics())
459 diff = q - q_to
460 difflen = nps.mag(diff)
461
462
463
464
465
466
467
468
469
470
471
472 import gnuplotlib as gp
473
474 plotkwargs_extra['hardcopy'] = args.hardcopy
475 plotkwargs_extra['terminal'] = args.terminal
476
477 contour_increment = None
478 contour_labels_styles = 'boxed'
479 contour_labels_font = None
480
481
482
483 if 'title' not in plotkwargs_extra:
484 title = f"Diff in fitted fov model to {lensmodel_to}"
485 if args.extratitle is not None:
486 title += ": " + args.extratitle
487 plotkwargs_extra['title'] = title
488
489
490 plot_options = plotkwargs_extra
491 gp.add_plot_option(plot_options,
492 cbrange = [0,args.cbmax])
493 color = difflen
494
495 # Any invalid values (nan or inf) are set to an effectively infinite
496 # difference
497 color[~np.isfinite(color)] = 1e6
498
499 curve_options = \
500 mrcal.visualization._options_heatmap_with_contours(
501 # update these plot options
502 plotkwargs_extra,
503
504 contour_max = args.cbmax,
505 contour_increment = contour_increment,
506 imagersize = args.w_h_fx_fy_cx_cy_s[:2],
507 gridn_width = gridn_width,
508 gridn_height = gridn_height,
509 contour_labels_styles = contour_labels_styles,
510 contour_labels_font = contour_labels_font,
511 do_contours = True)
512
513 plot_data_args = [ (color, curve_options) ]
514
515
516 data_tuples = plot_data_args
517
518 plot = gp.gnuplotlib(**plot_options)
519 plot.plot(*data_tuples)
520
521 if args.hardcopy is None:
522 plot.wait()