| 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() |