Debian Codegraph @

analyses/mrcal-reoptimize

raw
1 #!/usr/bin/env python3
2
3 r'''Loads a model, and re-runs the optimization problem used to generate it
4
5 This is useful to analyze the solve. We can generate perfect chessboard
6 observations, corrupted with perfect nominal noise to validate the idea that
7 differences observed with mrcal-show-projection-diff should be predictive by the
8 uncertainties reported by mrcal-show-projection-uncertainty IF the dominant
9 source of error is calibration-time sampling error.
10
11 By default we write the resulting model to disk into a file
12 INPUTMODEL-reoptimized.cameramodel. If --explore, we instead drop into a REPL.
13
14 '''
15
16
17 import sys
18 import argparse
19 import re
20 import os
21
22 def parse_args():
23
24 parser = \
25 argparse.ArgumentParser(description = __doc__,
26 formatter_class=argparse.RawDescriptionHelpFormatter)
27
28 parser.add_argument('--model-intrinsics',
29 help='''By default, all the nominal data comes from the
30 MODEL given in the positional argument. If
31 --model-intrinsics is given, the intrinsics only come
32 from this other model. These are applied only to the ONE
33 model in icam_intrinsics''')
34
35 parser.add_argument('--perfect',
36 action= 'store_true',
37 help='''Make perfect observations and add perfect noise''')
38
39 parser.add_argument('--verbose',
40 action = 'store_true',
41 help='''If given, reoptimize verbosely''')
42 parser.add_argument('--skip-outlier-rejection',
43 action='store_true',
44 help='''Reoptimize with no outlier rejection''')
45 parser.add_argument('--revive-outliers',
46 action='store_true',
47 help='''Un-mark the outliers''')
48
49 parser.add_argument('--force', '-f',
50 action='store_true',
51 default=False,
52 help='''By default existing models on disk are not
53 overwritten. Pass --force to overwrite them without
54 complaint''')
55 parser.add_argument('--outdir',
56 type=lambda d: d if os.path.isdir(d) else \
57 parser.error(f"--outdir requires an existing directory as the arg, but got '{d}'"),
58 help='''Directory to write the output into. If omitted,
59 we use the directory of the input model''')
60 parser.add_argument('--explore',
61 action = 'store_true',
62 help='''If given, we don't write the reoptimized model
63 to disk, but drop into a REPL instead ''')
64
65 parser.add_argument('model',
66 type=str,
67 help='''The input camera model. If "-", we read standard
68 input and write to standard output. We get the frame
69 poses and extrinsics from this model. If
70 --model-intrinsics isn't given, we get the intrinsics
71 from this model as well''')
72
73 args = parser.parse_args()
74
75 return args
76
77 args = parse_args()
78
79 # arg-parsing is done before the imports so that --help works without building
80 # stuff, so that I can generate the manpages and README
81
82
83
84
85 # I import the LOCAL mrcal
86 sys.path[:0] = f"{os.path.dirname(os.path.realpath(__file__))}/..",
87
88 import mrcal
89 import numpy as np
90 import numpysane as nps
91 import time
92
93 model = mrcal.cameramodel(args.model)
94
95 if args.model == '-':
96 # Input read from stdin. Write output to stdout
97 file_output = sys.stdout
98 else:
99 if args.outdir is None:
100 filename_base,extension = os.path.splitext(args.model)
101 file_output = f"{filename_base}-reoptimized{extension}"
102 else:
103 f,extension = os.path.splitext(args.model)
104 directory,filename_base = os.path.split(f)
105 file_output = f"{args.outdir}/{filename_base}-reoptimized{extension}"
106
107 if os.path.exists(file_output) and not args.force:
108 print(f"ERROR: '{file_output}' already exists. Not overwriting this file. Pass -f to overwrite",
109 file=sys.stderr)
110 sys.exit(1)
111
112
113
114 optimization_inputs = model.optimization_inputs()
115
116 if args.perfect:
117 if args.model_intrinsics is not None:
118 model_intrinsics = mrcal.cameramodel(args.model_intrinsics)
119 if model_intrinsics.intrinsics()[0] != model.intrinsics()[0]:
120 print("At this time, --model-intrinsics MUST use the same lens model as the reference model",
121 file=sys.stderr)
122 sys.exit(1)
123 optimization_inputs['intrinsics'][model.icam_intrinsics()] = \
124 model_intrinsics.intrinsics()[1]
125
126 mrcal.make_perfect_observations(optimization_inputs)
127
128 ######### Reoptimize
129 optimization_inputs['verbose'] = args.verbose
130 optimization_inputs['do_apply_outlier_rejection'] = not args.skip_outlier_rejection
131 if args.revive_outliers:
132 for what in ('observations_board','observations_point'):
133 observations = optimization_inputs[what]
134
135 print(f"Original solve has {np.count_nonzero(observations[...,2] <= 0)} outliers in {what}. Reviving them")
136 print("")
137 observations[observations[...,2] <= 0, 2] = 1.
138
139 mrcal.optimize(**optimization_inputs)
140
141 model = mrcal.cameramodel(optimization_inputs = optimization_inputs,
142 icam_intrinsics = model.icam_intrinsics())
143
144 if not args.explore:
145 note = \
146 "generated on {} with {}\n". \
147 format(time.strftime("%Y-%m-%d %H:%M:%S"),
148 ' '.join(mrcal.shellquote(s) for s in sys.argv))
149
150 model.write(file_output, note=note)
151 if isinstance(file_output, str):
152 print(f"Wrote '{file_output}'",
153 file=sys.stderr)
154
155 sys.exit(0)
156
157
158 print("")
159 print("Done. The results are in the 'model' and 'optimization_inputs' variables")
160 print("")
161
162 import IPython
163 IPython.embed()