def _extrinsics_Rt(self, toref, Rt=None):
r'''Get or set the extrinsics in this model
This function represents the pose as a shape (4,3) numpy array that
contains a (3,3) rotation matrix, followed by a (1,3) translation in the
last row:
R = Rt[:3,:]
t = Rt[ 3,:]
The transformation is b <-- R*a + t:
import numpysane as nps
b = nps.matmult(a, nps.transpose(R)) + t
if Rt is None: this is a getter; otherwise a setter.
toref is a boolean. if toref: then Rt maps points in the coord system of
THIS camera to the reference coord system. Otherwise in the opposite
direction
'''
# The internal representation is rt_fromref
if Rt is None:
# getter
rt_fromref = self._extrinsics
Rt_fromref = mrcal.Rt_from_rt(rt_fromref)
if not toref:
return Rt_fromref
return mrcal.invert_Rt(Rt_fromref)
# setter
if toref:
Rt_fromref = mrcal.invert_Rt(Rt)
self._extrinsics = mrcal.rt_from_Rt(Rt_fromref)
return True
self._extrinsics = mrcal.rt_from_Rt(Rt)
return True
R, J_r = mrcal.R_from_r(r0_ref_tiny, get_gradients=True, out=(out33, out333)) J_r_ref = grad(R_from_r, r0_ref_tiny) confirm_equal(R, R0_ref_tiny, msg='R_from_r result for tiny r0') confirm_equal(J_r, J_r_ref, msg='R_from_r J_r for tiny r0') # Do it again, actually calling opencv. This is both a test, and shows how to # migrate old code R, J_r = mrcal.R_from_r(r0_ref, get_gradients=True, out=(out33, out333)) Rref, J_r_ref = cv2.Rodrigues(r0_ref) J_r_ref = nps.transpose(J_r_ref) # fix opencv's weirdness. Now shape=(9,3) J_r_ref = J_r_ref.reshape(3, 3, 3) confirm_equal(R, Rref, msg='R_from_r result, comparing with cv2.Rodrigues') confirm_equal(J_r, J_r_ref, msg='R_from_r J_r, comparing with cv2.Rodrigues') rt = mrcal.rt_from_Rt(Rt0_ref, out=out6) confirm_equal(rt, rt0_ref, msg='rt_from_Rt result') rt, J_R = mrcal.rt_from_Rt(Rt0_ref, get_gradients=True, out=(out6, out333)) J_R_ref = grad(r_from_R, Rt0_ref[:3, :]) confirm_equal(rt, rt0_ref, msg='rt_from_Rt result') confirm_equal(J_R, J_R_ref, msg='rt_from_Rt grad result') Rt = mrcal.Rt_from_rt(rt0_ref, out=out43) confirm_equal(Rt, Rt0_ref, msg='Rt_from_rt result') Rt, J_r = mrcal.Rt_from_rt(rt0_ref, get_gradients=True, out=(out43, out333)) J_r_ref = grad(R_from_r, rt0_ref[:3]) confirm_equal(Rt, Rt0_ref, msg='Rt_from_rt result') confirm_equal(J_r, J_r_ref, msg='Rt_from_rt grad result')
weight01 = (np.random.rand(*q_ref.shape[:-1]) + 1.) / 2. # in [0,1]
weight0 = 0.2
weight1 = 1.0
weight = weight0 + (weight1 - weight0) * weight01
# I want observations of shape (Nframes*Ncameras, Nh, Nw, 3) where each row is
# (x,y,weight)
observations_ref = nps.clump(nps.glue(q_ref, nps.dummy(weight, -1), axis=-1),
n=2)
# These are perfect
intrinsics_ref = nps.cat(*[m.intrinsics()[1] for m in models_ref])
extrinsics_ref = nps.cat(*[m.extrinsics_rt_fromref() for m in models_ref[1:]])
if extrinsics_ref.size == 0:
extrinsics_ref = np.zeros((0, 6), dtype=float)
frames_ref = mrcal.rt_from_Rt(Rt_ref_board_ref)
# Dense observations. All the cameras see all the boards
indices_frame_camera = np.zeros((Nframes * Ncameras, 2), dtype=np.int32)
indices_frame = indices_frame_camera[:, 0].reshape(Nframes, Ncameras)
indices_frame.setfield(nps.outer(np.arange(Nframes, dtype=np.int32),
np.ones((Ncameras, ), dtype=np.int32)),
dtype=np.int32)
indices_camera = indices_frame_camera[:, 1].reshape(Nframes, Ncameras)
indices_camera.setfield(nps.outer(np.ones((Nframes, ), 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,)] - 1,
Rt_cam0_board_true_far = \
nps.glue( np.eye(3),
np.array((0,0,args.extra_observation_at)),
axis=-2)
q_true_far = \
mrcal.project(mrcal.transform_point_Rt(Rt_cam0_board_true_far, c),
*models_true[0].intrinsics())
q_true = nps.glue(q_true_far, q_true, axis=-5)
Rt_cam0_board_true = nps.glue(Rt_cam0_board_true_far,
Rt_cam0_board_true,
axis=-3)
args.Nframes += 1
frames_true = mrcal.rt_from_Rt(Rt_cam0_board_true)
############# I have perfect observations in q_true. I corrupt them by noise
# weight has shape (Nframes, Ncameras, Nh, Nw),
weight01 = (np.random.rand(*q_true.shape[:-1]) + 1.) / 2. # in [0,1]
weight0 = 0.2
weight1 = 1.0
weight = weight0 + (weight1 - weight0) * weight01
# I want observations of shape (Nframes*Ncameras, Nh, Nw, 3) where each row is
# (x,y,weight)
observations_true = nps.clump(nps.glue(q_true, nps.dummy(weight, -1), axis=-1),
n=2)
# Dense observations. All the cameras see all the boards
indices_frame_camera = np.zeros((args.Nframes * args.Ncameras, 2),
p)
R_fit = Rt_fit[:3, :]
t_fit = Rt_fit[3, :]
testutils.confirm_equal(R_fit, R, eps=1e-2, msg='Procrustes fit R')
testutils.confirm_equal(t_fit, t, eps=1e-2, msg='Procrustes fit t')
R_fit_vectors = \
mrcal.align_procrustes_vectors_R01(nps.matmult( p, nps.transpose(R) ) + noise,
p)
testutils.confirm_equal(R_fit_vectors,
R,
eps=1e-2,
msg='Procrustes fit R (vectors)')
testutils.confirm_equal(mrcal.invert_Rt(
mrcal.Rt_from_rt(mrcal.invert_rt(mrcal.rt_from_Rt(Rt)))),
Rt,
msg='Rt/rt and invert')
testutils.confirm_equal(mrcal.compose_Rt(Rt, mrcal.invert_Rt(Rt)),
nps.glue(np.eye(3), np.zeros((3, )), axis=-2),
msg='compose_Rt')
testutils.confirm_equal(mrcal.compose_rt(mrcal.rt_from_Rt(Rt),
mrcal.invert_rt(
mrcal.rt_from_Rt(Rt))),
np.zeros((6, )),
msg='compose_rt')
testutils.confirm_equal(mrcal.identity_Rt(),
nps.glue(np.eye(3), np.zeros((3, )), axis=-2),
# Generate the rectification maps
if kind != "narrow":
azel_kwargs = dict(az_fov_deg=145., el_fov_deg=135., el0_deg=5)
else:
azel_kwargs = dict(az_fov_deg=80., el_fov_deg=80., el0_deg=0)
rectification_maps, cookie = \
mrcal.stereo_rectify_prepare(models, **azel_kwargs)
# Display the geometry of the two cameras in the stereo pair, and of the
# rectified system
Rt_cam0_stereo = cookie['Rt_cam0_stereo']
Rt_cam0_ref = models[0].extrinsics_Rt_fromref()
Rt_stereo_ref = mrcal.compose_Rt(mrcal.invert_Rt(Rt_cam0_stereo),
Rt_cam0_ref)
rt_stereo_ref = mrcal.rt_from_Rt(Rt_stereo_ref)
terminal = dict(
pdf='pdf size 8in,6in noenhanced solid color font ",12"',
svg='svg size 800,600 noenhanced solid dynamic font ",14"',
png='pngcairo size 1024,768 transparent noenhanced crop font ",12"',
gp='gp')
for extension in terminal.keys():
mrcal.show_geometry(
models + [rt_stereo_ref], ("camera0", "camera1", "stereo"),
show_calobjects=False,
_set=('xyplane at -0.5', 'view 60,30,1.7'),
terminal=terminal[extension],
hardcopy=f'/tmp/stereo-geometry-{kind}.{extension}')
# Generate the rectified images, and write to disk
def calibration_baseline(model,
Ncameras,
Nframes,
extra_observation_at,
object_width_n,
object_height_n,
object_spacing,
extrinsics_rt_fromref_true,
calobject_warp_true,
fixedframes,
testdir,
cull_left_of_center=False,
allow_nonidentity_cam0_transform=False,
range_to_boards=4.0):
r'''Compute a calibration baseline as a starting point for experiments
This is a perfect, noiseless solve. Regularization IS enabled, and the returned
model is at the optimization optimum. So the returned models will not sit
exactly at the ground-truth.
NOTE: if not fixedframes: the ref frame in the returned
optimization_inputs_baseline is NOT the ref frame used by the returned
extrinsics and frames arrays. The arrays in optimization_inputs_baseline had to
be transformed to reference off camera 0. If the extrinsics of camera 0 are the
identity, then the two ref coord systems are the same. To avoid accidental bugs,
we have a kwarg allow_nonidentity_cam0_transform, which defaults to False. if
not allow_nonidentity_cam0_transform and norm(extrinsics_rt_fromref_true[0]) >
0: raise
This logic is here purely for safety. A caller that handles non-identity cam0
transforms has to explicitly say that
ARGUMENTS
- model: string. 'opencv4' or 'opencv8' or 'splined'
- ...
'''
if re.match('opencv', model):
models_true = (
mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.opencv8.cameramodel"))
if model == 'opencv4':
# I have opencv8 models_true, but I truncate to opencv4 models_true
for m in models_true:
m.intrinsics(intrinsics=('LENSMODEL_OPENCV4',
m.intrinsics()[1][:8]))
elif model == 'splined':
models_true = (
mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.splined.cameramodel"))
else:
raise Exception("Unknown lens being tested")
models_true = models_true[:Ncameras]
lensmodel = models_true[0].intrinsics()[0]
Nintrinsics = mrcal.lensmodel_num_params(lensmodel)
for i in range(Ncameras):
models_true[i].extrinsics_rt_fromref(extrinsics_rt_fromref_true[i])
if not allow_nonidentity_cam0_transform and \
nps.norm2(extrinsics_rt_fromref_true[0]) > 0:
raise Exception(
"A non-identity cam0 transform was given, but the caller didn't explicitly say that they support this"
)
imagersizes = nps.cat(*[m.imagersize() for m in models_true])
# These are perfect
intrinsics_true = nps.cat(*[m.intrinsics()[1] for m in models_true])
extrinsics_true_mounted = nps.cat(
*[m.extrinsics_rt_fromref() for m in models_true])
x_center = -(Ncameras - 1) / 2.
# shapes (Nframes, Ncameras, Nh, Nw, 2),
# (Nframes, 4,3)
q_true,Rt_ref_board_true = \
mrcal.synthesize_board_observations(models_true,
object_width_n, object_height_n, object_spacing,
calobject_warp_true,
np.array((0., 0., 0., x_center, 0, range_to_boards)),
np.array((np.pi/180.*30., np.pi/180.*30., np.pi/180.*20., 2.5, 2.5, range_to_boards/2.0)),
Nframes)
if extra_observation_at is not None:
q_true_extra,Rt_ref_board_true_extra = \
mrcal.synthesize_board_observations(models_true,
object_width_n, object_height_n, object_spacing,
calobject_warp_true,
np.array((0., 0., 0., x_center, 0, extra_observation_at)),
np.array((np.pi/180.*30., np.pi/180.*30., np.pi/180.*20., 2.5, 2.5, extra_observation_at/10.0)),
Nframes = 1)
q_true = nps.glue(q_true, q_true_extra, axis=-5)
Rt_ref_board_true = nps.glue(Rt_ref_board_true,
Rt_ref_board_true_extra,
axis=-3)
Nframes += 1
frames_true = mrcal.rt_from_Rt(Rt_ref_board_true)
############# I have perfect observations in q_true.
# weight has shape (Nframes, Ncameras, Nh, Nw),
weight01 = (np.random.rand(*q_true.shape[:-1]) + 1.) / 2. # in [0,1]
weight0 = 0.2
weight1 = 1.0
weight = weight0 + (weight1 - weight0) * weight01
if cull_left_of_center:
imagersize = models_true[0].imagersize()
for m in models_true[1:]:
if np.any(m.imagersize() - imagersize):
raise Exception(
"I'm assuming all cameras have the same imager size, but this is false"
)
weight[q_true[..., 0] < imagersize[0] / 2.] /= 1000.
# I want observations of shape (Nframes*Ncameras, Nh, Nw, 3) where each row is
# (x,y,weight)
observations_true = nps.clump(nps.glue(q_true,
nps.dummy(weight, -1),
axis=-1),
n=2)
# Dense observations. All the cameras see all the boards
indices_frame_camera = np.zeros((Nframes * Ncameras, 2), dtype=np.int32)
indices_frame = indices_frame_camera[:, 0].reshape(Nframes, Ncameras)
indices_frame.setfield(nps.outer(np.arange(Nframes, dtype=np.int32),
np.ones((Ncameras, ), dtype=np.int32)),
dtype=np.int32)
indices_camera = indices_frame_camera[:, 1].reshape(Nframes, Ncameras)
indices_camera.setfield(nps.outer(np.ones((Nframes, ), 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 fixedframes:
indices_frame_camintrinsics_camextrinsics[:, 2] -= 1
###########################################################################
# p = mrcal.show_geometry(models_true,
# frames = frames_true,
# object_width_n = object_width_n,
# object_height_n = object_height_n,
# object_spacing = object_spacing)
# sys.exit()
# I now reoptimize the perfect-observations problem. Without regularization,
# this is a no-op: I'm already at the optimum. With regularization, this will
# move us a certain amount (that the test will evaluate). Then I look at
# noise-induced motions off this optimization optimum
optimization_inputs_baseline = \
dict( intrinsics = copy.deepcopy(intrinsics_true),
points = None,
observations_board = observations_true,
indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics,
observations_point = None,
indices_point_camintrinsics_camextrinsics = None,
lensmodel = lensmodel,
calobject_warp = copy.deepcopy(calobject_warp_true),
imagersizes = imagersizes,
calibration_object_spacing = object_spacing,
verbose = False,
do_optimize_frames = not fixedframes,
do_optimize_intrinsics_core = False if model =='splined' else True,
do_optimize_intrinsics_distortions = True,
do_optimize_extrinsics = True,
do_optimize_calobject_warp = True,
do_apply_regularization = True,
do_apply_outlier_rejection = False)
if fixedframes:
# Frames are fixed: each camera has an independent pose
optimization_inputs_baseline['extrinsics_rt_fromref'] = \
copy.deepcopy(extrinsics_true_mounted)
optimization_inputs_baseline['frames_rt_toref'] = copy.deepcopy(
frames_true)
else:
# Frames are NOT fixed: cam0 is fixed as the reference coord system. I
# transform each optimization extrinsics vector to be relative to cam0
optimization_inputs_baseline['extrinsics_rt_fromref'] = \
mrcal.compose_rt(extrinsics_true_mounted[1:,:],
mrcal.invert_rt(extrinsics_true_mounted[0,:]))
optimization_inputs_baseline['frames_rt_toref'] = \
mrcal.compose_rt(extrinsics_true_mounted[0,:], frames_true)
mrcal.optimize(**optimization_inputs_baseline)
models_baseline = \
[ mrcal.cameramodel( optimization_inputs = optimization_inputs_baseline,
icam_intrinsics = i) \
for i in range(Ncameras) ]
return \
optimization_inputs_baseline, \
models_true, models_baseline, \
indices_frame_camintrinsics_camextrinsics, \
lensmodel, Nintrinsics, imagersizes, \
intrinsics_true, extrinsics_true_mounted, frames_true, \
observations_true, \
Nframes
def calibration_baseline(model,
Ncameras,
Nframes,
extra_observation_at,
pixel_uncertainty_stdev,
object_width_n,
object_height_n,
object_spacing,
extrinsics_rt_fromref_true,
calobject_warp_true,
fixedframes,
testdir,
cull_left_of_center=False):
r'''Compute a calibration baseline as a starting point for experiments
This is a perfect, noiseless solve. Regularization IS enabled, and the returned
model is at the optimization optimum. So the returned models will not sit
exactly at the ground-truth
ARGUMENTS
- model: string. 'opencv4' or 'opencv8' or 'splined'
- ...
'''
if re.match('opencv', model):
models_true = (
mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.opencv8.cameramodel"))
if model == 'opencv4':
# I have opencv8 models_true, but I truncate to opencv4 models_true
for m in models_true:
m.intrinsics(intrinsics=('LENSMODEL_OPENCV4',
m.intrinsics()[1][:8]))
elif model == 'splined':
models_true = (
mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.splined.cameramodel"))
else:
raise Exception("Unknown lens being tested")
models_true = models_true[:Ncameras]
lensmodel = models_true[0].intrinsics()[0]
Nintrinsics = mrcal.lensmodel_num_params(lensmodel)
for i in range(Ncameras):
models_true[i].extrinsics_rt_fromref(extrinsics_rt_fromref_true[i])
imagersizes = nps.cat(*[m.imagersize() for m in models_true])
# These are perfect
intrinsics_true = nps.cat(*[m.intrinsics()[1] for m in models_true])
extrinsics_true_mounted = nps.cat(
*[m.extrinsics_rt_fromref() for m in models_true])
x_center = -(Ncameras - 1) / 2.
# shapes (Nframes, Ncameras, Nh, Nw, 2),
# (Nframes, 4,3)
q_true,Rt_cam0_board_true = \
mrcal.synthesize_board_observations(models_true,
object_width_n, object_height_n, object_spacing,
calobject_warp_true,
np.array((0., 0., 0., x_center, 0, 4.0)),
np.array((np.pi/180.*30., np.pi/180.*30., np.pi/180.*20., 2.5, 2.5, 2.0)),
Nframes)
if extra_observation_at:
c = mrcal.ref_calibration_object(object_width_n, object_height_n,
object_spacing, calobject_warp_true)
Rt_cam0_board_true_far = \
nps.glue( np.eye(3),
np.array((0,0,extra_observation_at)),
axis=-2)
q_true_far = \
mrcal.project(mrcal.transform_point_Rt(Rt_cam0_board_true_far, c),
*models_true[0].intrinsics())
q_true = nps.glue(q_true_far, q_true, axis=-5)
Rt_cam0_board_true = nps.glue(Rt_cam0_board_true_far,
Rt_cam0_board_true,
axis=-3)
Nframes += 1
frames_true = mrcal.rt_from_Rt(Rt_cam0_board_true)
############# I have perfect observations in q_true. I corrupt them by noise
# weight has shape (Nframes, Ncameras, Nh, Nw),
weight01 = (np.random.rand(*q_true.shape[:-1]) + 1.) / 2. # in [0,1]
weight0 = 0.2
weight1 = 1.0
weight = weight0 + (weight1 - weight0) * weight01
if cull_left_of_center:
imagersize = models_true[0].imagersize()
for m in models_true[1:]:
if np.any(m.imagersize() - imagersize):
raise Exception(
"I'm assuming all cameras have the same imager size, but this is false"
)
weight[q_true[..., 0] < imagersize[0] / 2.] /= 1000.
# I want observations of shape (Nframes*Ncameras, Nh, Nw, 3) where each row is
# (x,y,weight)
observations_true = nps.clump(nps.glue(q_true,
nps.dummy(weight, -1),
axis=-1),
n=2)
# Dense observations. All the cameras see all the boards
indices_frame_camera = np.zeros((Nframes * Ncameras, 2), dtype=np.int32)
indices_frame = indices_frame_camera[:, 0].reshape(Nframes, Ncameras)
indices_frame.setfield(nps.outer(np.arange(Nframes, dtype=np.int32),
np.ones((Ncameras, ), dtype=np.int32)),
dtype=np.int32)
indices_camera = indices_frame_camera[:, 1].reshape(Nframes, Ncameras)
indices_camera.setfield(nps.outer(np.ones((Nframes, ), 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 fixedframes:
indices_frame_camintrinsics_camextrinsics[:, 2] -= 1
###########################################################################
# Now I apply pixel noise, and look at the effects on the resulting calibration.
# p = mrcal.show_geometry(models_true,
# frames = frames_true,
# object_width_n = object_width_n,
# object_height_n = object_height_n,
# object_spacing = object_spacing)
# sys.exit()
# I now reoptimize the perfect-observations problem. Without regularization,
# this is a no-op: I'm already at the optimum. With regularization, this will
# move us a certain amount (that the test will evaluate). Then I look at
# noise-induced motions off this optimization optimum
optimization_inputs_baseline = \
dict( intrinsics = copy.deepcopy(intrinsics_true),
extrinsics_rt_fromref = copy.deepcopy(extrinsics_true_mounted if fixedframes else extrinsics_true_mounted[1:,:]),
frames_rt_toref = copy.deepcopy(frames_true),
points = None,
observations_board = observations_true,
indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics,
observations_point = None,
indices_point_camintrinsics_camextrinsics = None,
lensmodel = lensmodel,
calobject_warp = copy.deepcopy(calobject_warp_true),
imagersizes = imagersizes,
calibration_object_spacing = object_spacing,
verbose = False,
observed_pixel_uncertainty = pixel_uncertainty_stdev,
do_optimize_frames = not fixedframes,
do_optimize_intrinsics_core = False if model =='splined' else True,
do_optimize_intrinsics_distortions = True,
do_optimize_extrinsics = True,
do_optimize_calobject_warp = True,
do_apply_regularization = True,
do_apply_outlier_rejection = False)
mrcal.optimize(**optimization_inputs_baseline)
models_baseline = \
[ mrcal.cameramodel( optimization_inputs = optimization_inputs_baseline,
icam_intrinsics = i) \
for i in range(Ncameras) ]
return \
optimization_inputs_baseline, \
models_true, models_baseline, \
indices_frame_camintrinsics_camextrinsics, \
lensmodel, Nintrinsics, imagersizes, \
intrinsics_true, extrinsics_true_mounted, frames_true, \
observations_true, \
Nframes