Skip to content
Permalink
master
Switch branches/tags
Go to file
 
 
Cannot retrieve contributors at this time
executable file 1112 lines (931 sloc) 57.3 KB
#!/usr/bin/python3
r'''Simulates different chessboard dances to find the best technique
We want the shortest chessboard dances that produce the most confident results.
In a perfect world would put the chessboard in all the locations where we would
expect to use the visual system, since the best confidence is obtained in
regions where the chessboard was observed.
However, due to geometric constraints it is sometimes impossible to put the
board in the right locations. This tool clearly shows that filling the field of
view produces best results. But very wide lenses require huge chessboards,
displayed very close to the lens in order to fill the field of view. This means
that using a wide lens to look out to infinity will always result in potentially
too little projection confidence. This tool is intended to find the kind of
chessboard dance to get good confidences by simulating different geometries and
dances.
We arrange --Ncameras cameras horizontally, with an identity rotation, evenly
spaced with a spacing of --camera-spacing meters. The left camera is at the
origin.
We show the cameras lots of dense chessboards ("dense" meaning that every camera
sees all the points of all the chessboards). The chessboard come in two
clusters: "near" and "far". Each cluster is centered straight ahead of the
midpoint of all the cameras, with some random noise on the position and
orientation. The distances from the center of the cameras to the center of the
clusters are given by --range. This tool solves the calibration problem, and
generates uncertainty-vs-range curves. Each run of this tool generates a family
of this curve, for different values of Nframes-far, the numbers of chessboard
observations in the "far" cluster.
This tool scans some parameter (selected by --scan), and reports the
uncertainty-vs-range for the different values of this parameter (as a plot and
as an output vnl).
If we don't want to scan any parameter, and just want a single
uncertainty-vs-range plot, don't pass --scan.
'''
import sys
import argparse
import re
import os
def parse_args():
def positive_float(string):
try:
value = float(string)
except:
raise argparse.ArgumentTypeError("argument MUST be a positive floating-point number. Got '{}'".format(string))
if value <= 0:
raise argparse.ArgumentTypeError("argument MUST be a positive floating-point number. Got '{}'".format(string))
return value
def positive_int(string):
try:
value = int(string)
except:
raise argparse.ArgumentTypeError("argument MUST be a positive integer. Got '{}'".format(string))
if value <= 0 or abs(value-float(string)) > 1e-6:
raise argparse.ArgumentTypeError("argument MUST be a positive integer. Got '{}'".format(string))
return value
parser = \
argparse.ArgumentParser(description = __doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument('--icam-uncertainty',
default = 0,
type=int,
help='''Which camera to use for the uncertainty reporting. I use the left-most one
(camera 0) by default''')
parser.add_argument('--camera-spacing',
default = 0.3,
type=float,
help='How many meters between adjacent cameras in our synthetic world')
parser.add_argument('--object-spacing',
default="0.077",
type=str,
help='''Width of each square in the calibration board, in meters. If --scan
object_width_n, this is used as the spacing for the
SMALLEST width. As I change object_width_n, I adjust
object_spacing accordingly, to keep the total board size
constant. If --scan object_spacing, this is MIN,MAX to
specify the bounds of the scan. In that case
object_width_n stays constant, so the board changes
size''')
parser.add_argument('--object-width-n',
type=str,
help='''How many points the calibration board has per horizontal side. If both are
omitted, we default the width and height to 10 (both
must be specified to use a non-default value). If --scan
object_width_n, this is MIN,MAX to specify the bounds of
the scan. In that case I assume a square object, and
ignore --object-height-n. Scanning object-width-n keeps
the board size constant''')
parser.add_argument('--object-height-n',
type=int,
help='''How many points the calibration board has per vertical side. If both are
omitted, we default the width and height to 10 (both
must be specified to use a non-default value). If --scan
object_width_n, this is ignored, and set equivalent to
the object width''')
parser.add_argument('--observed-pixel-uncertainty',
type=positive_float,
default = 1.0,
help='''The standard deviation of x and y pixel coordinates of the input observations
I generate. The distribution of the inputs is gaussian,
with the standard deviation specified by this argument.
Note: this is the x and y standard deviation, treated
independently. If each of these is s, then the LENGTH of
the deviation of each pixel is a Rayleigh distribution
with expected value s*sqrt(pi/2) ~ s*1.25''')
parser.add_argument('--lensmodel',
required=False,
type=str,
help='''Which lens model to use for the simulation. If omitted, we use the model
given on the commandline. We may want to use a
parametric model to generate data (model on the
commandline), but a richer splined model to solve''')
parser.add_argument('--show-geometry-first-solve',
action = 'store_true',
help='''If given, display the camera, chessboard geometry after the first solve, and
exit. Used for debugging''')
parser.add_argument('--show-uncertainty-first-solve',
action = 'store_true',
help='''If given, display the uncertainty (at infinity) and observations after the
first solve, and exit. Used for debugging''')
parser.add_argument('--write-models-first-solve',
action = 'store_true',
help='''If given, write the solved models to disk after the first solve, and exit.
Used for debugging. Useful to check fov_x_deg when solving for a splined model''')
parser.add_argument('--full-observations-only',
action = 'store_true',
help='''If given, every chessboard observation must be complete for all cameras.
Otherwise (by default), at least half the chessboard
must be observed by every camera''')
parser.add_argument('--fixed-frames',
action='store_true',
help='''Optimize the geometry keeping the chessboard frames fixed. This reduces the
freedom of the solution, and produces more confident
calibrations. It's possible to use this in reality by
surverying the chessboard poses''')
parser.add_argument('--ymax',
type=float,
default = 10.0,
help='''If given, use this as the upper extent of the uncertainty plot.''')
parser.add_argument('--uncertainty-at-range-sampled-min',
type=float,
help='''If given, use this as the lower bound of the ranges we look at when
evaluating projection confidence''')
parser.add_argument('--uncertainty-at-range-sampled-max',
type=float,
help='''If given, use this as the upper bound of the ranges we look at when
evaluating projection confidence''')
parser.add_argument('--explore',
action='store_true',
help='''Drop into a REPL at the end''')
parser.add_argument('--scan',
type=str,
default='',
choices=('range','tilt_radius','Nframes','Ncameras',
'object_width_n', 'object_spacing',
'num_far_constant_Nframes_near', 'num_far_constant_Nframes_all'),
help='''Study the effect of some parameter on uncertainty. The parameter is given as
an argument ot this function. Valid choices:
('range','tilt_radius','Nframes','Ncameras',
'object_width_n', 'object_spacing', 'num_far_constant_Nframes_near',
'num_far_constant_Nframes_all'). Scanning object-width-n
keeps the board size constant''')
parser.add_argument('--scan-object-spacing-compensate-range',
action='store_true',
help=f'''Only applies if --scan object-spacing. By default we vary the object spacings
without touching anything else: the chessboard grows in
size as we increase the spacing. If
--scan-object-spacing-compensate-range then we try to
keep the apparent size constant: as the object spacing
grows, we increase the range. The nominal range given in
--range applies to the MINIMUM object spacing''')
parser.add_argument('--range',
default = '0.5,4.0',
type=str,
help='''if --scan num_far_...: this is "NEAR,FAR"; specifying the near and far ranges
to the chessboard to evaluate. if --scan range this is
"MIN,MAX"; specifying the extents of the ranges to
evaluate. Otherwise: this is RANGE, the one range of
chessboards to evaluate''')
parser.add_argument('--tilt-radius',
default='30.',
type=str,
help='''The radius of the uniform distribution used to sample the pitch and yaw of
the chessboard observations, in degrees. The default is
30, meaning that the chessboard pitch and yaw are
sampled from [-30deg,30deg]. if --scan tilt_radius: this is
TILT-MIN,TILT-MAX specifying the bounds of the two
tilt-radius values to evaluate. Otherwise: this is the
one value we use''')
parser.add_argument('--Ncameras',
default = '1',
type=str,
help='''How many cameras oberve our synthetic world. By default we just have one
camera. if --scan Ncameras: this is
NCAMERAS-MIN,NCAMERAS-MAX specifying the bounds of the
camera counts to evaluate. Otherwise: this is the one
Ncameras value to use''')
parser.add_argument('--Nframes',
type=str,
help='''How many observed frames we have. Ignored if --scan num_far_... if
--scan Nframes: this is NFRAMES-MIN,NFRAMES-MAX specifying the bounds of the
frame counts to evaluate. Otherwise: this is the one Nframes value to use''')
parser.add_argument('--Nframes-near',
type=positive_int,
help='''Used if --scan num_far_constant_Nframes_near. The number of "near" frames is
given by this argument, while we look at the effect of
adding more "far" frames.''')
parser.add_argument('--Nframes-all',
type=positive_int,
help='''Used if --scan num_far_constant_Nframes_all. The number of "near"+"far"
frames is given by this argument, while we look at the
effect of replacing "near" frames with "far" frames.''')
parser.add_argument('--Nscan-samples',
type=positive_int,
default=8,
help='''How many values of the parameter being scanned to evaluate. If we're scanning
something (--scan ... given) then by default the scan
evaluates 8 different values. Otherwise this is set to 1''')
parser.add_argument('--hardcopy',
help=f'''Filename to plot into. If omitted, we make an interactive plot. This is
passed directly to gnuplotlib''')
parser.add_argument('--terminal',
help=f'''gnuplot terminal to use for the plots. This is passed directly to
gnuplotlib. Omit this unless you know what you're doing''')
parser.add_argument('model',
type = str,
help='''Baseline camera model. I use the intrinsics from this model to generate
synthetic data. We probably want the "true" model to not
be too crazy, so this should probably by a parametric
(not splined) model''')
return parser.parse_args()
args = parse_args()
if args.object_width_n is None and \
args.object_height_n is None:
args.object_width_n = "10"
args.object_height_n = 10
elif not ( args.object_width_n is not None and \
args.object_height_n is not None):
raise Exception("Either --object-width-n or --object-height-n are given: you must pass both or neither")
# arg-parsing is done before the imports so that --help works without building
# stuff, so that I can generate the manpages and README
import numpy as np
import numpysane as nps
import gnuplotlib as gp
import copy
import os.path
# I import the LOCAL mrcal
scriptdir = os.path.dirname(os.path.realpath(__file__))
sys.path[:0] = f"{scriptdir}/../..",
import mrcal
def split_list(s, t):
r'''Splits a comma-separated list
None -> None
"A,B,C" -> (A,B,C)
"A" -> A
'''
if s is None: return None,0
l = [t(x) for x in s.split(',')]
if len(l) == 1: return l[0],1
return l,len(l)
def first(s):
r'''first in a list, or the value if a scalar'''
if hasattr(s, '__iter__'): return s[0]
return s
def last(s):
r'''last in a list, or the value if a scalar'''
if hasattr(s, '__iter__'): return s[-1]
return s
controllable_args = \
dict( tilt_radius = dict(_type = float),
range = dict(_type = float),
Ncameras = dict(_type = int),
Nframes = dict(_type = int),
object_spacing = dict(_type = float),
object_width_n = dict(_type = int) )
for a in controllable_args.keys():
l,n = split_list(getattr(args, a), controllable_args[a]['_type'])
controllable_args[a]['value'] = l
controllable_args[a]['listlen'] = n
if any( controllable_args[a]['listlen'] > 2 for a in controllable_args.keys() ):
raise Exception(f"All controllable args must have either at most 2 values. {a} has {controllable_args[a]['listlen']}")
controllable_arg_0values = [ a for a in controllable_args.keys() if controllable_args[a]['listlen'] == 0 ]
controllable_arg_2values = [ a for a in controllable_args.keys() if controllable_args[a]['listlen'] == 2 ]
if len(controllable_arg_2values) == 0: controllable_arg_2values = ''
elif len(controllable_arg_2values) == 1: controllable_arg_2values = controllable_arg_2values[0]
else: raise Exception(f"At most 1 controllable arg may have 2 values. Instead I saw: {controllable_arg_2values}")
if re.match("num_far_constant_Nframes_", args.scan):
# special case
if 'Nframes' not in controllable_arg_0values:
raise Exception(f"I'm scanning '{args.scan}', so --Nframes must not have been given")
if 'range' != controllable_arg_2values:
raise Exception(f"I'm scanning '{args.scan}', so --range must have 2 values")
if args.scan == "num_far_constant_Nframes_near":
if args.Nframes_all is not None:
raise Exception(f"I'm scanning '{args.scan}', so --Nframes-all must not have have been given")
if args.Nframes_near is None:
raise Exception(f"I'm scanning '{args.scan}', so --Nframes-near must have have been given")
else:
if args.Nframes_near is not None:
raise Exception(f"I'm scanning '{args.scan}', so --Nframes-near must not have have been given")
if args.Nframes_all is None:
raise Exception(f"I'm scanning '{args.scan}', so --Nframes-all must have have been given")
else:
if args.scan != controllable_arg_2values:
# This covers the scanning-nothing-no2value-anything case
raise Exception(f"I'm scanning '{args.scan}', the arg given 2 values is '{controllable_arg_2values}'. They must match")
if len(controllable_arg_0values):
raise Exception(f"I'm scanning '{args.scan}', so all controllable args should have some value. Missing: '{controllable_arg_0values}")
if args.scan == '':
args.Nscan_samples = 1
Nuncertainty_at_range_samples = 80
uncertainty_at_range_sampled_min = args.uncertainty_at_range_sampled_min
if uncertainty_at_range_sampled_min is None:
uncertainty_at_range_sampled_min = first(controllable_args['range']['value'])/10.
uncertainty_at_range_sampled_max = args.uncertainty_at_range_sampled_max
if uncertainty_at_range_sampled_max is None:
uncertainty_at_range_sampled_max = last(controllable_args['range']['value']) *10.
uncertainty_at_range_samples = \
np.logspace( np.log10(uncertainty_at_range_sampled_min),
np.log10(uncertainty_at_range_sampled_max),
Nuncertainty_at_range_samples)
# I want the RNG to be deterministic
np.random.seed(0)
model_intrinsics = mrcal.cameramodel(args.model)
calobject_warp_true_ref = np.array((0.002, -0.005))
def solve(Ncameras,
Nframes_near, Nframes_far,
object_spacing,
models_true,
# q.shape = (Nframes, Ncameras, object_height, object_width, 2)
# Rt_cam0_board.shape = (Nframes, 4,3)
q_true_near, Rt_cam0_board_true_near,
q_true_far, Rt_cam0_board_true_far,
fixed_frames = args.fixed_frames):
q_true_near = q_true_near [:Nframes_near]
Rt_cam0_board_true_near = Rt_cam0_board_true_near[:Nframes_near]
if q_true_far is not None:
q_true_far = q_true_far [:Nframes_far ]
Rt_cam0_board_true_far = Rt_cam0_board_true_far [:Nframes_far ]
else:
q_true_far = np.zeros( (0,) + q_true_near.shape[1:],
dtype = q_true_near.dtype)
Rt_cam0_board_true_far = np.zeros( (0,) + Rt_cam0_board_true_near.shape[1:],
dtype = Rt_cam0_board_true_near.dtype)
calobject_warp_true = calobject_warp_true_ref.copy()
Nframes_all = Nframes_near + Nframes_far
Rt_cam0_board_true = nps.glue( Rt_cam0_board_true_near,
Rt_cam0_board_true_far,
axis = -3 )
# Dense observations. All the cameras see all the boards
indices_frame_camera = np.zeros( (Nframes_all*Ncameras, 2), dtype=np.int32)
indices_frame = indices_frame_camera[:,0].reshape(Nframes_all,Ncameras)
indices_frame.setfield(nps.outer(np.arange(Nframes_all, dtype=np.int32),
np.ones((Ncameras,), dtype=np.int32)),
dtype = np.int32)
indices_camera = indices_frame_camera[:,1].reshape(Nframes_all,Ncameras)
indices_camera.setfield(nps.outer(np.ones((Nframes_all,), dtype=np.int32),
np.arange(Ncameras, dtype=np.int32)),
dtype = np.int32)
indices_frame_camintrinsics_camextrinsics = \
nps.glue(indices_frame_camera,
indices_frame_camera[:,(1,)],
axis=-1)
# If not fixed_frames: we use camera0 as the reference cordinate system, and
# we allow the chessboard poses to move around. Else: the reference
# coordinate system is arbitrary, but all cameras are allowed to move
# around. The chessboards poses are fixed
if not fixed_frames:
indices_frame_camintrinsics_camextrinsics[:,2] -= 1
q = nps.glue( q_true_near,
q_true_far,
axis = -5 )
# apply noise
q += np.random.randn(*q.shape) * args.observed_pixel_uncertainty
# The observations are dense (in the data every camera sees all the
# chessboards), but some of the observations WILL be out of bounds. I
# pre-mark those as outliers so that the solve doesn't do weird stuff
# Set the weights to 1 initially
# shape (Nframes, Ncameras, object_height_n, object_width_n, 3)
observations = nps.glue(q,
np.ones( q.shape[:-1] + (1,) ),
axis = -1)
# shape (Ncameras, 1, 1, 2)
imagersizes = nps.mv( nps.cat(*[ m.imagersize() for m in models_true ]),
-2, -4 )
# mark the out-of-view observations as outliers
observations[ np.any( q < 0, axis=-1 ), 2 ] = -1.
observations[ np.any( q-imagersizes >= 0, axis=-1 ), 2 ] = -1.
# shape (Nobservations, Nh, Nw, 2)
observations = nps.clump( observations,
n = 2 )
intrinsics = nps.cat( *[m.intrinsics()[1] for m in models_true] )
# If not fixed_frames: we use camera0 as the reference cordinate system, and
# we allow the chessboard poses to move around. Else: the reference
# coordinate system is arbitrary, but all cameras are allowed to move
# around. The chessboards poses are fixed
if fixed_frames:
extrinsics = nps.cat( *[m.extrinsics_rt_fromref() for m in models_true] )
else:
extrinsics = nps.cat( *[m.extrinsics_rt_fromref() for m in models_true[1:]] )
if len(extrinsics) == 0: extrinsics = None
if nps.norm2(models_true[0].extrinsics_rt_fromref()) > 1e-6:
raise Exception("models_true[0] must sit at the origin")
imagersizes = nps.cat( *[m.imagersize() for m in models_true] )
optimization_inputs = \
dict( # intrinsics filled in later
extrinsics_rt_fromref = extrinsics,
frames_rt_toref = mrcal.rt_from_Rt(Rt_cam0_board_true),
points = None,
observations_board = observations,
indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics,
observations_point = None,
indices_point_camintrinsics_camextrinsics = None,
# lensmodel filled in later
calobject_warp = copy.deepcopy(calobject_warp_true),
imagersizes = imagersizes,
calibration_object_spacing = object_spacing,
verbose = False,
observed_pixel_uncertainty = args.observed_pixel_uncertainty,
# do_optimize_extrinsics filled in later
# do_optimize_intrinsics_core filled in later
do_optimize_frames = False,
do_optimize_intrinsics_distortions = True,
do_optimize_calobject_warp = False, # turn this on, and reoptimize later
do_apply_regularization = True,
do_apply_outlier_rejection = False)
if args.lensmodel is None:
lensmodel = model_intrinsics.intrinsics()[0]
else:
lensmodel = args.lensmodel
Nintrinsics = mrcal.lensmodel_num_params(lensmodel)
if re.search("SPLINED", lensmodel):
# These are already mostly right, So I lock them down while I seed the
# intrinsics
optimization_inputs['do_optimize_extrinsics'] = False
# I pre-optimize the core, and then lock it down
optimization_inputs['lensmodel'] = 'LENSMODEL_STEREOGRAPHIC'
optimization_inputs['intrinsics'] = intrinsics[:,:4].copy()
optimization_inputs['do_optimize_intrinsics_core'] = True
stats = mrcal.optimize(**optimization_inputs)
print(f"## optimized. rms = {stats['rms_reproj_error__pixels']}", file=sys.stderr)
# core is good. Lock that down, and get an estimate for the control
# points
optimization_inputs['do_optimize_intrinsics_core'] = False
optimization_inputs['lensmodel'] = lensmodel
optimization_inputs['intrinsics'] = nps.glue(optimization_inputs['intrinsics'],
np.zeros((Ncameras,Nintrinsics-4),),axis=-1)
stats = mrcal.optimize(**optimization_inputs)
print(f"## optimized. rms = {stats['rms_reproj_error__pixels']}", file=sys.stderr)
# Ready for a final reoptimization with the geometry
optimization_inputs['do_optimize_extrinsics'] = True
if not fixed_frames:
optimization_inputs['do_optimize_frames'] = True
else:
optimization_inputs['lensmodel'] = lensmodel
if not mrcal.lensmodel_metadata_and_config(lensmodel)['has_core'] or \
not mrcal.lensmodel_metadata_and_config(model_intrinsics.intrinsics()[0])['has_core']:
raise Exception("I'm assuming all the models here have a core. It's just lazy coding. If you see this, feel free to fix.")
if lensmodel == model_intrinsics.intrinsics()[0]:
# Same model. Grab the intrinsics. They're 99% right
optimization_inputs['intrinsics'] = intrinsics.copy()
else:
# Different model. Grab the intrinsics core, optimize the rest
optimization_inputs['intrinsics'] = nps.glue(intrinsics[:,:4],
np.zeros((Ncameras,Nintrinsics-4),),axis=-1)
optimization_inputs['do_optimize_intrinsics_core'] = True
optimization_inputs['do_optimize_extrinsics'] = True
if not fixed_frames:
optimization_inputs['do_optimize_frames'] = True
stats = mrcal.optimize(**optimization_inputs)
print(f"## optimized. rms = {stats['rms_reproj_error__pixels']}", file=sys.stderr)
optimization_inputs['do_optimize_calobject_warp'] = True
stats = mrcal.optimize(**optimization_inputs)
print(f"## optimized. rms = {stats['rms_reproj_error__pixels']}", file=sys.stderr)
return optimization_inputs
def observation_centroid(optimization_inputs, icam):
r'''mean pixel coordinate of all non-outlier points seen by a given camera'''
ifcice = optimization_inputs['indices_frame_camintrinsics_camextrinsics']
observations = optimization_inputs['observations_board']
# pick the camera I want
observations = observations[ifcice[:,1] == icam]
# ignore outliers
q = observations[ (observations[...,2] > 0), :2]
return np.mean(q, axis=-2)
def eval_one_rangenear_tilt(models_true,
range_near, range_far, tilt_radius,
object_width_n, object_height_n, object_spacing,
uncertainty_at_range_samples,
Ncameras,
Nframes_near_samples, Nframes_far_samples):
# I want the RNG to be deterministic
np.random.seed(0)
uncertainties = np.zeros((len(Nframes_far_samples),
len(uncertainty_at_range_samples)),
dtype=float)
radius_cameras = (args.camera_spacing * (Ncameras-1)) / 2.
if args.full_observations_only:
which = 'all_cameras_must_see_full_board'
else:
which = 'all_cameras_must_see_half_board'
# shapes (Nframes, Ncameras, Nh, Nw, 2),
# (Nframes, 4,3)
q_true_near, Rt_cam0_board_true_near = \
mrcal.synthesize_board_observations(models_true,
object_width_n, object_height_n, object_spacing,
calobject_warp_true_ref,
np.array((0., 0., 0., radius_cameras, 0, range_near,)),
np.array((np.pi/180. * tilt_radius,
np.pi/180. * tilt_radius,
np.pi/180. * 20.,
range_near*2. + radius_cameras,
range_near*2.,
range_near/10.)),
np.max(Nframes_near_samples),
which = which)
if range_far is not None:
q_true_far, Rt_cam0_board_true_far = \
mrcal.synthesize_board_observations(models_true,
object_width_n, object_height_n, object_spacing,
calobject_warp_true_ref,
np.array((0., 0., 0., radius_cameras, 0, range_far,)),
np.array((np.pi/180. * tilt_radius,
np.pi/180. * tilt_radius,
np.pi/180. * 20.,
range_far*2. + radius_cameras,
range_far*2.,
range_far/10.)),
np.max(Nframes_far_samples),
which = which)
else:
q_true_far = None
Rt_cam0_board_true_far = None
for i_Nframes_far in range(len(Nframes_far_samples)):
Nframes_far = Nframes_far_samples [i_Nframes_far]
Nframes_near = Nframes_near_samples[i_Nframes_far]
optimization_inputs = solve(Ncameras,
Nframes_near, Nframes_far,
object_spacing,
models_true,
q_true_near, Rt_cam0_board_true_near,
q_true_far, Rt_cam0_board_true_far)
models_out = \
[ mrcal.cameramodel( optimization_inputs = optimization_inputs,
icam_intrinsics = icam ) \
for icam in range(Ncameras) ]
model = models_out[args.icam_uncertainty]
if args.show_geometry_first_solve:
mrcal.show_geometry(models_out, wait = True)
sys.exit()
if args.show_uncertainty_first_solve:
mrcal.show_projection_uncertainty(model,
observations= True,
wait = True)
sys.exit()
if args.write_models_first_solve:
for i in range(len(models_out)):
f = f"/tmp/camera{i}.cameramodel"
if os.path.isfile(f):
input(f"File {f} already exists, and I want to overwrite it. Press enter to overwrite. Ctrl-c to exit")
models_out[i].write(f)
print(f"Wrote {f}")
sys.exit()
# shape (N,3)
# I sample the center of the imager
pcam_samples = \
mrcal.unproject( observation_centroid(optimization_inputs,
args.icam_uncertainty),
*model.intrinsics(),
normalize = True) * \
nps.dummy(uncertainty_at_range_samples, -1)
uncertainties[i_Nframes_far] = \
mrcal.projection_uncertainty(pcam_samples,
model,
what='worstdirection-stdev')
return uncertainties
output_table_legend = 'range_uncertainty_sample Nframes_near Nframes_far Ncameras range_near range_far tilt_radius object_width_n object_spacing uncertainty'
output_table_fmt = '%f %d %d %d %f %f %f %d %f %f'
output_table_icol__range_uncertainty_sample = 0
output_table_icol__Nframes_near = 1
output_table_icol__Nframes_far = 2
output_table_icol__Ncameras = 3
output_table_icol__range_near = 4
output_table_icol__range_far = 5
output_table_icol__tilt_radius = 6
output_table_icol__object_width_n = 7
output_table_icol__object_spacing = 8
output_table_icol__uncertainty = 9
output_table_Ncols = 10
output_table = np.zeros( (args.Nscan_samples, Nuncertainty_at_range_samples, output_table_Ncols), dtype=float)
output_table[:,:, output_table_icol__range_uncertainty_sample] += uncertainty_at_range_samples
if re.match("num_far_constant_Nframes_", args.scan):
Nfar_samples = args.Nscan_samples
if args.scan == "num_far_constant_Nframes_near":
Nframes_far_samples = np.linspace(0,
args.Nframes_near//4,
Nfar_samples, dtype=int)
Nframes_near_samples = Nframes_far_samples*0 + args.Nframes_near
else:
Nframes_far_samples = np.linspace(0,
args.Nframes_all,
Nfar_samples, dtype=int)
Nframes_near_samples = args.Nframes_all - Nframes_far_samples
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
extrinsics_rt_toref = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(controllable_args['Ncameras']['value']) ]
# shape (args.Nscan_samples, Nuncertainty_at_range_samples)
output_table[:,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
*controllable_args['range']['value'],
controllable_args['tilt_radius']['value'],
controllable_args['object_width_n']['value'],
args.object_height_n,
controllable_args['object_spacing']['value'],
uncertainty_at_range_samples,
controllable_args['Ncameras']['value'],
Nframes_near_samples, Nframes_far_samples)
output_table[:,:, output_table_icol__Nframes_near] += nps.transpose(Nframes_near_samples)
output_table[:,:, output_table_icol__Nframes_far] += nps.transpose(Nframes_far_samples)
output_table[:,:, output_table_icol__Ncameras] = controllable_args['Ncameras']['value']
output_table[:,:, output_table_icol__range_near] = controllable_args['range']['value'][0]
output_table[:,:, output_table_icol__range_far] = controllable_args['range']['value'][1]
output_table[:,:, output_table_icol__tilt_radius ] = controllable_args['tilt_radius']['value']
output_table[:,:, output_table_icol__object_width_n ] = controllable_args['object_width_n']['value']
output_table[:,:, output_table_icol__object_spacing ] = controllable_args['object_spacing']['value']
samples = Nframes_far_samples
elif args.scan == "range":
Nframes_near_samples = np.array( (controllable_args['Nframes']['value'],), dtype=int)
Nframes_far_samples = np.array( (0,), dtype=int)
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
extrinsics_rt_toref = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(controllable_args['Ncameras']['value']) ]
Nrange_samples = args.Nscan_samples
range_samples = np.linspace(*controllable_args['range']['value'],
Nrange_samples, dtype=float)
for i_range in range(Nrange_samples):
output_table[i_range,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
range_samples[i_range], None,
controllable_args['tilt_radius']['value'],
controllable_args['object_width_n']['value'],
args.object_height_n,
controllable_args['object_spacing']['value'],
uncertainty_at_range_samples,
controllable_args['Ncameras']['value'],
Nframes_near_samples, Nframes_far_samples)[0]
output_table[:,:, output_table_icol__Nframes_near] = controllable_args['Nframes']['value']
output_table[:,:, output_table_icol__Nframes_far] = 0
output_table[:,:, output_table_icol__Ncameras] = controllable_args['Ncameras']['value']
output_table[:,:, output_table_icol__range_near] += nps.transpose(range_samples)
output_table[:,:, output_table_icol__range_far] = -1
output_table[:,:, output_table_icol__tilt_radius ] = controllable_args['tilt_radius']['value']
output_table[:,:, output_table_icol__object_width_n ] = controllable_args['object_width_n']['value']
output_table[:,:, output_table_icol__object_spacing ] = controllable_args['object_spacing']['value']
samples = range_samples
elif args.scan == "tilt_radius":
Nframes_near_samples = np.array( (controllable_args['Nframes']['value'],), dtype=int)
Nframes_far_samples = np.array( (0,), dtype=int)
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
extrinsics_rt_toref = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(controllable_args['Ncameras']['value']) ]
Ntilt_rad_samples = args.Nscan_samples
tilt_rad_samples = np.linspace(*controllable_args['tilt_radius']['value'],
Ntilt_rad_samples, dtype=float)
for i_tilt in range(Ntilt_rad_samples):
output_table[i_tilt,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
controllable_args['range']['value'], None,
tilt_rad_samples[i_tilt],
controllable_args['object_width_n']['value'],
args.object_height_n,
controllable_args['object_spacing']['value'],
uncertainty_at_range_samples,
controllable_args['Ncameras']['value'],
Nframes_near_samples, Nframes_far_samples)[0]
output_table[:,:, output_table_icol__Nframes_near] = controllable_args['Nframes']['value']
output_table[:,:, output_table_icol__Nframes_far] = 0
output_table[:,:, output_table_icol__Ncameras] = controllable_args['Ncameras']['value']
output_table[:,:, output_table_icol__range_near] = controllable_args['range']['value']
output_table[:,:, output_table_icol__range_far] = -1
output_table[:,:, output_table_icol__tilt_radius] += nps.transpose(tilt_rad_samples)
output_table[:,:, output_table_icol__object_width_n ] = controllable_args['object_width_n']['value']
output_table[:,:, output_table_icol__object_spacing ] = controllable_args['object_spacing']['value']
samples = tilt_rad_samples
elif args.scan == "Ncameras":
Nframes_near_samples = np.array( (controllable_args['Nframes']['value'],), dtype=int)
Nframes_far_samples = np.array( (0,), dtype=int)
N_Ncameras_samples = args.Nscan_samples
Ncameras_samples = np.linspace(*controllable_args['Ncameras']['value'],
N_Ncameras_samples, dtype=int)
for i_Ncameras in range(N_Ncameras_samples):
Ncameras = Ncameras_samples[i_Ncameras]
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
extrinsics_rt_toref = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(Ncameras) ]
output_table[i_Ncameras,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
controllable_args['range']['value'], None,
controllable_args['tilt_radius']['value'],
controllable_args['object_width_n']['value'],
args.object_height_n,
controllable_args['object_spacing']['value'],
uncertainty_at_range_samples,
Ncameras,
Nframes_near_samples, Nframes_far_samples)[0]
output_table[:,:, output_table_icol__Nframes_near] = controllable_args['Nframes']['value']
output_table[:,:, output_table_icol__Nframes_far] = 0
output_table[:,:, output_table_icol__Ncameras] += nps.transpose(Ncameras_samples)
output_table[:,:, output_table_icol__range_near] = controllable_args['range']['value']
output_table[:,:, output_table_icol__range_far] = -1
output_table[:,:, output_table_icol__tilt_radius] = controllable_args['tilt_radius']['value']
output_table[:,:, output_table_icol__object_width_n ] = controllable_args['object_width_n']['value']
output_table[:,:, output_table_icol__object_spacing ] = controllable_args['object_spacing']['value']
samples = Ncameras_samples
elif args.scan == "Nframes":
Nframes_far_samples = np.array( (0,), dtype=int)
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
extrinsics_rt_toref = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(controllable_args['Ncameras']['value']) ]
N_Nframes_samples = args.Nscan_samples
Nframes_samples = np.linspace(*controllable_args['Nframes']['value'],
N_Nframes_samples, dtype=int)
for i_Nframes in range(N_Nframes_samples):
Nframes = Nframes_samples[i_Nframes]
Nframes_near_samples = np.array( (Nframes,), dtype=int)
output_table[i_Nframes,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
controllable_args['range']['value'], None,
controllable_args['tilt_radius']['value'],
controllable_args['object_width_n']['value'],
args.object_height_n,
controllable_args['object_spacing']['value'],
uncertainty_at_range_samples,
controllable_args['Ncameras']['value'],
Nframes_near_samples, Nframes_far_samples)[0]
output_table[:,:, output_table_icol__Nframes_near]+= nps.transpose(Nframes_samples)
output_table[:,:, output_table_icol__Nframes_far] = 0
output_table[:,:, output_table_icol__Ncameras] = controllable_args['Ncameras']['value']
output_table[:,:, output_table_icol__range_near] = controllable_args['range']['value']
output_table[:,:, output_table_icol__range_far] = -1
output_table[:,:, output_table_icol__tilt_radius] = controllable_args['tilt_radius']['value']
output_table[:,:, output_table_icol__object_width_n ] = controllable_args['object_width_n']['value']
output_table[:,:, output_table_icol__object_spacing ] = controllable_args['object_spacing']['value']
samples = Nframes_samples
elif args.scan == "object_width_n":
Nframes_near_samples = np.array( (controllable_args['Nframes']['value'],), dtype=int)
Nframes_far_samples = np.array( (0,), dtype=int)
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
extrinsics_rt_toref = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(controllable_args['Ncameras']['value']) ]
Nsamples = args.Nscan_samples
samples = np.linspace(*controllable_args['object_width_n']['value'],
Nsamples, dtype=int)
# As I move the width, I adjust the spacing to keep the total board size
# constant. The object spacing in the argument applies to the MIN value of
# the object_width_n.
W = (controllable_args['object_width_n']['value'][0]-1) * controllable_args['object_spacing']['value']
object_spacing = W / samples
for i_sample in range(Nsamples):
output_table[i_sample,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
controllable_args['range']['value'], None,
controllable_args['tilt_radius']['value'],
samples[i_sample],
samples[i_sample],
object_spacing[i_sample],
uncertainty_at_range_samples,
controllable_args['Ncameras']['value'],
Nframes_near_samples, Nframes_far_samples)[0]
output_table[:,:, output_table_icol__Nframes_near] = controllable_args['Nframes']['value']
output_table[:,:, output_table_icol__Nframes_far] = 0
output_table[:,:, output_table_icol__Ncameras] = controllable_args['Ncameras']['value']
output_table[:,:, output_table_icol__range_near] = controllable_args['range']['value']
output_table[:,:, output_table_icol__range_far] = -1
output_table[:,:, output_table_icol__tilt_radius] = controllable_args['tilt_radius']['value']
output_table[:,:, output_table_icol__object_width_n ]+= nps.transpose(samples)
output_table[:,:, output_table_icol__object_spacing ]+= nps.transpose(object_spacing)
elif args.scan == "object_spacing":
Nframes_near_samples = np.array( (controllable_args['Nframes']['value'],), dtype=int)
Nframes_far_samples = np.array( (0,), dtype=int)
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
extrinsics_rt_toref = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(controllable_args['Ncameras']['value']) ]
Nsamples = args.Nscan_samples
samples = np.linspace(*controllable_args['object_spacing']['value'],
Nsamples, dtype=float)
# As I move the spacing, I leave object_width_n, letting the total board
# size change. The object spacing in the argument applies to the MIN value
# of the object_width_n.
for i_sample in range(Nsamples):
r = controllable_args['range']['value']
if args.scan_object_spacing_compensate_range:
r *= samples[i_sample]/samples[0]
output_table[i_sample,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
r, None,
controllable_args['tilt_radius']['value'],
controllable_args['object_width_n']['value'],
args.object_height_n,
samples[i_sample],
uncertainty_at_range_samples,
controllable_args['Ncameras']['value'],
Nframes_near_samples, Nframes_far_samples)[0]
output_table[:,:, output_table_icol__Nframes_near] = controllable_args['Nframes']['value']
output_table[:,:, output_table_icol__Nframes_far] = 0
output_table[:,:, output_table_icol__Ncameras] = controllable_args['Ncameras']['value']
output_table[:,:, output_table_icol__range_far] = -1
output_table[:,:, output_table_icol__tilt_radius] = controllable_args['tilt_radius']['value']
output_table[:,:, output_table_icol__object_width_n ] = controllable_args['object_width_n']['value']
output_table[:,:, output_table_icol__object_spacing ]+= nps.transpose(samples)
if args.scan_object_spacing_compensate_range:
output_table[:,:, output_table_icol__range_near] += controllable_args['range']['value'] * nps.transpose(samples/samples[0])
else:
output_table[:,:, output_table_icol__range_near] = controllable_args['range']['value']
else:
# no --scan. We just want one sample
Nframes_near_samples = np.array( (controllable_args['Nframes']['value'],), dtype=int)
Nframes_far_samples = np.array( (0,), dtype=int)
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
extrinsics_rt_toref = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(controllable_args['Ncameras']['value']) ]
output_table[0,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
controllable_args['range']['value'], None,
controllable_args['tilt_radius']['value'],
controllable_args['object_width_n']['value'],
args.object_height_n,
controllable_args['object_spacing']['value'],
uncertainty_at_range_samples,
controllable_args['Ncameras']['value'],
Nframes_near_samples, Nframes_far_samples)[0]
output_table[:,:, output_table_icol__Nframes_near] = controllable_args['Nframes']['value']
output_table[:,:, output_table_icol__Nframes_far] = 0
output_table[:,:, output_table_icol__Ncameras] = controllable_args['Ncameras']['value']
output_table[:,:, output_table_icol__range_near] = controllable_args['range']['value']
output_table[:,:, output_table_icol__range_far] = -1
output_table[:,:, output_table_icol__tilt_radius] = controllable_args['tilt_radius']['value']
output_table[:,:, output_table_icol__object_width_n ] = controllable_args['object_width_n']['value']
output_table[:,:, output_table_icol__object_spacing ] = controllable_args['object_spacing']['value']
samples = None
if isinstance(controllable_args['range']['value'], float):
guides = [ f"arrow nohead dashtype 3 from {controllable_args['range']['value']},graph 0 to {controllable_args['range']['value']},graph 1" ]
else:
guides = [ f"arrow nohead dashtype 3 from {r},graph 0 to {r},graph 1" for r in controllable_args['range']['value'] ]
guides.append(f"arrow nohead dashtype 3 from graph 0,first {args.observed_pixel_uncertainty} to graph 1,first {args.observed_pixel_uncertainty}")
if args.scan == "num_far_constant_Nframes_near":
title = f"Scanning 'far' observations added to a set of 'near' observations. Have {controllable_args['Ncameras']['value']} cameras, {args.Nframes_near} 'near' observations, at ranges {controllable_args['range']['value']}."
legend_what = 'Nframes_far'
elif args.scan == "num_far_constant_Nframes_all":
title = f"Scanning 'far' observations replacing 'near' observations. Have {controllable_args['Ncameras']['value']} cameras, {args.Nframes_all} total observations, at ranges {controllable_args['range']['value']}."
legend_what = 'Nframes_far'
elif args.scan == "Nframes":
title = f"Scanning Nframes. Have {controllable_args['Ncameras']['value']} cameras looking out at {controllable_args['range']['value']:.2f}m."
legend_what = 'Nframes'
elif args.scan == "Ncameras":
title = f"Scanning Ncameras. Observing {controllable_args['Nframes']['value']} boards at {controllable_args['range']['value']:.2f}m."
legend_what = 'Ncameras'
elif args.scan == "range":
title = f"Scanning the distance to observations. Have {controllable_args['Ncameras']['value']} cameras looking at {controllable_args['Nframes']['value']} boards."
legend_what = 'Range-to-chessboards'
elif args.scan == "tilt_radius":
title = f"Scanning the board tilt. Have {controllable_args['Ncameras']['value']} cameras looking at {controllable_args['Nframes']['value']} boards at {controllable_args['range']['value']:.2f}m"
legend_what = 'Random chessboard tilt radius'
elif args.scan == "object_width_n":
title = f"Scanning the calibration object density, keeping the board size constant. Have {controllable_args['Ncameras']['value']} cameras looking at {controllable_args['Nframes']['value']} boards at {controllable_args['range']['value']:.2f}m"
legend_what = 'Number of chessboard points per side'
elif args.scan == "object_spacing":
if args.scan_object_spacing_compensate_range:
title = f"Scanning the calibration object spacing, keeping the point count constant, and letting the board grow. Range grows with spacing. Have {controllable_args['Ncameras']['value']} cameras looking at {controllable_args['Nframes']['value']} boards at {controllable_args['range']['value']:.2f}m"
else:
title = f"Scanning the calibration object spacing, keeping the point count constant, and letting the board grow. Range is constant. Have {controllable_args['Ncameras']['value']} cameras looking at {controllable_args['Nframes']['value']} boards at {controllable_args['range']['value']:.2f}m"
legend_what = 'Distance between adjacent chessboard corners'
else:
# no --scan. We just want one sample
title = f"Have {controllable_args['Ncameras']['value']} cameras looking at {controllable_args['Nframes']['value']} boards at {controllable_args['range']['value']:.2f}m with tilt radius {controllable_args['tilt_radius']['value']}"
if samples is None:
legend = None
elif samples.dtype.kind == 'i':
legend = np.array([ f"{legend_what} = {x}" for x in samples])
else:
legend = np.array([ f"{legend_what} = {x:.2f}" for x in samples])
np.savetxt(sys.stdout,
nps.clump(output_table, n=2),
fmt = output_table_fmt,
header= output_table_legend)
kwargs = dict( yrange = (0, args.ymax),
_with = 'lines',
_set = guides,
unset = 'grid',
title = title,
xlabel = 'Range (m)',
ylabel = 'Expected worst-direction uncertainty (pixels)',
hardcopy = args.hardcopy,
terminal = args.terminal,
wait = not args.explore and args.hardcopy is None)
if legend is not None: kwargs['legend'] = legend
gp.plot(uncertainty_at_range_samples,
output_table[:,:, output_table_icol__uncertainty],
**kwargs)
if args.explore:
import IPython
IPython.embed()
sys.exit()