def grad(f, x, step=1e-6):
r'''Computes df/dx at x
f is a function of one argument. If the input has shape Si and the output
has shape So, the returned gradient has shape So+Si. This applies central
differences
'''
d = np.zeros(x.shape, dtype=float)
dflat = d.ravel()
def df_dxi(i, d, dflat):
dflat[i] = step
fplus = f(x + d)
fminus = f(x - d)
j = (fplus - fminus) / (2. * step)
dflat[i] = 0
return j
# grad variable is in first dim
Jflat = nps.cat(*[df_dxi(i, d, dflat) for i in range(len(dflat))])
# grad variable is in last dim
Jflat = nps.mv(Jflat, 0, -1)
return Jflat.reshape(Jflat.shape[:-1] + d.shape)
def get_observation_chunk():
'''Make Nframes observations, and return them all, even the out-of-view ones'''
# I compute the full random block in one shot. This is useful for
# simulations that want to see identical poses when asking for N-1
# random poses and when asking for the first N-1 of a set of N random
# poses
# shape (Nframes,6)
randomblock = np.random.uniform(low=-1.0, high=1.0, size=(Nframes, 6))
# shape(Nframes,4,3)
Rt_ref_boardref = \
mrcal.Rt_from_rt( rt_ref_boardcenter + randomblock * rt_ref_boardcenter__noiseradius )
# shape = (Nframes, Nh,Nw,3)
boards_ref = mrcal.transform_point_Rt( # shape (Nframes, 1,1,4,3)
nps.mv(Rt_ref_boardref, 0, -5),
# shape ( Nh,Nw,3)
board_reference)
# I project full_board. Shape: (Nframes,Ncameras,Nh,Nw,2)
q = \
nps.mv( \
nps.cat( \
*[ mrcal.project( mrcal.transform_point_Rt(models[i].extrinsics_Rt_fromref(), boards_ref),
*models[i].intrinsics()) \
for i in range(Ncameras) ]),
0,1 )
return q, Rt_ref_boardref
def _splined_stereographic_domain(lensmodel):
r'''Return the stereographic domain for splined-stereographic lens models
SYNOPSIS
model = mrcal.cameramodel(model_filename)
lensmodel = model.intrinsics()[0]
domain_contour = mrcal._splined_stereographic_domain(lensmodel)
Splined stereographic models are defined by a splined surface. This surface is
indexed by normalized stereographic-projected points. This surface is defined in
some finite area, and this function reports a piecewise linear contour reporting
this region.
This function only makes sense for splined stereographic models.
RETURNED VALUE
An array of shape (N,2) containing a contour representing the projection domain.
'''
if not re.match('LENSMODEL_SPLINED_STEREOGRAPHIC', lensmodel):
raise Exception(f"This only makes sense with splined models. Input uses {lensmodel}")
ux,uy = mrcal.knots_for_splined_models(lensmodel)
# shape (Ny,Nx,2)
u = np.ascontiguousarray(nps.mv(nps.cat(*np.meshgrid(ux,uy)), 0, -1))
meta = mrcal.lensmodel_metadata(lensmodel)
if meta['order'] == 2:
# spline order is 3. The valid region is 1/2 segments inwards from the
# outer contour
return \
nps.glue( (u[0,1:-2] + u[1,1:-2]) / 2.,
(u[0,-2] + u[1,-2] + u[0,-1] + u[1,-1]) / 4.,
(u[1:-2, -2] + u[1:-2, -1]) / 2.,
(u[-2,-2] + u[-1,-2] + u[-2,-1] + u[-1,-1]) / 4.,
(u[-2, -2:1:-1] + u[-1, -2:1:-1]) / 2.,
(u[-2, 1] + u[-1, 1] + u[-2, 0] + u[-1, 0]) / 4.,
(u[-2:0:-1, 0] +u[-2:0:-1, 1]) / 2.,
(u[0, 0] +u[0, 1] + u[1, 0] +u[1, 1]) / 4.,
(u[0,1] + u[1,1]) / 2.,
axis = -2 )
elif meta['order'] == 3:
# spline order is 3. The valid region is the outer contour, leaving one
# knot out
return \
nps.glue( u[1,1:-2], u[1:-2, -2], u[-2, -2:1:-1], u[-2:0:-1, 1],
axis=-2 )
else:
raise Exception("I only support cubic (order==3) and quadratic (order==2) models")
def sample_imager(gridn_width, gridn_height, imager_width, imager_height):
r'''Returns regularly-sampled, gridded pixels coordinates across the imager
SYNOPSIS
q = sample_imager( 60, 40, *model.imagersize() )
print(q.shape)
===>
(40,60,2)
Note that the arguments are given in width,height order, as is customary when
generally talking about images and indexing. However, the output is in
height,width order, as is customary when talking about matrices and numpy
arrays.
If we ask for gridding dimensions (gridn_width, gridn_height), the output has
shape (gridn_height,gridn_width,2) where each row is an (x,y) pixel coordinate.
The top-left corner is at [0,0,:]:
sample_imager(...)[0,0] = [0,0]
The the bottom-right corner is at [-1,-1,:]:
sample_imager(...)[ -1, -1,:] =
sample_imager(...)[gridn_height-1,gridn_width-1,:] =
(imager_width-1,imager_height-1)
When making plots you probably want to call mrcal.imagergrid_using(). See the
that docstring for details.
ARGUMENTS
- gridn_width: how many points along the horizontal gridding dimension
- gridn_height: how many points along the vertical gridding dimension. If None,
we compute an integer gridn_height to maintain a square-ish grid:
gridn_height/gridn_width ~ imager_height/imager_width
- imager_width,imager_height: the width, height of the imager. With a
mrcal.cameramodel object this is *model.imagersize()
RETURNED VALUES
We return an array of shape (gridn_height,gridn_width,2). Each row is an (x,y)
pixel coordinate.
'''
if gridn_height is None:
gridn_height = int(round(imager_height/imager_width*gridn_width))
w = np.linspace(0,imager_width -1,gridn_width)
h = np.linspace(0,imager_height-1,gridn_height)
return np.ascontiguousarray(nps.mv(nps.cat(*np.meshgrid(w,h)),
0,-1))
def model_matrix(q, order):
r'''Returns the model matrix S for particular domain points
Here the "order" is the number of parameters in the fit. Thus order==2 means
"linear" and order==3 means "quadratic""
'''
q = nps.atleast_dims(q,-1)
return nps.transpose(nps.cat(*[q ** i for i in range(order)]))
def rotation_any_v_to_z(v):
r'''Return any rotation matrix that maps the given unit vector v to [0,0,1]'''
z = v
if np.abs(v[0]) < .9:
x = np.array((1,0,0))
else:
x = np.array((0,1,0))
x -= nps.inner(x,v)*v
x /= nps.mag(x)
y = np.cross(z,x)
return nps.cat(x,y,z)
def sample_dqref(observations, pixel_uncertainty_stdev, make_outliers=False):
# Outliers have weight < 0. The code will adjust the outlier observations
# also. But that shouldn't matter: they're outliers so those observations
# should be ignored
weight = observations[..., -1]
q_noise = np.random.randn(*observations.shape[:-1],
2) * pixel_uncertainty_stdev / nps.mv(
nps.cat(weight, weight), 0, -1)
if make_outliers:
if not hasattr(sample_dqref, 'idx_outliers_ref_flat'):
NobservedPoints = observations.size // 3
sample_dqref.idx_outliers_ref_flat = \
np.random.choice( NobservedPoints,
(NobservedPoints//100,), # 1% outliers
replace = False )
nps.clump(q_noise, n=3)[sample_dqref.idx_outliers_ref_flat, :] *= 20
observations_perturbed = observations.copy()
observations_perturbed[..., :2] += q_noise
return q_noise, observations_perturbed
p,
msg='transform_point_Rt inverse')
testutils.confirm_equal(mrcal.transform_point_rt(
mrcal.invert_rt(mrcal.rt_from_Rt(Rt)), Tp),
p,
msg='transform_point_rt inverse')
######### quaternion business
testutils.confirm_equal(
mrcal.R_from_quat(mrcal.quat_from_R(R)),
R,
msg='R <-> quaternion transforms are inverses of one another')
# shape (2,3,3)
RR = nps.cat(R, nps.matmult(R, R))
# shape (2,4,3)
RtRt = nps.cat(Rt, mrcal.compose_Rt(Rt, Rt))
testutils.confirm_equal(
mrcal.R_from_quat(mrcal.quat_from_R(RR)),
RR,
msg='R <-> quaternion transforms are inverses of one another. Broadcasted')
# I'm concerned about quat_from_R() broadcasting properly. I check
testutils.confirm_equal(mrcal.quat_from_R(RR.reshape(2, 1, 3, 3)).shape,
(2, 1, 4),
msg='quat_from_R() shape')
testutils.confirm_equal(mrcal.quat_from_R(RR.reshape(2, 3, 3)).shape, (2, 4),
msg='quat_from_R() shape')
testutils.confirm_equal(mrcal.quat_from_R(R).shape, (4, ),
msg='quat_from_R() shape')
import copy
import testutils
from test_calibration_helpers import sample_dqref
# I want the RNG to be deterministic
np.random.seed(0)
############# Set up my world, and compute all the perfect positions, pixel
############# observations of everything
models_ref = (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"))
imagersizes = nps.cat(*[m.imagersize() for m in models_ref])
lensmodel = models_ref[0].intrinsics()[0]
# I have opencv8 models_ref, but let me truncate to opencv4 models_ref to keep this
# simple and fast
lensmodel = 'LENSMODEL_OPENCV4'
for m in models_ref:
m.intrinsics(intrinsics=(lensmodel, m.intrinsics()[1][:8]))
Nintrinsics = mrcal.lensmodel_num_params(lensmodel)
Ncameras = len(models_ref)
Ncameras_extrinsics = Ncameras - 1
Nframes = 50
models_ref[0].extrinsics_rt_fromref(np.zeros((6, ), dtype=float))
models_ref[1].extrinsics_rt_fromref(np.array((0.08, 0.2, 0.02, 1., 0.9, 0.1)))
def test_geometry(Rt01,
p,
whatgeometry,
out_of_bounds=False,
check_gradients=False):
R01 = Rt01[:3, :]
t01 = Rt01[3, :]
# p now has shape (Np,3). The leading dims have been flattened
p = p.reshape(p.size // 3, 3)
Np = len(p)
# p has shape (Np,3)
# v has shape (Np,2)
v0local_noisy, v1local_noisy,v0_noisy,v1_noisy,q0_ref,q1_ref,q0_noisy,q1_noisy = \
[v[...,0,:] for v in \
mrcal.synthetic_data.
_noisy_observation_vectors_for_triangulation(p, Rt01,
model0.intrinsics(),
model1.intrinsics(),
1,
sigma = 0.1)]
scenarios = \
( (mrcal.triangulate_geometric, callback_l2_geometric, v0_noisy, v1_noisy, t01),
(mrcal.triangulate_leecivera_l1, callback_l1_angle, v0_noisy, v1_noisy, t01),
(mrcal.triangulate_leecivera_linf, callback_linf_angle, v0_noisy, v1_noisy, t01),
(mrcal.triangulate_leecivera_mid2, None, v0_noisy, v1_noisy, t01),
(mrcal.triangulate_leecivera_wmid2,None, v0_noisy, v1_noisy, t01),
(mrcal.triangulate_lindstrom, callback_l2_reprojection, v0local_noisy, v1local_noisy, Rt01),
)
for scenario in scenarios:
f, callback = scenario[:2]
args = scenario[2:]
result = f(*args, get_gradients=True)
p_reported = result[0]
what = f"{whatgeometry} {f.__name__}"
if out_of_bounds:
p_optimized = np.zeros(p_reported.shape)
else:
# Check all the gradients
if check_gradients:
grads = result[1:]
for ip in range(Np):
args_cut = (args[0][ip], args[1][ip], args[2])
for ivar in range(len(args)):
grad_empirical = \
grad( lambda x: f( *args_cut[:ivar],
x,
*args_cut[ivar+1:]),
args_cut[ivar],
step = 1e-6)
testutils.confirm_equal(
grads[ivar][ip],
grad_empirical,
relative=True,
worstcase=True,
msg=f"{what}: grad(ip={ip}, ivar = {ivar})",
eps=2e-2)
if callback is not None:
# I run an optimization to directly optimize the quantity each triangulation
# routine is supposed to be optimizing, and then I compare
p_optimized = \
nps.cat(*[ scipy.optimize.minimize(callback,
p_reported[ip], # seed from the "right" value
args = (args[0][ip], args[1][ip], args[2]),
method = 'Nelder-Mead',
# options = dict(disp = True)
)['x'] \
for ip in range(Np) ])
# print( f"{what} p reported,optimized:\n{nps.cat(p_reported, p_optimized)}" )
# print( f"{what} p_err: {p_reported - p_optimized}" )
# print( f"{what} optimum reported/optimized:\n{callback(p_reported, *args)/callback(p_optimized, *args)}" )
testutils.confirm_equal(p_reported,
p_optimized,
relative=True,
worstcase=True,
msg=what,
eps=1e-3)
else:
# No callback defined. Compare projected q
q0 = mrcal.project(p_reported, *model0.intrinsics())
q1 = mrcal.project(
mrcal.transform_point_Rt(mrcal.invert_Rt(Rt01),
p_reported), *model1.intrinsics())
testutils.confirm_equal(q0,
q0_ref,
relative=False,
worstcase=True,
msg=f'{what} q0',
eps=25.)
testutils.confirm_equal(q1,
q1_ref,
relative=False,
worstcase=True,
msg=f'{what} q1',
eps=25.)
i = (1, 2, 4, 5)
observations = observations[i, ...]
indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics[
i, ...]
paths = [paths[_] for _ in i]
# reference models
models = [
mrcal.cameramodel(m) for m in (
f"{testdir}/data/cam0.opencv8.cameramodel",
f"{testdir}/data/cam1.opencv8.cameramodel",
)
]
lensmodel = models[0].intrinsics()[0]
intrinsics_data = nps.cat(models[0].intrinsics()[1], models[1].intrinsics()[1])
extrinsics_rt_fromref = mrcal.compose_rt(models[1].extrinsics_rt_fromref(),
models[0].extrinsics_rt_toref())
imagersizes = nps.cat(models[0].imagersize(), models[1].imagersize())
# I now have the "right" camera parameters. I don't have the frames or points,
# but it's fine to just make them up. This is a regression test.
frames_rt_toref = linspace_shaped(3, 6)
frames_rt_toref[:, 5] += 5 # push them back
indices_point_camintrinsics_camextrinsics = \
np.array(((0,1,-1),
(1,0,-1),
(1,1, 0),
(2,0,-1),
(2,1, 0)),
import mrcal
import testutils
from test_calibration_helpers import sample_dqref
# I want the RNG to be deterministic
np.random.seed(0)
############# Set up my world, and compute all the perfect positions, pixel
############# observations of everything
models_ref = (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"))
imagersizes = nps.cat(*[m.imagersize() for m in models_ref])
lensmodel = models_ref[0].intrinsics()[0]
# I have opencv8 models_ref, but let me truncate to opencv4 models_ref to keep this
# simple and fast
lensmodel = 'LENSMODEL_OPENCV4'
for m in models_ref:
m.intrinsics(intrinsics=(lensmodel, m.intrinsics()[1][:8]))
Nintrinsics = mrcal.lensmodel_num_params(lensmodel)
Ncameras = len(models_ref)
Nframes = 50
models_ref[0].extrinsics_rt_fromref(np.zeros((6, ), dtype=float))
models_ref[1].extrinsics_rt_fromref(np.array((0.08, 0.2, 0.02, 1., 0.9, 0.1)))
models_ref[2].extrinsics_rt_fromref(np.array((0.01, 0.07, 0.2, 2.1, 0.4, 0.2)))
models_ref[3].extrinsics_rt_fromref(np.array(
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
nps.transpose(rectification_maps[0][...,0]))
interp_rectification_map0y = \
scipy.interpolate.RectBivariateSpline(cookie['az_row'].ravel(),
cookie['el_col'].ravel(),
nps.transpose(rectification_maps[0][...,1]))
interp_rectification_map1x = \
scipy.interpolate.RectBivariateSpline(cookie['az_row'].ravel(),
cookie['el_col'].ravel(),
nps.transpose(rectification_maps[1][...,0]))
interp_rectification_map1y = \
scipy.interpolate.RectBivariateSpline(cookie['az_row'].ravel(),
cookie['el_col'].ravel(),
nps.transpose(rectification_maps[1][...,1]))
qrect0 = nps.transpose(
nps.cat(interp_rectification_map0x(az0, el0, grid=False),
interp_rectification_map0y(az0, el0, grid=False)))
qrect1 = nps.transpose(
nps.cat(interp_rectification_map1x(az1, el1, grid=False),
interp_rectification_map1y(az1, el1, grid=False)))
testutils.confirm_equal(qrect0,
qcam0,
eps=1e-1,
msg='rectification map for camera 0 points')
testutils.confirm_equal(qrect1,
qcam1,
eps=1e-1,
msg='rectification map for camera 1 points')
# same point, so we should have the same el
testutils.confirm_equal(el0,
def image_transformation_map(model_from, model_to,
use_rotation = False,
plane_n = None,
plane_d = None,
mask_valid_intrinsics_region_from = False):
r'''Compute a reprojection map between two models
SYNOPSIS
model_orig = mrcal.cameramodel("xxx.cameramodel")
image_orig = cv2.imread("image.jpg")
model_pinhole = mrcal.pinhole_model_for_reprojection(model_orig,
fit = "corners")
mapxy = mrcal.image_transformation_map(model_orig, model_pinhole)
image_undistorted = mrcal.transform_image(image_orig, mapxy)
# image_undistorted is now a pinhole-reprojected version of image_orig
Returns the transformation that describes a mapping
- from pixel coordinates of an image of a scene observed by model_to
- to pixel coordinates of an image of the same scene observed by model_from
This transformation can then be applied to a whole image by calling
mrcal.transform_image().
This function returns a transformation map in an (Nheight,Nwidth,2) array. The
image made by model_to will have shape (Nheight,Nwidth). Each pixel (x,y) in
this image corresponds to a pixel mapxy[y,x,:] in the image made by model_from.
This function has 3 modes of operation:
- intrinsics-only
This is the default. Selected if
- use_rotation = False
- plane_n = None
- plane_d = None
All of the extrinsics are ignored. If the two cameras have the same
orientation, then their observations of infinitely-far-away objects will line
up exactly
- rotation
This can be selected explicitly with
- use_rotation = True
- plane_n = None
- plane_d = None
Here we use the rotation component of the relative extrinsics. The relative
translation is impossible to use without knowing what we're looking at, so IT
IS ALWAYS IGNORED. If the relative orientation in the models matches reality,
then the two cameras' observations of infinitely-far-away objects will line up
exactly
- plane
This is selected if
- use_rotation = True
- plane_n is not None
- plane_d is not None
We map observations of a given plane in camera FROM coordinates
coordinates to where this plane would be observed by camera TO. This uses
ALL the intrinsics, extrinsics and the plane representation. If all of
these are correct, the observations of this plane would line up exactly in
the remapped-camera-fromimage and the camera-to image. The plane is
represented in camera-from coordinates by a normal vector plane_n, and the
distance to the normal plane_d. The plane is all points p such that
inner(p,plane_n) = plane_d. plane_n does not need to be normalized; any
scaling is compensated in plane_d.
ARGUMENTS
- model_from: the mrcal.cameramodel object describing the camera used to capture
the input image
- model_to: the mrcal.cameramodel object describing the camera that would have
captured the image we're producing
- use_rotation: optional boolean, defaulting to False. If True: we respect the
relative rotation in the extrinsics of the camera models.
- plane_n: optional numpy array of shape (3,); None by default. If given, we
produce a transformation to map observations of a given plane to the same
pixels in the source and target images. This argument describes the normal
vector in the coordinate system of model_from. The plane is all points p such
that inner(p,plane_n) = plane_d. plane_n does not need to be normalized; any
scaling is compensated in plane_d. If given, plane_d should be given also, and
use_rotation should be True. if given, we use the full intrinsics and
extrinsics of both camera models
- plane_d: optional floating-point value; None by default. If given, we produce
a transformation to map observations of a given plane to the same pixels in
the source and target images. The plane is all points p such that
inner(p,plane_n) = plane_d. plane_n does not need to be normalized; any
scaling is compensated in plane_d. If given, plane_n should be given also, and
use_rotation should be True. if given, we use the full intrinsics and
extrinsics of both camera models
- mask_valid_intrinsics_region_from: optional boolean defaulting to False. If
True, points outside the valid-intrinsics region in the FROM image are set to
black, and thus do not appear in the output image
RETURNED VALUE
A numpy array of shape (Nheight,Nwidth,2) where Nheight and Nwidth represent the
imager dimensions of model_to. This array contains 32-bit floats, as required by
cv2.remap() (the function providing the internals of mrcal.transform_image()).
This array can be passed to mrcal.transform_image()
'''
if (plane_n is None and plane_d is not None) or \
(plane_n is not None and plane_d is None):
raise Exception("plane_n and plane_d should both be None or neither should be None")
if plane_n is not None and plane_d is not None and \
not use_rotation:
raise Exception("We're looking at remapping a plane (plane_d, plane_n are not None), so use_rotation should be True")
Rt_to_from = None
if use_rotation:
Rt_to_r = model_to. extrinsics_Rt_fromref()
Rt_r_from = model_from.extrinsics_Rt_toref()
Rt_to_from = mrcal.compose_Rt(Rt_to_r, Rt_r_from)
lensmodel_from,intrinsics_data_from = model_from.intrinsics()
lensmodel_to, intrinsics_data_to = model_to. intrinsics()
if re.match("LENSMODEL_OPENCV",lensmodel_from) and \
lensmodel_to == "LENSMODEL_PINHOLE" and \
plane_n is None and \
not mask_valid_intrinsics_region_from:
# This is a common special case. This branch works identically to the
# other path (the other side of this "if" can always be used instead),
# but the opencv-specific code is optimized and at one point ran faster
# than the code on the other side.
#
# The mask_valid_intrinsics_region_from isn't implemented in this path.
# It COULD be, then this faster path could be used
fxy_from = intrinsics_data_from[0:2]
cxy_from = intrinsics_data_from[2:4]
cameraMatrix_from = np.array(((fxy_from[0], 0, cxy_from[0]),
( 0, fxy_from[1], cxy_from[1]),
( 0, 0, 1)))
fxy_to = intrinsics_data_to[0:2]
cxy_to = intrinsics_data_to[2:4]
cameraMatrix_to = np.array(((fxy_to[0], 0, cxy_to[0]),
( 0, fxy_to[1], cxy_to[1]),
( 0, 0, 1)))
output_shape = model_to.imagersize()
distortion_coeffs = intrinsics_data_from[4: ]
if Rt_to_from is not None:
R_to_from = Rt_to_from[:3,:]
if np.trace(R_to_from) > 3. - 1e-12:
R_to_from = None # identity, so I pass None
else:
R_to_from = None
return nps.glue( *[ nps.dummy(arr,-1) for arr in \
cv2.initUndistortRectifyMap(cameraMatrix_from, distortion_coeffs,
R_to_from,
cameraMatrix_to, tuple(output_shape),
cv2.CV_32FC1)],
axis = -1)
W_from,H_from = model_from.imagersize()
W_to, H_to = model_to. imagersize()
# shape: (Nheight,Nwidth,2). Contains (x,y) rows
grid = np.ascontiguousarray(nps.mv(nps.cat(*np.meshgrid(np.arange(W_to),
np.arange(H_to))),
0,-1),
dtype = float)
if lensmodel_to == "LENSMODEL_PINHOLE":
# Faster path for the unproject. Nice, simple closed-form solution
fxy_to = intrinsics_data_to[0:2]
cxy_to = intrinsics_data_to[2:4]
v = np.zeros( (grid.shape[0], grid.shape[1], 3), dtype=float)
v[..., :2] = (grid-cxy_to)/fxy_to
v[..., 2] = 1
elif lensmodel_to == "LENSMODEL_STEREOGRAPHIC":
# Faster path for the unproject. Nice, simple closed-form solution
v = mrcal.unproject_stereographic(grid, *intrinsics_data_to[:4])
else:
v = mrcal.unproject(grid, lensmodel_to, intrinsics_data_to)
if plane_n is not None:
R_to_from = Rt_to_from[:3,:]
t_to_from = Rt_to_from[ 3,:]
# The homography definition. Derived in many places. For instance in
# "Motion and structure from motion in a piecewise planar environment"
# by Olivier Faugeras, F. Lustman.
A_to_from = plane_d * R_to_from + nps.outer(t_to_from, plane_n)
A_from_to = np.linalg.inv(A_to_from)
v = nps.matmult( v, nps.transpose(A_from_to) )
else:
if Rt_to_from is not None:
R_to_from = Rt_to_from[:3,:]
if np.trace(R_to_from) < 3. - 1e-12:
# rotation isn't identity. apply
v = nps.matmult(v, R_to_from)
mapxy = mrcal.project( v, lensmodel_from, intrinsics_data_from )
if mask_valid_intrinsics_region_from:
# Using matplotlib to compute the out-of-bounds points. It doesn't
# support broadcasting, so I do that manually with a clump/reshape
from matplotlib.path import Path
region = Path(model_from.valid_intrinsics_region())
is_inside = region.contains_points(nps.clump(mapxy,n=2)).reshape(mapxy.shape[:2])
mapxy[ ~is_inside, :] = -1
return mapxy.astype(np.float32)
y_3d = (np.cos(ph) * np.sin(th)) .ravel()
z_3d = (np.sin(ph) * np.ones( th.shape )) .ravel()
rho = np.linspace(0, 2*np.pi, 1000) # dim=( 1000,)
a = np.arange(-4,3)[:, np.newaxis] # dim=(7,1)
#################################
# Now the demos!
#################################
# first, some very basic stuff. Testing implicit domains, multiple curves in
# arguments, packed broadcastable data, etc
gp.plot(x**2, wait=1)
gp.plot(( np.transpose(nps.cat(x,x**2)),
dict(_with='linespoints pt 4 ps 2'),
),
( 5,60,
dict(tuplesize=2,
_with='linespoints pt 5 ps 2'),
),
( np.array((3,40)),
dict(_with='linespoints pt 6 ps 2'),
),
tuplesize = -2,
wait=1)
gp.plot(-x, x**3, wait=1)
gp.plot((x**2), wait=1)
gp.plot((-x, x**3, {'with': 'lines'}), (x**2,), wait=1)
gp.plot( x, np.vstack((x**3, x**2)) , wait=1)
x_3d = (np.cos(ph) * np.cos(th)).ravel()
y_3d = (np.cos(ph) * np.sin(th)).ravel()
z_3d = (np.sin(ph) * np.ones(th.shape)).ravel()
rho = np.linspace(0, 2 * np.pi, 1000) # dim=( 1000,)
a = np.arange(-4, 3)[:, np.newaxis] # dim=(7,1)
#################################
# Now the demos!
#################################
# first, some very basic stuff. Testing implicit domains, multiple curves in
# arguments, packed broadcastable data, etc
gp.plot(x**2, wait=1)
gp.plot((
np.transpose(nps.cat(x, x**2)),
dict(_with='linespoints pt 4 ps 2'),
), (
5,
60,
dict(tuplesize=2, _with='linespoints pt 5 ps 2'),
), (
np.array((3, 40)),
dict(_with='linespoints pt 6 ps 2'),
),
tuplesize=-2,
wait=1)
gp.plot(-x, x**3, wait=1)
gp.plot((x**2), wait=1)
gp.plot((-x, x**3, dict(_with='lines')), (x**2, ), wait=1)
gp.plot(x, nps.cat(x**3, x**2), wait=1)
else:
obs_cam = [ ( (observed_points(icam),),
(q0_baseline, dict(_with = f'points pt 3 lw 2 lc "red" ps {2*pointscale[""]}'))) \
for icam in range(args.Ncameras) ]
processoptions_output = dict(wait=True)
gp.plot(*obs_cam,
tuplesize=-2,
_with='dots',
square=1,
_xrange=(0, models_true[0].imagersize()[0] - 1),
_yrange=(models_true[0].imagersize()[1] - 1, 0),
multiplot='layout 2,2',
**processoptions_output)
# These are at the optimum
intrinsics_baseline = nps.cat(*[m.intrinsics()[1] for m in models_baseline])
extrinsics_baseline_mounted = nps.cat(
*[m.extrinsics_rt_fromref() for m in models_baseline])
frames_baseline = optimization_inputs_baseline['frames_rt_toref']
calobject_warp_baseline = optimization_inputs_baseline['calobject_warp']
if args.write_models:
for i in range(args.Ncameras):
models_true[i].write(f"/tmp/models-true-camera{i}.cameramodel")
models_baseline[i].write(f"/tmp/models-baseline-camera{i}.cameramodel")
sys.exit()
def reproject_perturbed__mean_frames(
q,
distance,
def ref_calibration_object(W, H, object_spacing, calobject_warp=None):
r'''Return the geometry of the calibration object
SYNOPSIS
import gnuplotlib as gp
import numpysane as nps
obj = mrcal.ref_calibration_object( 10,6, 0.1 )
print(obj.shape)
===> (6, 10, 3)
gp.plot( nps.clump( obj[...,:2], n=2),
tuplesize = -2,
_with = 'points',
_xrange = (-0.1,1.0),
_yrange = (-0.1,0.6),
unset = 'grid',
square = True,
terminal = 'dumb 74,45')
0.6 +---------------------------------------------------------------+
| + + + + + |
| |
0.5 |-+ A A A A A A A A A A +-|
| |
| |
0.4 |-+ A A A A A A A A A A +-|
| |
| |
0.3 |-+ A A A A A A A A A A +-|
| |
| |
0.2 |-+ A A A A A A A A A A +-|
| |
| |
0.1 |-+ A A A A A A A A A A +-|
| |
| |
0 |-+ A A A A A A A A A A +-|
| |
| + + + + + |
-0.1 +---------------------------------------------------------------+
0 0.2 0.4 0.6 0.8 1
Returns the geometry of a calibration object in its own reference coordinate
system in a (H,W,3) array. Only a grid-of-points calibration object is
supported, possibly with some bowing (i.e. what the internal mrcal solver
supports). Each row of the output is an (x,y,z) point. The origin is at the
corner of the grid, so ref_calibration_object(...)[0,0,:] is
np.array((0,0,0)). The grid spans x and y, with z representing the depth: z=0
for a flat calibration object.
A simple parabolic board warping model is supported by passing a (2,) array in
calobject_warp. These 2 values describe additive flex along the x axis and along
the y axis, in that order. In each direction the flex is a parabola, with the
parameter k describing the max deflection at the center. If the edges were at
+-1 we'd have
z = k*(1 - x^2)
The edges we DO have are at (0,N-1), so the equivalent expression is
xr = x / (N-1)
z = k*( 1 - 4*xr^2 + 4*xr - 1 ) =
4*k*(xr - xr^2) =
ARGUMENTS
- W: how many points we have in the horizontal direction
- H: how many points we have in the vertical direction
- object_spacing: the distance between adjacent points in the calibration
object. If a scalar is given, a square object is assumed, and the vertical and
horizontal distances are assumed to be identical. An array of shape (..., 2)
can be given: the last dimension is (spacing_h, spacing_w), and the preceding
dimensions are used for broadcasting
- calobject_warp: optional array of shape (2,) defaults to None. Describes the
warping of the calibration object. If None, the object is flat. If an array is
given, the values describe the maximum additive deflection along the x and y
axes. Extended array can be given for broadcasting
This function supports broadcasting across object_spacing and calobject_warp
RETURNED VALUES
The calibration object geometry in a (..., H,W,3) array, with the leading
dimensions set by the broadcasting rules
'''
# shape (H,W)
xx, yy = np.meshgrid(np.arange(W, dtype=float), np.arange(H, dtype=float))
# shape (H,W,3)
full_object = nps.glue(nps.mv(nps.cat(xx, yy), 0, -1),
np.zeros((H, W, 1)),
axis=-1)
# object_spacing has shape (..., 2)
object_spacing = np.array(object_spacing)
if object_spacing.ndim == 0:
object_spacing = np.array((1, 1)) * object_spacing
object_spacing = nps.dummy(object_spacing, -2, -2)
# object_spacing now has shape (..., 1,1,2)
if object_spacing.ndim > 3:
# extend full_object to the output shape I want
full_object = full_object * np.ones(object_spacing.shape[:-3] +
(1, 1, 1))
full_object[..., :2] *= object_spacing
if calobject_warp is not None:
xr = xx / (W - 1)
yr = yy / (H - 1)
dx = 4. * xr * (1. - xr)
dy = 4. * yr * (1. - yr)
# To allow broadcasting over calobject_warp
if calobject_warp.ndim > 1:
# shape (..., 1,1,2)
calobject_warp = nps.dummy(calobject_warp, -2, -2)
# extend full_object to the output shape I want
full_object = full_object * np.ones(calobject_warp.shape[:-3] +
(1, 1, 1))
full_object[..., 2] += calobject_warp[..., 0] * dx
full_object[..., 2] += calobject_warp[..., 1] * dy
return full_object
return 0
y = sample_cubic(x, c)
c2 = c.copy()
c2[int(N // 2)] *= 1.1
y2 = sample_cubic(x, c2)
if not skip_plots:
plot1 = gp.gnuplotlib(
title='Cubic splines: response to control point perturbation',
# hardcopy = '/tmp/cubic-spline-perturbations.svg',
# terminal = 'svg size 800,600 noenhanced solid dynamic font ",12"'
)
plot1.plot(
(x, nps.cat(y, y2),
dict(_with=np.array(('lines lc "blue"', 'lines lc "sea-green"')),
legend=np.array(
('Spline: baseline', 'Spline: tweaked one control point')))),
(t[:len(c)], nps.cat(c, c2),
dict(_with=np.array(('points pt 1 ps 1 lc "blue"',
'points pt 2 ps 1 lc "sea-green"')),
legend=np.array(('Control points: baseline',
'Control points: tweaked one control point')))),
(x, y - y2, dict(_with='lines lc "red"', legend='Difference', y2=1)),
_xrange=(10.5, 19.5),
y2max=0.01,
ylabel='Spline value',
y2label='Difference due to perturbation',
)
interp_rectification_map0x = \
scipy.interpolate.RectBivariateSpline(row, col,
nps.transpose(rectification_maps[0][...,0]))
interp_rectification_map0y = \
scipy.interpolate.RectBivariateSpline(row, col,
nps.transpose(rectification_maps[0][...,1]))
interp_rectification_map1x = \
scipy.interpolate.RectBivariateSpline(row, col,
nps.transpose(rectification_maps[1][...,0]))
interp_rectification_map1y = \
scipy.interpolate.RectBivariateSpline(row, col,
nps.transpose(rectification_maps[1][...,1]))
if lensmodel == 'LENSMODEL_LATLON':
qcam0_from_map = \
nps.transpose( nps.cat( interp_rectification_map0x(*nps.transpose(qrect0), grid=False),
interp_rectification_map0y(*nps.transpose(qrect0), grid=False) ) )
qcam1_from_map = \
nps.transpose( nps.cat( interp_rectification_map1x(*nps.transpose(qrect1), grid=False),
interp_rectification_map1y(*nps.transpose(qrect1), grid=False) ) )
else:
qcam0_from_map = \
nps.transpose( nps.cat( interp_rectification_map0x(*nps.transpose(qrect0), grid=False),
interp_rectification_map0y(*nps.transpose(qrect0), grid=False) ) )
qcam1_from_map = \
nps.transpose( nps.cat( interp_rectification_map1x(*nps.transpose(qrect1), grid=False),
interp_rectification_map1y(*nps.transpose(qrect1), grid=False) ) )
testutils.confirm_equal( qcam0_from_map, qcam0,
eps=1e-1,
msg=f'rectification map for camera 0 points ({lensmodel})')
def image_transformation_map(model_from,
model_to,
intrinsics_only=False,
distance=None,
plane_n=None,
plane_d=None,
mask_valid_intrinsics_region_from=False):
r'''Compute a reprojection map between two models
SYNOPSIS
model_orig = mrcal.cameramodel("xxx.cameramodel")
image_orig = cv2.imread("image.jpg")
model_pinhole = mrcal.pinhole_model_for_reprojection(model_orig,
fit = "corners")
mapxy = mrcal.image_transformation_map(model_orig, model_pinhole,
intrinsics_only = True)
image_undistorted = mrcal.transform_image(image_orig, mapxy)
# image_undistorted is now a pinhole-reprojected version of image_orig
Returns the transformation that describes a mapping
- from pixel coordinates of an image of a scene observed by model_to
- to pixel coordinates of an image of the same scene observed by model_from
This transformation can then be applied to a whole image by calling
mrcal.transform_image().
This function returns a transformation map in an (Nheight,Nwidth,2) array. The
image made by model_to will have shape (Nheight,Nwidth). Each pixel (x,y) in
this image corresponds to a pixel mapxy[y,x,:] in the image made by model_from.
One application of this function is to validate the models in a stereo pair. For
instance, reprojecting one camera's image at distance=infinity should produce
exactly the same image that is observed by the other camera when looking at very
far objects, IF the intrinsics and rotation are correct. If the images don't
line up well, we know that some part of the models is off. Similarly, we can use
big planes (such as observations of the ground) and plane_n, plane_d to validate.
This function has several modes of operation:
- intrinsics, extrinsics
Used if not intrinsics_only and \
plane_n is None and \
plane_d is None
This is the default. For each pixel in the output, we use the full model to
unproject a given distance out, and then use the full model to project back
into the other camera.
- intrinsics only
Used if intrinsics_only and \
plane_n is None and \
plane_d is None
Similar, but the extrinsics are ignored. We unproject the pixels in one model,
and project the into the other camera. The two camera coordinate systems are
assumed to line up perfectly
- plane
Used if plane_n is not None and
plane_d is not None
We map observations of a given plane in camera FROM coordinates coordinates to
where this plane would be observed by camera TO. We unproject each pixel in
one camera, compute the intersection point with the plane, and project that
intersection point back to the other camera. This uses ALL the intrinsics,
extrinsics and the plane representation. The plane is represented by a normal
vector plane_n, and the distance to the normal plane_d. The plane is all
points p such that inner(p,plane_n) = plane_d. plane_n does not need to be
normalized; any scaling is compensated in plane_d.
ARGUMENTS
- model_from: the mrcal.cameramodel object describing the camera used to capture
the input image. We always use the intrinsics. if not intrinsics_only: we use
the extrinsics also
- model_to: the mrcal.cameramodel object describing the camera that would have
captured the image we're producing. We always use the intrinsics. if not
intrinsics_only: we use the extrinsics also
- intrinsics_only: optional boolean, defaulting to False. If False: we respect
the relative transformation in the extrinsics of the camera models.
- distance: optional value, defaulting to None. Used only if not
intrinsics_only. We reproject points in space a given distance out. If
distance is None (the default), we look out to infinity. This is equivalent to
using only the rotation component of the extrinsics, ignoring the translation.
- plane_n: optional numpy array of shape (3,); None by default. If given, we
produce a transformation to map observations of a given plane to the same
pixels in the source and target images. This argument describes the normal
vector in the coordinate system of model_from. The plane is all points p such
that inner(p,plane_n) = plane_d. plane_n does not need to be normalized; any
scaling is compensated in plane_d. If given, plane_d should be given also, and
intrinsics_only should be False. if given, we use the full intrinsics and
extrinsics of both camera models
- plane_d: optional floating-point value; None by default. If given, we produce
a transformation to map observations of a given plane to the same pixels in
the source and target images. The plane is all points p such that
inner(p,plane_n) = plane_d. plane_n does not need to be normalized; any
scaling is compensated in plane_d. If given, plane_n should be given also, and
intrinsics_only should be False. if given, we use the full intrinsics and
extrinsics of both camera models
- mask_valid_intrinsics_region_from: optional boolean defaulting to False. If
True, points outside the valid-intrinsics region in the FROM image are set to
black, and thus do not appear in the output image
RETURNED VALUE
A numpy array of shape (Nheight,Nwidth,2) where Nheight and Nwidth represent the
imager dimensions of model_to. This array contains 32-bit floats, as required by
cv2.remap() (the function providing the internals of mrcal.transform_image()).
This array can be passed to mrcal.transform_image()
'''
if (plane_n is None and plane_d is not None) or \
(plane_n is not None and plane_d is None):
raise Exception(
"plane_n and plane_d should both be None or neither should be None"
)
if plane_n is not None and \
intrinsics_only:
raise Exception(
"We're looking at remapping a plane (plane_d, plane_n are not None), so intrinsics_only should be False"
)
if distance is not None and \
(plane_n is not None or intrinsics_only):
raise Exception(
"'distance' makes sense only without plane_n/plane_d and without intrinsics_only"
)
if intrinsics_only:
Rt_to_from = None
else:
Rt_to_from = mrcal.compose_Rt(model_to.extrinsics_Rt_fromref(),
model_from.extrinsics_Rt_toref())
lensmodel_from, intrinsics_data_from = model_from.intrinsics()
lensmodel_to, intrinsics_data_to = model_to.intrinsics()
if re.match("LENSMODEL_OPENCV",lensmodel_from) and \
lensmodel_to == "LENSMODEL_PINHOLE" and \
plane_n is None and \
not mask_valid_intrinsics_region_from and \
distance is None:
# This is a common special case. This branch works identically to the
# other path (the other side of this "if" can always be used instead),
# but the opencv-specific code is optimized and at one point ran faster
# than the code on the other side.
#
# The mask_valid_intrinsics_region_from isn't implemented in this path.
# It COULD be, then this faster path could be used
import cv2
fxy_from = intrinsics_data_from[0:2]
cxy_from = intrinsics_data_from[2:4]
cameraMatrix_from = np.array(
((fxy_from[0], 0, cxy_from[0]), (0, fxy_from[1], cxy_from[1]),
(0, 0, 1)))
fxy_to = intrinsics_data_to[0:2]
cxy_to = intrinsics_data_to[2:4]
cameraMatrix_to = np.array(
((fxy_to[0], 0, cxy_to[0]), (0, fxy_to[1], cxy_to[1]), (0, 0, 1)))
output_shape = model_to.imagersize()
distortion_coeffs = intrinsics_data_from[4:]
if Rt_to_from is not None:
R_to_from = Rt_to_from[:3, :]
if np.trace(R_to_from) > 3. - 1e-12:
R_to_from = None # identity, so I pass None
else:
R_to_from = None
return nps.glue( *[ nps.dummy(arr,-1) for arr in \
cv2.initUndistortRectifyMap(cameraMatrix_from, distortion_coeffs,
R_to_from,
cameraMatrix_to, tuple(output_shape),
cv2.CV_32FC1)],
axis = -1)
W_from, H_from = model_from.imagersize()
W_to, H_to = model_to.imagersize()
# shape: (Nheight,Nwidth,2). Contains (x,y) rows
grid = np.ascontiguousarray(nps.mv(
nps.cat(*np.meshgrid(np.arange(W_to), np.arange(H_to))), 0, -1),
dtype=float)
v = mrcal.unproject(grid, lensmodel_to, intrinsics_data_to)
if plane_n is not None:
R_to_from = Rt_to_from[:3, :]
t_to_from = Rt_to_from[3, :]
# The homography definition. Derived in many places. For instance in
# "Motion and structure from motion in a piecewise planar environment"
# by Olivier Faugeras, F. Lustman.
A_to_from = plane_d * R_to_from + nps.outer(t_to_from, plane_n)
A_from_to = np.linalg.inv(A_to_from)
v = nps.matmult(v, nps.transpose(A_from_to))
else:
if Rt_to_from is not None:
if distance is not None:
v = mrcal.transform_point_Rt(
mrcal.invert_Rt(Rt_to_from),
v / nps.dummy(nps.mag(v), -1) * distance)
else:
R_to_from = Rt_to_from[:3, :]
v = nps.matmult(v, R_to_from)
mapxy = mrcal.project(v, lensmodel_from, intrinsics_data_from)
if mask_valid_intrinsics_region_from:
# Using matplotlib to compute the out-of-bounds points. It doesn't
# support broadcasting, so I do that manually with a clump/reshape
from matplotlib.path import Path
region = Path(model_from.valid_intrinsics_region())
is_inside = region.contains_points(nps.clump(mapxy, n=2)).reshape(
mapxy.shape[:2])
mapxy[~is_inside, :] = -1
return mapxy.astype(np.float32)
**plot_options)
elif args.ranges:
# Plot the range distribution
range_ref = nps.mag(p)
if args.title is not None:
title = args.title
else:
title = "Range distribution"
if args.extratitle is not None:
title += ': ' + args.extratitle
gp.plot( nps.cat( range_sampled_geometric,
range_sampled_lindstrom,
range_sampled_leecivera_l1,
range_sampled_leecivera_linf,
range_sampled_leecivera_mid2,
range_sampled_leecivera_wmid2 ),
legend = np.array(( 'range_sampled_geometric',
'range_sampled_lindstrom',
'range_sampled_leecivera_l1',
'range_sampled_leecivera_linf',
'range_sampled_leecivera_mid2',
'range_sampled_leecivera_wmid2' )),
histogram=True,
binwidth=200,
_with='lines',
_set = f'arrow from {range_ref},graph 0 to {range_ref},graph 1 nohead lw 5',
wait = 'hardcopy' not in plot_options,
title = title,
**plot_options)
m.intrinsics()[1][:8]))
elif args.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[:args.Ncameras]
lensmodel = models_true[0].intrinsics()[0]
Nintrinsics = mrcal.lensmodel_num_params(lensmodel)
imagersizes = nps.cat(*[m.imagersize() for m in models_true])
extrinsics_rt_fromref_toset = \
np.array(((0, 0, 0, 0, 0, 0),
(0.08, 0.2, 0.02, 1., 0.9, 0.1),
(0.01, 0.07, 0.2, 2.1, 0.4, 0.2),
(-0.1, 0.08, 0.08, 3.4, 0.2, 0.1), ))
for i in range(args.Ncameras):
models_true[i].extrinsics_rt_fromref(extrinsics_rt_fromref_toset[i])
pixel_uncertainty_stdev = 1.5
object_spacing = 0.1
object_width_n = 10
object_height_n = 9
def _read(s, name):
r'''Reads a .cahvor file into a cameramodel
The input is the .cahvor file contents as a string'''
re_f = '[-+]?(?:\d+(?:\.\d*)?|\.\d+)(?:[eE][-+]?\d+)?'
re_u = '\d+'
re_d = '[-+]?\d+'
re_s = '.+'
# I parse all key=value lines into my dict as raw text. Further I
# post-process some of these raw lines.
x = {}
for l in s.splitlines():
if re.match('^\s*#|^\s*$', l):
continue
m = re.match('\s*(\w+)\s*=\s*(.+?)\s*\n?$',
l, flags=re.I)
if m:
key = m.group(1)
if key in x:
raise Exception("Reading '{}': key '{}' seen more than once".format(name,
m.group(1)))
value = m.group(2)
# for compatibility
if re.match('^DISTORTION', key):
key = key.replace('DISTORTION', 'LENSMODEL')
x[key] = value
# Done reading. Any values that look like numbers, I convert to numbers.
for i in x:
if re.match('{}$'.format(re_f), x[i]):
x[i] = float(x[i])
# I parse the fields I know I care about into numpy arrays
for i in ('Dimensions','C','A','H','V','O','R','E',
'LENSMODEL_OPENCV4',
'LENSMODEL_OPENCV5',
'LENSMODEL_OPENCV8',
'LENSMODEL_OPENCV12',
'VALID_INTRINSICS_REGION'):
if i in x:
# Any data that's composed only of digits and whitespaces (no "."),
# use integers
if re.match('[0-9\s]+$', x[i]): totype = int
else: totype = float
x[i] = np.array( [ totype(v) for v in re.split('\s+', x[i])], dtype=totype)
# Now I sanity-check the results and call it done
for k in ('Dimensions','C','A','H','V'):
if not k in x:
raise Exception("Cahvor file '{}' incomplete. Missing values for: {}".
format(name, k))
is_cahvor_or_cahvore = False
if 'LENSMODEL_OPENCV12' in x:
distortions = x["LENSMODEL_OPENCV12"]
lensmodel = 'LENSMODEL_OPENCV12'
elif 'LENSMODEL_OPENCV8' in x:
distortions = x["LENSMODEL_OPENCV8"]
lensmodel = 'LENSMODEL_OPENCV8'
elif 'LENSMODEL_OPENCV5' in x:
distortions = x["LENSMODEL_OPENCV5"]
lensmodel = 'LENSMODEL_OPENCV5'
elif 'LENSMODEL_OPENCV4' in x:
distortions = x["LENSMODEL_OPENCV4"]
lensmodel = 'LENSMODEL_OPENCV4'
elif 'R' not in x:
distortions = np.array(())
lensmodel = 'LENSMODEL_PINHOLE'
else:
is_cahvor_or_cahvore = True
if 'VALID_INTRINSICS_REGION' in x:
x['VALID_INTRINSICS_REGION'] = \
x['VALID_INTRINSICS_REGION'].reshape( len(x['VALID_INTRINSICS_REGION'])//2, 2)
# get extrinsics from cahvor
if 'Model' not in x:
x['Model'] = ''
m = re.match('CAHVORE3,([0-9\.e-]+)\s*=\s*general',x['Model'])
if m:
is_cahvore = True
cahvore_linearity = float(m.group(1))
else:
is_cahvore = False
Hp,Vp = _HVs_HVc_HVp(x)[-2:]
R_toref = nps.transpose( nps.cat( Hp,
Vp,
x['A'] ))
t_toref = x['C']
if is_cahvor_or_cahvore:
if 'O' not in x:
alpha = 0
beta = 0
else:
o = nps.matmult( x['O'], R_toref )
alpha = np.arctan2(o[0], o[2])
beta = np.arcsin( o[1] )
if is_cahvore:
# CAHVORE
if 'E' not in x:
raise Exception('Cahvor file {} LOOKS like a cahvore, but lacks the E'.format(name))
R0,R1,R2 = x['R'].ravel()
E0,E1,E2 = x['E'].ravel()
distortions = np.array((alpha,beta,R0,R1,R2,E0,E1,E2), dtype=float)
lensmodel = f'LENSMODEL_CAHVORE_linearity={cahvore_linearity}'
else:
# CAHVOR
if 'E' in x:
raise Exception('Cahvor file {} LOOKS like a cahvor, but has an E'.format(name))
if abs(beta) < 1e-8 and \
( 'R' not in x or np.linalg.norm(x['R']) < 1e-8):
# pinhole
alpha = 0
beta = 0
else:
R0,R1,R2 = x['R'].ravel()
if alpha == 0 and beta == 0:
distortions = np.array(())
lensmodel = 'LENSMODEL_PINHOLE'
else:
distortions = np.array((alpha,beta,R0,R1,R2), dtype=float)
lensmodel = 'LENSMODEL_CAHVOR'
m = mrcal.cameramodel(imagersize = x['Dimensions'].astype(np.int32),
intrinsics = (lensmodel, nps.glue( np.array(_fxy_cxy(x), dtype=float),
distortions,
axis = -1)),
valid_intrinsics_region = x.get('VALID_INTRINSICS_REGION'),
extrinsics_Rt_toref = np.ascontiguousarray(nps.glue(R_toref,t_toref, axis=-2)))
return m
def reproject_perturbed__mean_frames(
q,
distance,
# shape (Ncameras, Nintrinsics)
baseline_intrinsics,
# shape (Ncameras, 6)
baseline_rt_cam_ref,
# shape (Nframes, 6)
baseline_rt_ref_frame,
# shape (2)
baseline_calobject_warp,
# shape (..., Ncameras, Nintrinsics)
query_intrinsics,
# shape (..., Ncameras, 6)
query_rt_cam_ref,
# shape (..., Nframes, 6)
query_rt_ref_frame,
# shape (..., 2)
query_calobject_warp):
r'''Reproject by computing the mean in the space of frames
This is what the uncertainty computation does (as of 2020/10/26). The implied
rotation here is aphysical (it is a mean of multiple rotation matrices)
'''
# shape (Ncameras, 3)
p_cam_baseline = mrcal.unproject(
q, lensmodel, baseline_intrinsics, normalize=True) * distance
# shape (Ncameras, 3)
p_ref_baseline = \
mrcal.transform_point_rt( mrcal.invert_rt(baseline_rt_cam_ref),
p_cam_baseline )
if fixedframes:
p_ref_query = p_ref_baseline
else:
# shape (Nframes, Ncameras, 3)
# The point in the coord system of all the frames
p_frames = mrcal.transform_point_rt( \
nps.dummy(mrcal.invert_rt(baseline_rt_ref_frame),-2),
p_ref_baseline)
# shape (..., Nframes, Ncameras, 3)
p_ref_query_allframes = \
mrcal.transform_point_rt( nps.dummy(query_rt_ref_frame, -2),
p_frames )
if args.reproject_perturbed == 'mean-frames':
# "Normal" path: I take the mean of all the frame-coord-system
# representations of my point
if not fixedframes:
# shape (..., Ncameras, 3)
p_ref_query = np.mean(p_ref_query_allframes, axis=-3)
# shape (..., Ncameras, 3)
p_cam_query = \
mrcal.transform_point_rt(query_rt_cam_ref, p_ref_query)
# shape (..., Ncameras, 2)
return mrcal.project(p_cam_query, lensmodel, query_intrinsics)
else:
# Experimental path: I take the mean of the projections, not the points
# in the reference frame
# guaranteed that not fixedframes: I asserted this above
# shape (..., Nframes, Ncameras, 3)
p_cam_query_allframes = \
mrcal.transform_point_rt(nps.dummy(query_rt_cam_ref, -3),
p_ref_query_allframes)
# shape (..., Nframes, Ncameras, 2)
q_reprojected = mrcal.project(p_cam_query_allframes, lensmodel,
nps.dummy(query_intrinsics, -3))
if args.reproject_perturbed != 'mean-frames-using-meanq-penalize-big-shifts':
return np.mean(q_reprojected, axis=-3)
else:
# Experiment. Weighted mean to de-emphasize points with huge shifts
w = 1. / nps.mag(q_reprojected - q)
w = nps.mv(nps.cat(w, w), 0, -1)
return \
np.sum(q_reprojected*w, axis=-3) / \
np.sum(w, axis=-3)
def reproject_perturbed__fit_boards_ref(
q,
distance,
# shape (Ncameras, Nintrinsics)
baseline_intrinsics,
# shape (Ncameras, 6)
baseline_rt_cam_ref,
# shape (Nframes, 6)
baseline_rt_ref_frame,
# shape (2)
baseline_calobject_warp,
# shape (..., Ncameras, Nintrinsics)
query_intrinsics,
# shape (..., Ncameras, 6)
query_rt_cam_ref,
# shape (..., Nframes, 6)
query_rt_ref_frame,
# shape (..., 2)
query_calobject_warp):
r'''Reproject by explicitly computing a procrustes fit to align the reference
coordinate systems of the two solves. We match up the two sets of chessboard
points
'''
calobject_height, calobject_width = optimization_inputs_baseline[
'observations_board'].shape[1:3]
# shape (Nsamples, Nh, Nw, 3)
if query_calobject_warp.ndim > 1:
calibration_object_query = \
nps.cat(*[ mrcal.ref_calibration_object(calobject_width, calobject_height,
optimization_inputs_baseline['calibration_object_spacing'],
calobject_warp=calobject_warp) \
for calobject_warp in query_calobject_warp] )
else:
calibration_object_query = \
mrcal.ref_calibration_object(calobject_width, calobject_height,
optimization_inputs_baseline['calibration_object_spacing'],
calobject_warp=query_calobject_warp)
# shape (Nsamples, Nframes, Nh, Nw, 3)
pcorners_ref_query = \
mrcal.transform_point_rt( nps.dummy(query_rt_ref_frame, -2, -2),
nps.dummy(calibration_object_query, -4))
# shape (Nh, Nw, 3)
calibration_object_baseline = \
mrcal.ref_calibration_object(calobject_width, calobject_height,
optimization_inputs_baseline['calibration_object_spacing'],
calobject_warp=baseline_calobject_warp)
# frames_ref.shape is (Nframes, 6)
# shape (Nframes, Nh, Nw, 3)
pcorners_ref_baseline = \
mrcal.transform_point_rt( nps.dummy(baseline_rt_ref_frame, -2, -2),
calibration_object_baseline)
# shape (Nsamples,4,3)
Rt_refq_refb = \
mrcal.align_procrustes_points_Rt01( \
# shape (Nsamples,N,3)
nps.mv(nps.clump(nps.mv(pcorners_ref_query, -1,0),n=-3),0,-1),
# shape (N,3)
nps.clump(pcorners_ref_baseline, n=3))
# shape (Ncameras, 3)
p_cam_baseline = mrcal.unproject(
q, lensmodel, baseline_intrinsics, normalize=True) * distance
# shape (Ncameras, 3). In the ref coord system
p_ref_baseline = \
mrcal.transform_point_rt( mrcal.invert_rt(baseline_rt_cam_ref),
p_cam_baseline )
# shape (Nsamples,Ncameras,3)
p_ref_query = \
mrcal.transform_point_Rt(nps.mv(Rt_refq_refb,-3,-4),
p_ref_baseline)
# shape (..., Ncameras, 3)
p_cam_query = \
mrcal.transform_point_rt(query_rt_cam_ref, p_ref_query)
# shape (..., Ncameras, 2)
q1 = mrcal.project(p_cam_query, lensmodel, query_intrinsics)
if q1.shape[-3] == 1: q1 = q1[0, :, :]
return q1
def check_uncertainties_at(q0_baseline, idistance):
distance = args.distances[idistance]
# distance of "None" means I'll simulate a large distance, but compare
# against a special-case distance of "infinity"
if distance is None:
distance = 1e5
atinfinity = True
distancestr = "infinity"
else:
atinfinity = False
distancestr = str(distance)
# shape (Ncameras,3)
p_cam_baseline = mrcal.unproject(
q0_baseline, lensmodel, intrinsics_baseline, normalize=True) * distance
# shape (Nsamples, Ncameras, 2)
q_sampled = \
reproject_perturbed(q0_baseline,
distance,
intrinsics_baseline,
extrinsics_baseline_mounted,
frames_baseline,
calobject_warp_baseline,
intrinsics_sampled,
extrinsics_sampled_mounted,
frames_sampled,
calobject_warp_sampled)
# shape (Ncameras, 2)
q_sampled_mean = np.mean(q_sampled, axis=-3)
# shape (Ncameras, 2,2)
Var_dq_observed = np.mean(nps.outer(q_sampled - q_sampled_mean,
q_sampled - q_sampled_mean),
axis=-4)
# shape (Ncameras)
worst_direction_stdev_observed = mrcal.worst_direction_stdev(
Var_dq_observed)
# shape (Ncameras, 2,2)
Var_dq = \
nps.cat(*[ mrcal.projection_uncertainty( \
p_cam_baseline[icam],
atinfinity = atinfinity,
model = models_baseline[icam]) \
for icam in range(args.Ncameras) ])
# shape (Ncameras)
worst_direction_stdev_predicted = mrcal.worst_direction_stdev(Var_dq)
# q_sampled should be evenly distributed around q0_baseline. I can make eps
# as tight as I want by increasing Nsamples
testutils.confirm_equal(
nps.mag(q_sampled_mean - q0_baseline),
0,
eps=0.3,
worstcase=True,
msg=
f"Sampled projections cluster around the sample point at distance = {distancestr}"
)
# I accept 20% error. This is plenty good-enough. And I can get tighter matches
# if I grab more samples
testutils.confirm_equal(
worst_direction_stdev_observed,
worst_direction_stdev_predicted,
eps=0.2,
worstcase=True,
relative=True,
msg=
f"Predicted worst-case projections match sampled observations at distance = {distancestr}"
)
# I now compare the variances. The cross terms have lots of apparent error,
# but it's more meaningful to compare the eigenvectors and eigenvalues, so I
# just do that
# First, the thing is symmetric, right?
testutils.confirm_equal(
nps.transpose(Var_dq),
Var_dq,
worstcase=True,
msg=f"Var(dq) is symmetric at distance = {distancestr}")
for icam in range(args.Ncameras):
l_predicted, v = sorted_eig(Var_dq[icam])
v0_predicted = v[:, 0]
l_observed, v = sorted_eig(Var_dq_observed[icam])
v0_observed = v[:, 0]
testutils.confirm_equal(
l_observed,
l_predicted,
eps=0.35, # high error tolerance. Nsamples is too low for better
worstcase=True,
relative=True,
msg=
f"Var(dq) eigenvalues match for camera {icam} at distance = {distancestr}"
)
if icam == 3:
# I only check the eigenvectors for camera 3. The other cameras have
# isotropic covariances, so the eigenvectors aren't well defined. If
# one isn't isotropic for some reason, the eigenvalue check will
# fail
testutils.confirm_equal(
np.arcsin(nps.mag(np.cross(v0_observed, v0_predicted))) *
180. / np.pi,
0,
eps=15, # high error tolerance. Nsamples is too low for better
worstcase=True,
msg=
f"Var(dq) eigenvectors match for camera {icam} at distance = {distancestr}"
)
# I don't bother checking v1. I already made sure the matrix is
# symmetric. Thus the eigenvectors are orthogonal, so any angle offset
# in v0 will be exactly the same in v1
return q_sampled, Var_dq
imagersizes, intrinsics_true, extrinsics_true_mounted, frames_true,
observations_true, intrinsics_sampled, extrinsics_sampled_mounted,
frames_sampled, calobject_warp_sampled) = pickle.load(f)
baseline_rt_ref_frame = optimization_inputs_baseline['frames_rt_toref']
icam0, icam1 = args.cameras
Rt01_true = mrcal.compose_Rt(
mrcal.Rt_from_rt(extrinsics_rt_fromref_true[icam0]),
mrcal.invert_Rt(mrcal.Rt_from_rt(extrinsics_rt_fromref_true[icam1])))
Rt10_true = mrcal.invert_Rt(Rt01_true)
# shape (Npoints,Ncameras,3)
p_triangulated_true_local = nps.xchg(
nps.cat(p_triangulated_true0,
mrcal.transform_point_Rt(Rt10_true, p_triangulated_true0)), 0, 1)
# Pixel coords at the perfect intersection
# shape (Npoints,Ncameras,2)
q_true = nps.xchg( np.array([ mrcal.project(p_triangulated_true_local[:,i,:],
lensmodel,
intrinsics_true[args.cameras[i]]) \
for i in range(2)]),
0,1)
# Sanity check. Without noise, the triangulation should report the test point exactly
p_triangulated0 = \
triangulate_nograd(models_true[icam0].intrinsics()[1],
models_true[icam1].intrinsics()[1],
models_true[icam0].extrinsics_rt_fromref(),
models_true[icam0].extrinsics_rt_fromref(),
return np.array((1 + k[0] * r2 + k[1] * r4 + k[4] * r6,
1 + k[5] * r2 + k[6] * r4 + k[7] * r6))
try:
m = mrcal.cameramodel(args.model)
except:
print(f"Couldn't read '{args.model}' as a camera model", file=sys.stderr)
sys.exit(1)
W, H = m.imagersize()
Nw = 40
Nh = 30
# shape (Nh,Nw,2)
xy = \
nps.mv(nps.cat(*np.meshgrid( np.linspace(0,W-1,Nw),
np.linspace(0,H-1,Nh) )),
0,-1)
fxy = m.intrinsics()[1][0:2]
cxy = m.intrinsics()[1][2:4]
# shape (Nh,Nw,2)
v = mrcal.unproject(np.ascontiguousarray(xy), *m.intrinsics())
v0 = mrcal.unproject(cxy, *m.intrinsics())
# shape (Nh,Nw)
costh = nps.inner(v, v0) / (nps.mag(v) * nps.mag(v0))
th = np.arccos(costh)
# shape (Nh,Nw,2)
xy_rel = xy - cxy
# shape (Nh,Nw)
