def _noisy_observation_vectors_for_triangulation(p, Rt01, intrinsics0,
intrinsics1, Nsamples, sigma):
# p has shape (...,3)
# shape (..., 2)
q0 = mrcal.project(p, *intrinsics0)
q1 = mrcal.project(mrcal.transform_point_Rt(mrcal.invert_Rt(Rt01), p),
*intrinsics1)
# shape (..., 1,2). Each has x,y
q0 = nps.dummy(q0, -2)
q1 = nps.dummy(q1, -2)
q_noise = np.random.randn(*p.shape[:-1], Nsamples, 2, 2) * sigma
# shape (..., Nsamples,2). Each has x,y
q0_noise = q_noise[..., :, 0, :]
q1_noise = q_noise[..., :, 1, :]
q0_noisy = q0 + q0_noise
q1_noisy = q1 + q1_noise
# shape (..., Nsamples, 3)
v0local_noisy = mrcal.unproject(q0_noisy, *intrinsics0)
v1local_noisy = mrcal.unproject(q1_noisy, *intrinsics1)
v0_noisy = v0local_noisy
v1_noisy = mrcal.rotate_point_R(Rt01[:3, :], v1local_noisy)
# All have shape (..., Nsamples,3)
return \
v0local_noisy, v1local_noisy, v0_noisy,v1_noisy, \
q0,q1, q0_noisy, q1_noisy
def callback_l2_reprojection(p, v0local, v1local, Rt01):
dq0 = \
mrcal.project(p, *model0.intrinsics()) - \
mrcal.project(v0local, *model0.intrinsics())
dq1 = \
mrcal.project(mrcal.transform_point_Rt(mrcal.invert_Rt(Rt01),p),
*model1.intrinsics()) - \
mrcal.project(v1local, *model1.intrinsics())
return nps.norm2(dq0) + nps.norm2(dq1)
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 reproject_perturbed__diff(
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 using the "diff" method to compute a rotation
'''
# shape (Ncameras, 3)
p_cam_baseline = mrcal.unproject(
q, lensmodel, baseline_intrinsics, normalize=True) * distance
p_cam_query = np.zeros((args.Ncameras, 3), dtype=float)
for icam in range(args.Ncameras):
# This method only cares about the intrinsics
model_baseline = \
mrcal.cameramodel( intrinsics = (lensmodel, baseline_intrinsics[icam]),
imagersize = imagersizes[0] )
model_query = \
mrcal.cameramodel( intrinsics = (lensmodel, query_intrinsics[icam]),
imagersize = imagersizes[0] )
implied_Rt10_query = \
mrcal.projection_diff( (model_baseline,
model_query),
distance = distance,
use_uncertainties = False,
focus_center = None,
focus_radius = 1000.)[3]
mrcal.transform_point_Rt(implied_Rt10_query,
p_cam_baseline[icam],
out=p_cam_query[icam])
# shape (Ncameras, 2)
return \
mrcal.project( p_cam_query,
lensmodel, query_intrinsics)
def check(intrinsics, p_ref, q_ref, unproject=True):
q_projected = mrcal.project(p_ref, *intrinsics)
testutils.confirm_equal(q_projected,
q_ref,
msg=f"Projecting {intrinsics[0]}",
eps=1e-2)
if not unproject:
return
v_unprojected = mrcal.unproject(q_projected, *intrinsics, normalize=True)
testutils.confirm_equal(nps.norm2(v_unprojected),
np.ones((p_ref.shape[0], ), dtype=float),
msg=f"Unprojected v are normalized",
eps=1e-6)
cos = nps.inner(v_unprojected, p_ref) / nps.mag(p_ref)
cos = np.clip(cos, -1, 1)
testutils.confirm_equal(np.arccos(cos),
np.zeros((p_ref.shape[0], ), dtype=float),
msg=f"Unprojecting {intrinsics[0]}",
eps=1e-6)
def fit_check(scale_focal, intrinsics, v,
W = W,
H = H,
scale_imagersize_pinhole = 1.0,
eps = 1e-2):
r'''Makes sure projected vectors fit into the imager perfectly
I'm given a number of points in the camera coords. These much project such
that
- All projected points lie INSIDE the imager
- At least one point lies exactly on the imager boundary
'''
intrinsics = intrinsics.copy()
intrinsics[:2] *= scale_focal
intrinsics *= scale_imagersize_pinhole
q = mrcal.project(v, 'LENSMODEL_PINHOLE', intrinsics)
if any( q[:,0] < -eps ) or \
any( q[:,0] > W-1+eps ) or \
any( q[:,1] < -eps ) or \
any( q[:,1] > H-1+eps ):
return ": Some points lie out of bounds"
on_left_edge = np.abs(q[:,0] ) < eps
on_right_edge = np.abs(q[:,0] - (W-1)) < eps
on_top_edge = np.abs(q[:,1] ) < eps
on_bottom_edge = np.abs(q[:,1] - (H-1)) < eps
if not ( \
any(on_left_edge) or \
any(on_top_edge) or \
any(on_right_edge) or \
any(on_bottom_edge) ):
return ": No points lie on the edge"
# all good
return ''
def projection_diff(models_ref, max_dist_from_center, fit_implied_Rt=True):
lensmodels = [model.intrinsics()[0] for model in models_ref]
intrinsics_data = [model.intrinsics()[1] for model in models_ref]
# v shape (...,Ncameras,Nheight,Nwidth,...)
# q0 shape (..., Nheight,Nwidth,...)
v,q0 = \
mrcal.sample_imager_unproject(Nw,None,
*imagersizes[0],
lensmodels, intrinsics_data,
normalize = True)
W, H = imagersizes[0]
focus_center = None
focus_radius = -1 if fit_implied_Rt else 0
if focus_center is None: focus_center = ((W - 1.) / 2., (H - 1.) / 2.)
if focus_radius < 0: focus_radius = min(W, H) / 6.
implied_Rt10 = \
mrcal.implied_Rt10__from_unprojections(q0,
v[0,...], v[1,...],
focus_center = focus_center,
focus_radius = focus_radius)
q1 = mrcal.project(mrcal.transform_point_Rt(implied_Rt10, v[0, ...]),
lensmodels[1], intrinsics_data[1])
diff = nps.mag(q1 - q0)
# zero-out everything too far from the center
center = (imagersizes[0] - 1.) / 2.
diff[nps.norm2(q0 - center) > max_dist_from_center *
max_dist_from_center] = 0
# gp.plot(diff,
# ascii = True,
# using = mrcal.imagergrid_using(imagersizes[0], Nw),
# square=1, _with='image', tuplesize=3, hardcopy='/tmp/yes.gp', cbmax=3)
return diff
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.)
def check(intrinsics, p_ref, q_ref):
########## project
q_projected = mrcal.project(p_ref, *intrinsics)
testutils.confirm_equal(q_projected,
q_ref,
msg = f"Projecting {intrinsics[0]}",
eps = 1e-2)
q_projected *= 0
mrcal.project(p_ref, *intrinsics,
out = q_projected)
testutils.confirm_equal(q_projected,
q_ref,
msg = f"Projecting {intrinsics[0]} in-place",
eps = 1e-2)
meta = mrcal.lensmodel_metadata_and_config(intrinsics[0])
if meta['has_gradients']:
@nps.broadcast_define( ((3,),('N',)) )
def grad_broadcasted(p_ref, i_ref):
return grad(lambda pi: mrcal.project(pi[:3], intrinsics[0], pi[3:]),
nps.glue(p_ref,i_ref, axis=-1))
dq_dpi_ref = grad_broadcasted(p_ref,intrinsics[1])
q_projected,dq_dp,dq_di = mrcal.project(p_ref, *intrinsics, get_gradients=True)
testutils.confirm_equal(q_projected,
q_ref,
msg = f"Projecting {intrinsics[0]} with grad",
eps = 1e-2)
testutils.confirm_equal(dq_dp,
dq_dpi_ref[...,:3],
msg = f"dq_dp {intrinsics[0]}",
eps = 1e-2)
testutils.confirm_equal(dq_di,
dq_dpi_ref[...,3:],
msg = f"dq_di {intrinsics[0]}",
eps = 1e-2)
out=[q_projected,dq_dp,dq_di]
out[0] *= 0
out[1] *= 0
out[2] *= 0
mrcal.project(p_ref, *intrinsics, get_gradients=True, out=out)
testutils.confirm_equal(q_projected,
q_ref,
msg = f"Projecting {intrinsics[0]} with grad in-place",
eps = 1e-2)
testutils.confirm_equal(dq_dp,
dq_dpi_ref[...,:3],
msg = f"dq_dp in-place",
eps = 1e-2)
testutils.confirm_equal(dq_di,
dq_dpi_ref[...,3:],
msg = f"dq_di in-place",
eps = 1e-2)
########## unproject
if 1:
##### Un-normalized
v_unprojected = mrcal.unproject(q_projected, *intrinsics,
normalize = False)
cos = nps.inner(v_unprojected, p_ref) / nps.mag(p_ref)
cos = np.clip(cos, -1, 1)
testutils.confirm_equal( np.arccos(cos),
np.zeros((p_ref.shape[0],), dtype=float),
msg = f"Unprojecting {intrinsics[0]}",
eps = 1e-6)
if 1:
##### Normalized
v_unprojected_nograd = mrcal.unproject(q_projected, *intrinsics,
normalize = True)
testutils.confirm_equal( nps.norm2(v_unprojected_nograd),
1,
msg = f"Unprojected v are normalized",
eps = 1e-6)
cos = nps.inner(v_unprojected_nograd, p_ref) / nps.mag(p_ref)
cos = np.clip(cos, -1, 1)
testutils.confirm_equal( np.arccos(cos),
np.zeros((p_ref.shape[0],), dtype=float),
msg = f"Unprojecting {intrinsics[0]} (normalized)",
eps = 1e-6)
if not meta['has_gradients']:
# no in-place output for the no-gradients unproject() path
return
v_unprojected *= 0
mrcal.unproject(q_projected, *intrinsics,
normalize = True,
out = v_unprojected)
testutils.confirm_equal( nps.norm2(v_unprojected),
1,
msg = f"Unprojected in-place v are normalized",
eps = 1e-6)
cos = nps.inner(v_unprojected, p_ref) / nps.mag(p_ref)
cos = np.clip(cos, -1, 1)
testutils.confirm_equal( np.arccos(cos),
np.zeros((p_ref.shape[0],), dtype=float),
msg = f"Unprojecting in-place {intrinsics[0]}",
eps = 1e-6)
### unproject gradients
v_unprojected,dv_dq,dv_di = mrcal.unproject(q_projected,
*intrinsics, get_gradients=True)
# I'd like to turn this on, but unproject() doesn't behave the way it
# should, so this test always fails currently
#
# testutils.confirm_equal( v_unprojected,
# v_unprojected_nograd,
# msg = f"Unproject() should return the same thing whether get_gradients or not",
# eps = 1e-6)
# Two different gradient computations, to match the two different ways the
# internal computation is performed
if intrinsics[0] == 'LENSMODEL_PINHOLE' or \
intrinsics[0] == 'LENSMODEL_STEREOGRAPHIC' or \
intrinsics[0] == 'LENSMODEL_LATLON' or \
intrinsics[0] == 'LENSMODEL_LONLAT':
@nps.broadcast_define( ((2,),('N',)) )
def grad_broadcasted(q_ref, i_ref):
return grad(lambda qi: mrcal.unproject(qi[:2], intrinsics[0], qi[2:]),
nps.glue(q_ref,i_ref, axis=-1))
dv_dqi_ref = grad_broadcasted(q_projected,intrinsics[1])
else:
@nps.broadcast_define( ((2,),('N',)) )
def grad_broadcasted(q_ref, i_ref):
return grad(lambda qi: \
mrcal.unproject_stereographic( \
mrcal.project_stereographic(
mrcal.unproject(qi[:2], intrinsics[0], qi[2:]))),
nps.glue(q_ref,i_ref, axis=-1))
dv_dqi_ref = grad_broadcasted(q_projected,intrinsics[1])
testutils.confirm_equal(mrcal.project(v_unprojected, *intrinsics),
q_projected,
msg = f"Unprojecting {intrinsics[0]} with grad",
eps = 1e-2)
testutils.confirm_equal(dv_dq,
dv_dqi_ref[...,:2],
msg = f"dv_dq: {intrinsics[0]}",
worstcase = True,
relative = True,
eps = 0.01)
testutils.confirm_equal(dv_di,
dv_dqi_ref[...,2:],
msg = f"dv_di {intrinsics[0]}",
worstcase = True,
relative = True,
eps = 0.01)
# Normalized unprojected gradients
v_unprojected,dv_dq,dv_di = mrcal.unproject(q_projected,
*intrinsics,
normalize = True,
get_gradients = True)
testutils.confirm_equal( nps.norm2(v_unprojected),
1,
msg = f"Unprojected v (with gradients) are normalized",
eps = 1e-6)
cos = nps.inner(v_unprojected, p_ref) / nps.mag(p_ref)
cos = np.clip(cos, -1, 1)
testutils.confirm_equal( np.arccos(cos),
np.zeros((p_ref.shape[0],), dtype=float),
msg = f"Unprojecting (normalized, with gradients) {intrinsics[0]}",
eps = 1e-6)
@nps.broadcast_define( ((2,),('N',)) )
def grad_normalized_broadcasted(q_ref, i_ref):
return grad(lambda qi: \
mrcal.unproject(qi[:2], intrinsics[0], qi[2:], normalize=True),
nps.glue(q_ref,i_ref, axis=-1))
dvnormalized_dqi_ref = grad_normalized_broadcasted(q_projected,intrinsics[1])
testutils.confirm_equal(dv_dq,
dvnormalized_dqi_ref[...,:2],
msg = f"dv_dq (normalized v): {intrinsics[0]}",
worstcase = True,
relative = True,
eps = 0.01)
testutils.confirm_equal(dv_di,
dvnormalized_dqi_ref[...,2:],
msg = f"dv_di (normalized v): {intrinsics[0]}",
worstcase = True,
relative = True,
eps = 0.01)
# unproject() with gradients, in-place
if 1:
# Normalized output
out=[v_unprojected,dv_dq,dv_di]
out[0] *= 0
out[1] *= 0
out[2] *= 0
mrcal.unproject(q_projected,
*intrinsics,
normalize = True,
get_gradients = True,
out = out)
testutils.confirm_equal( nps.norm2(v_unprojected),
1,
msg = f"Unprojected v (with gradients, in-place) are normalized",
eps = 1e-6)
cos = nps.inner(v_unprojected, p_ref) / nps.mag(p_ref)
cos = np.clip(cos, -1, 1)
testutils.confirm_equal( np.arccos(cos),
np.zeros((p_ref.shape[0],), dtype=float),
msg = f"Unprojecting (normalized, with gradients, in-place) {intrinsics[0]}",
eps = 1e-6)
testutils.confirm_equal(dv_dq,
dvnormalized_dqi_ref[...,:2],
msg = f"dv_dq (normalized v, in-place): {intrinsics[0]}",
worstcase = True,
relative = True,
eps = 0.01)
testutils.confirm_equal(dv_di,
dvnormalized_dqi_ref[...,2:],
msg = f"dv_di (normalized v, in-place): {intrinsics[0]}",
worstcase = True,
relative = True,
eps = 0.01)
if 1:
# un-normalized output
out=[v_unprojected,dv_dq,dv_di]
out[0] *= 0
out[1] *= 0
out[2] *= 0
mrcal.unproject(q_projected,
*intrinsics,
normalize = False,
get_gradients = True,
out = out)
cos = nps.inner(v_unprojected, p_ref) / nps.mag(p_ref)
cos = np.clip(cos, -1, 1)
testutils.confirm_equal( np.arccos(cos),
np.zeros((p_ref.shape[0],), dtype=float),
msg = f"Unprojecting (non-normalized, with gradients, in-place) {intrinsics[0]}",
eps = 1e-6)
testutils.confirm_equal(dv_dq,
dv_dqi_ref[...,:2],
msg = f"dv_dq (unnormalized v, in-place): {intrinsics[0]}",
worstcase = True,
relative = True,
eps = 0.01)
testutils.confirm_equal(dv_di,
dv_dqi_ref[...,2:],
msg = f"dv_di (unnormalized v, in-place): {intrinsics[0]}",
worstcase = True,
relative = True,
eps = 0.01)
model0 = mrcal.cameramodel( intrinsics = ('LENSMODEL_PINHOLE',
np.array((1000., 1000., 500., 500.))),
imagersize = np.array((1000,1000)) )
model1 = mrcal.cameramodel( intrinsics = ('LENSMODEL_PINHOLE',
np.array((1100., 1100., 500., 500.))),
imagersize = np.array((1000,1000)) )
# square camera layout
t01 = np.array(( 1., 0.1, -0.2))
R01 = mrcal.R_from_r(np.array((0.001, -0.002, -0.003)))
Rt01 = nps.glue(R01, t01, axis=-2)
p = np.array(args.observed_point)
q0 = mrcal.project(p, *model0.intrinsics())
sigma = 0.1
cache_file = "/tmp/triangulation-study-cache.pickle"
if args.cache is None or args.cache == 'write':
v0local_noisy, v1local_noisy,v0_noisy,v1_noisy,_,_,_,_ = \
mrcal.synthetic_data. \
_noisy_observation_vectors_for_triangulation(p,Rt01,
model0.intrinsics(),
model1.intrinsics(),
args.Nsamples,
sigma = sigma)
mrcal.stereo_rectify_prepare( (model0, model1),
az_fov_deg = az_fov_deg,
el_fov_deg = el_fov_deg,
pixels_per_deg_az = -1./8.,
pixels_per_deg_el = -1./4.)
Rt_cam0_stereo = cookie['Rt_cam0_stereo']
testutils.confirm_equal(Rt_cam0_stereo,
mrcal.identity_Rt(),
msg='vanilla stereo has a vanilla geometry')
testutils.confirm_equal(cookie['baseline'],
nps.mag(rt01[3:]),
msg='vanilla stereo: baseline')
q0, q0x, q0y = mrcal.project(
np.array(((0, 0, 1.), (1e-6, 0, 1.), (0, 1e-6, 1.))), *model0.intrinsics())
testutils.confirm_equal(cookie['pixels_per_deg_az'] * 8.,
(q0x - q0)[0] / 1e-6 * np.pi / 180.,
msg='vanilla stereo: correct az pixel density')
testutils.confirm_equal(cookie['pixels_per_deg_el'] * 4.,
(q0y - q0)[1] / 1e-6 * np.pi / 180.,
msg='vanilla stereo: correct el pixel density')
testutils.confirm_equal(cookie['az_row'].ndim, 1, msg='correct az shape')
Naz = cookie['az_row'].shape[0]
testutils.confirm_equal(cookie['el_col'].ndim, 2, msg='correct el shape')
testutils.confirm_equal(cookie['el_col'].shape[-1], 1, msg='correct el shape')
Nel = cookie['el_col'].shape[0]
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)
v0_rect = mrcal.unproject_pinhole(np.array((np.tan(az0), np.tan(el0))))
v0_rect /= nps.mag(v0_rect)
testutils.confirm_equal( v0_rect, v0,
msg=f'vanilla stereo: az0,el0 represent the same point ({lensmodel})',
eps = 1e-3)
dq0x = np.array((1e-1, 0))
dq0y = np.array((0, 1e-1))
v0x = mrcal.unproject(q0+dq0x, *models_rectified[0].intrinsics())
v0y = mrcal.unproject(q0+dq0y, *models_rectified[0].intrinsics())
dthx = np.arccos(nps.inner(v0x,v0)/np.sqrt(nps.norm2(v0x)*nps.norm2(v0)))
dthy = np.arccos(nps.inner(v0y,v0)/np.sqrt(nps.norm2(v0y)*nps.norm2(v0)))
pixels_per_rad_az_rect = nps.mag(dq0x)/dthx
pixels_per_rad_el_rect = nps.mag(dq0y)/dthy
q0_cam0 = mrcal.project(mrcal.rotate_point_R(Rt_cam0_rect[:3,:], v0),
*model0.intrinsics())
q0x_cam0 = mrcal.project(mrcal.rotate_point_R(Rt_cam0_rect[:3,:], v0x),
*model0.intrinsics())
q0y_cam0 = mrcal.project(mrcal.rotate_point_R(Rt_cam0_rect[:3,:], v0y),
*model0.intrinsics())
pixels_per_rad_az_cam0 = nps.mag(q0x_cam0 - q0_cam0)/dthx
pixels_per_rad_el_cam0 = nps.mag(q0y_cam0 - q0_cam0)/dthy
testutils.confirm_equal(pixels_per_rad_az_rect * 8.,
pixels_per_rad_az_cam0,
msg=f'vanilla stereo: az pixel density ({lensmodel})',
eps = 0.1)
testutils.confirm_equal(pixels_per_rad_el_rect * 4.,
pixels_per_rad_el_cam0,
msg=f'vanilla stereo: el pixel density ({lensmodel})',
####################################################################
# I have the calibrated models, and I can compute the triangulations
#
# I triangulate my 5 points while observed by cameras (2,1) and cameras (1,0)
icameras = ((2, 1), (1, 0))
Nmodelpairs = len(icameras)
# shape (Nmodelpairs,2)
M = [[models_baseline[i] for i in icameras[0]],
[models_baseline[i] for i in icameras[1]]]
# shape (Npoints,Nmodelpairs, 2,2)
q = np.zeros((Npoints, Nmodelpairs, 2, 2), dtype=float)
for ipt in range(Npoints):
for imp in range(Nmodelpairs):
q[ipt, imp, 0, :] = mrcal.project(p_triangulated_true0[ipt],
*M[imp][0].intrinsics())
q[ipt, imp, 1, :] = mrcal.project(
mrcal.transform_point_Rt(
mrcal.compose_Rt(M[imp][1].extrinsics_Rt_fromref(),
M[imp][0].extrinsics_Rt_toref()),
p_triangulated_true0[ipt]), *M[imp][1].intrinsics())
p, \
Var_p0p1_calibration_big, \
Var_p0p1_observation_big, \
Var_p0p1_joint_big = \
mrcal.triangulate( q, M,
q_calibration_stdev = args.q_calibration_stdev,
q_observation_stdev = args.q_observation_stdev,
q_observation_stdev_correlation = args.q_observation_stdev_correlation,
stabilize_coords = args.stabilize_coords )
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(),
models_true[icam1].extrinsics_rt_fromref(),
frames_true, frames_true,
q_true,
lensmodel,
stabilize_coords = args.stabilize_coords)
# world points to the same place
out = subprocess.check_output( (f"{testdir}/../mrcal-graft-models",
'--radius', '-1',
filename0, filename1),
encoding = 'ascii',
stderr = subprocess.DEVNULL)
filename01_compensated = f"{workdir}/model01_compensated.cameramodel"
with open(filename01_compensated, "w") as f:
print(out, file=f)
model01_compensated = mrcal.cameramodel(filename01_compensated)
p1 = np.array((11., 17., 10000.))
pref = mrcal.transform_point_rt( model1.extrinsics_rt_toref(),
p1)
q = mrcal.project( mrcal.transform_point_rt( model1.extrinsics_rt_fromref(),
pref ),
*model1.intrinsics())
q_compensated = \
mrcal.project( mrcal.transform_point_rt( model01_compensated.extrinsics_rt_fromref(),
pref),
*model01_compensated.intrinsics())
testutils.confirm_equal(q_compensated, q,
msg = f"Compensated projection ended up in the same place",
eps = 0.1)
testutils.finish()
def unproject(q, lensmodel, intrinsics_data,
normalize = False,
get_gradients = False,
out = None):
r'''Unprojects pixel coordinates to observation vectors
SYNOPSIS
# q is a (...,2) array of pixel observations
v = mrcal.unproject( q,
lensmodel, intrinsics_data )
### OR ###
m = mrcal.cameramodel(...)
v = mrcal.unproject( q, *m.intrinsics() )
Maps a set of 2D imager points q to a set of 3D vectors in camera coordinates
that produced these pixel observations. Each 3D vector is unique only
up-to-length, and the returned vectors aren't normalized by default. The default
length of the returned vector is arbitrary, and selected for the convenience of
the implementation. Pass normalize=True to always return unit vectors.
This is the "reverse" direction, so an iterative nonlinear optimization is
performed internally to compute this result. This is much slower than
mrcal_project. For OpenCV distortions specifically, OpenCV has
cvUndistortPoints() (and cv2.undistortPoints()), but these are inaccurate and we
do not use them: https://github.com/opencv/opencv/issues/8811
Gradients are available by passing get_gradients=True. Since unproject() is
implemented as an iterative solve around project(), the unproject() gradients
are computed by manipulating the gradients reported by project() at the
solution. The reported gradients are relative to whatever unproject() is
reporting; the unprojection is unique only up-to-length, and the magnitude isn't
fixed. So the gradients may include a component in the direction of the returned
observation vector: this follows the arbitrary scaling used by unproject(). It
is possible to pass normalize=True; we then return NORMALIZED observation
vectors and the gradients of those NORMALIZED vectors. In that case, those
gradients are guaranteed to be orthogonal to the observation vector. The vector
normalization involves a bit more computation, so it isn't the default.
NOTE: THE MAGNITUDE OF THE RETURNED VECTOR CHANGES IF get_gradients CHANGES. The
reported gradients are correct relative to the output returned with
get_gradients=True. Passing normalize=True can be used to smooth this out:
unproject(..., normalize=True)
returns the same vectors as
unproject(..., normalize=True, get_gradients=True)[0]
Broadcasting is fully supported across q and intrinsics_data.
Models that have no gradients available cannot use mrcal_unproject() in C, but
CAN still use this mrcal.unproject() Python routine: a slower routine is
employed that uses numerical differences instead of analytical gradients.
ARGUMENTS
- q: array of dims (...,2); the pixel coordinates we're unprojecting
- lensmodel: a string such as
LENSMODEL_PINHOLE
LENSMODEL_OPENCV4
LENSMODEL_CAHVOR
LENSMODEL_SPLINED_STEREOGRAPHIC_order=3_Nx=16_Ny=12_fov_x_deg=100
- intrinsics_data: array of dims (Nintrinsics):
(focal_x, focal_y, center_pixel_x, center_pixel_y, distortion0, distortion1,
...)
The focal lengths are given in pixels.
- normalize: optional boolean defaults to False. If True: normalize the output
vectors
- get_gradients: optional boolean that defaults to False. Whether we should
compute and report the gradients. This affects what we return (see below). If
not normalize, the magnitude of the reported vectors changes if get_gradients
is turned on/off (see above)
- out: optional argument specifying the destination. By default, new numpy
array(s) are created and returned. To write the results into existing arrays,
specify them with the 'out' kwarg. If not get_gradients: 'out' is the one
numpy array we will write into. Else: 'out' is a tuple of all the output numpy
arrays. If 'out' is given, we return the same arrays passed in. This is the
standard behavior provided by numpysane_pywrap.
RETURNED VALUE
if not get_gradients:
we return an (...,3) array of unprojected observation vectors. Not normalized
by default; see description above
if get_gradients: we return a tuple:
- (...,3) array of unprojected observation vectors
- (...,3,2) array of gradients of unprojected observation vectors in respect
to pixel coordinates
- (...,3,Nintrinsics) array of gradients of unprojected observation vectors in
respect to the intrinsics
'''
def apply_normalization_to_output_with_gradients(v,dv_dq,dv_di):
# vn = v/mag(v)
# dvn = dv (1/mag(v)) + v d(1/mag(v))
# = dv( 1/mag(v) - v vt / mag^3(v) )
# = dv( 1/mag(v) - vn vnt / mag(v) )
# = dv/mag(v) ( 1 - vn vnt )
# v has shape (...,3)
# dv_dq has shape (...,3,2)
# dv_di has shape (...,3,N)
# shape (...,1)
magv_recip = 1. / nps.dummy(nps.mag(v), -1)
v *= magv_recip
# shape (...,1,1)
magv_recip = nps.dummy(magv_recip,-1)
dv_dq *= magv_recip
dv_dq -= nps.xchg( nps.matmult( nps.dummy(nps.xchg(dv_dq, -1,-2), -2),
nps.dummy(nps.outer(v,v),-3) )[...,0,:],
-1, -2)
dv_di *= magv_recip
dv_di -= nps.xchg( nps.matmult( nps.dummy(nps.xchg(dv_di, -1,-2), -2),
nps.dummy(nps.outer(v,v),-3) )[...,0,:],
-1, -2)
# First, handle some trivial cases. I don't want to run the
# optimization-based unproject() if I don't have to
if lensmodel == 'LENSMODEL_PINHOLE' or \
lensmodel == 'LENSMODEL_LONLAT' or \
lensmodel == 'LENSMODEL_LATLON' or \
lensmodel == 'LENSMODEL_STEREOGRAPHIC':
if lensmodel == 'LENSMODEL_PINHOLE':
func = mrcal.unproject_pinhole
always_normalized = False
elif lensmodel == 'LENSMODEL_LONLAT':
func = mrcal.unproject_lonlat
always_normalized = True
elif lensmodel == 'LENSMODEL_LATLON':
func = mrcal.unproject_latlon
always_normalized = True
elif lensmodel == 'LENSMODEL_STEREOGRAPHIC':
func = mrcal.unproject_stereographic
always_normalized = False
if not get_gradients:
v = func(q, intrinsics_data, out = out)
if normalize and not always_normalized:
v /= nps.dummy(nps.mag(v), axis=-1)
return v
# shapes (...,2)
fxy = intrinsics_data[..., :2]
cxy = intrinsics_data[..., 2:]
# shapes (...,3) and (...,3,2)
v, dv_dq = \
func(q, intrinsics_data,
get_gradients = True,
out = None if out is None else (out[0],out[1]))
# q = f l(v) + c
# l(v) = (q-c)/f
#
# dl/dv dv/df = (c-q) / f^2
# dl/dv dv/dq = 1/f
# -> dl/dv = 1 / ( f dv/dq )
# -> dv/df = (c-q) / (f^2 dl/dv) = (c-q) dv/dq / f
#
# dl/dv dv/dc = -1/f
# -> dv/dc = -1 / (f dl/dv) = -1 / (f /( f dv/dq )) = -dv/dq
dv_di_shape = dv_dq.shape[:-1] + (4,)
if out is None:
dv_di = np.zeros( dv_di_shape, dtype=float)
else:
if not (out[2].shape[-len(dv_di_shape):] == dv_di_shape and \
not any(np.array(out[2].shape[:-len(dv_di_shape)]) - 1)):
raise Exception(f"Shape of out[2] doesn't match broadcasted shape for dv_di. Wanted {dv_di_shape}, but got {out[2].shape}")
dv_di = out[2]
dv_di *= 0
# dv/df
dv_di[..., :2] += nps.dummy((cxy - q)/fxy, -2) * dv_dq
# dv/dc
dv_di[..., 2:] -= dv_dq
if normalize and not always_normalized:
apply_normalization_to_output_with_gradients(v,dv_dq,dv_di)
return v,dv_dq,dv_di
try:
meta = mrcal.lensmodel_metadata_and_config(lensmodel)
except:
raise Exception(f"Invalid lens model '{lensmodel}': couldn't get the metadata")
if meta['has_gradients']:
# Main path. We have gradients.
#
# Internal function must have a different argument order so
# that all the broadcasting stuff is in the leading arguments
if not get_gradients:
v = mrcal._mrcal_npsp._unproject(q, intrinsics_data, lensmodel=lensmodel,
out=out)
if normalize:
# Explicitly handle nan and inf to set their normalized values
# to 0. Otherwise I get a scary-looking warning from numpy
i_vgood = \
np.isfinite(v[...,0]) * \
np.isfinite(v[...,1]) * \
np.isfinite(v[...,2])
v[~i_vgood] = np.array((0.,0.,1.))
v /= nps.dummy(nps.mag(v), -1)
v[~i_vgood] = np.array((0.,0.,0.))
return v
# We need to report gradients
vs = mrcal._mrcal_npsp._unproject(q, intrinsics_data, lensmodel=lensmodel)
# I have no gradients available for unproject(), and I need to invert a
# non-square matrix to use the gradients from project(). I deal with this
# with a stereographic mapping
#
# With a simple unprojection I have q -> v
# Instead I now do q -> vs -> u -> v
# I reproject vs, to produce a scaled v = k*vs. I'm assuming all
# projections are central, so vs represents q just as well as v does. u
# is a 2-vector, so dq_du is (2x2), and I can invert it
u = mrcal.project_stereographic(vs)
dv_du = np.zeros( vs.shape + (2,), dtype=float)
v, dv_du = \
mrcal.unproject_stereographic(u,
get_gradients = True,
out = (vs if out is None else out[0],
dv_du))
_,dq_dv,dq_di = mrcal.project(v,
lensmodel, intrinsics_data,
get_gradients = True)
# shape (..., 2,2). Square. Invertible!
dq_du = nps.matmult( dq_dv, dv_du )
# dv/dq = dv/du du/dq =
# = dv/du inv(dq/du)
# = transpose(inv(transpose(dq/du)) transpose(dv/du))
dv_dq = nps.transpose(np.linalg.solve( nps.transpose(dq_du),
nps.transpose(dv_du) ))
if out is not None:
out[1] *= 0.
out[1] += dv_dq
dv_dq = out[1]
# dv/di is a bit different. I have (q,i) -> v. I want to find out
# how moving i affects v while keeping q constant. Taylor expansion
# of projection: q = q0 + dq/dv dv + dq/di di. q is constant so
# dq/dv dv + dq/di di = 0 -> dv/di = - dv/dq dq/di
dv_di = nps.matmult(dv_dq, dq_di,
out = None if out is None else out[2])
dv_di *= -1.
if normalize:
apply_normalization_to_output_with_gradients(v,dv_dq,dv_di)
return v, dv_dq, dv_di
# No projection gradients implemented in C. We should get here approximately
# never. At this time, the only projection function that has no gradients
# implemented is LENSMODEL_CAHVORE, which nobody is really expected to be
# using. If these see use, real gradients should be implemented
#
# We compute the gradients numerically. This is a reimplementation of the C
# code. It's barely maintained, and here for legacy compatibility only
if get_gradients:
raise Exception(f"unproject(..., get_gradients=True) is unsupported for models with no gradients, such as '{lensmodel}'")
if q is None: return q
if q.size == 0:
s = q.shape
return np.zeros(s[:-1] + (3,))
if out is not None:
raise Exception(f"unproject(..., out) is unsupported if out is not None and we're using a model with no gradients, such as '{lensmodel}'")
fxy = intrinsics_data[..., :2]
cxy = intrinsics_data[..., 2:4]
# undistort the q, by running an optimizer
import scipy.optimize
# I optimize each point separately because the internal optimization
# algorithm doesn't know that each point is independent, so if I optimized
# it all together, it would solve a dense linear system whose size is linear
# in Npoints. The computation time thus would be much slower than
# linear(Npoints)
@nps.broadcast_define( ((2,),), )
def undistort_this(q0):
def cost_no_gradients(vxy, *args, **kwargs):
'''Optimization functions'''
return \
mrcal.project(np.array((vxy[0],vxy[1],1.)), lensmodel, intrinsics_data) - \
q0
# seed assuming distortions aren't there
vxy_seed = (q0 - cxy) / fxy
# no gradients available
result = scipy.optimize.least_squares(cost_no_gradients, vxy_seed,
'3-point')
vxy = result.x
# This needs to be precise; if it isn't, I barf. Shouldn't happen
# very often
if np.sqrt(result.cost/2.) > 1e-3:
if not unproject.__dict__.get('already_complained'):
sys.stderr.write("WARNING: unproject() wasn't able to precisely compute some points. Returning nan for those. Will complain just once\n")
unproject.already_complained = True
return np.array((np.nan,np.nan))
return vxy
vxy = undistort_this(q)
# I append a 1. shape = (..., 3)
v = nps.glue(vxy, np.ones( vxy.shape[:-1] + (1,) ), axis=-1)
if normalize:
v /= nps.dummy(nps.mag(v), -1)
return v
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 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)
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)),
args.Nframes)
if args.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,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
def grad_broadcasted(p_ref, i_ref):
return grad(lambda pi: mrcal.project(pi[:3], intrinsics[0], pi[3:]),
nps.glue(p_ref,i_ref, axis=-1))
def stereo_rectify_prepare(models,
az_fov_deg,
el_fov_deg,
az0_deg = None,
el0_deg = 0,
pixels_per_deg_az = None,
pixels_per_deg_el = None):
r'''Precompute everything needed for stereo rectification and matching
SYNOPSIS
import sys
import mrcal
import cv2
import numpy as np
# Read commandline arguments: model0 model1 image0 image1
models = [ mrcal.cameramodel(sys.argv[1]),
mrcal.cameramodel(sys.argv[2]), ]
images = [ cv2.imread(sys.argv[i]) \
for i in (3,4) ]
# Prepare the stereo system
rectification_maps,cookie = \
mrcal.stereo_rectify_prepare(models,
az_fov_deg = 120,
el_fov_deg = 100)
# Visualize the geometry of the two cameras and of the rotated stereo
# coordinate system
Rt_cam0_ref = models[0].extrinsics_Rt_fromref()
Rt_cam0_stereo = cookie['Rt_cam0_stereo']
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)
mrcal.show_geometry( models + [ rt_stereo_ref ],
( "camera0", "camera1", "stereo" ),
show_calobjects = False,
wait = True )
# Rectify the images
images_rectified = \
[ mrcal.transform_image(images[i], rectification_maps[i]) \
for i in range(2) ]
cv2.imwrite('/tmp/rectified0.jpg', images_rectified[0])
cv2.imwrite('/tmp/rectified1.jpg', images_rectified[1])
# Find stereo correspondences using OpenCV
block_size = 3
max_disp = 160 # in pixels
stereo = \
cv2.StereoSGBM_create(minDisparity = 0,
numDisparities = max_disp,
blockSize = block_size,
P1 = 8 *3*block_size*block_size,
P2 = 32*3*block_size*block_size,
uniquenessRatio = 5,
disp12MaxDiff = 1,
speckleWindowSize = 50,
speckleRange = 1)
disparity16 = stereo.compute(*images_rectified) # in pixels*16
cv2.imwrite('/tmp/disparity.png',
mrcal.apply_color_map(disparity16,
0, max_disp*16.))
# Convert the disparities to range to camera0
r = mrcal.stereo_range( disparity16.astype(np.float32) / 16.,
**cookie )
cv2.imwrite('/tmp/range.png', mrcal.apply_color_map(r, 5, 1000))
This function does the initial computation required to perform stereo matching,
and to get ranges from a stereo pair. It computes
- the pose of the rectified stereo coordinate system
- the azimuth/elevation grid used in the rectified images
- the rectification maps used to transform images into the rectified space
Using the results of one call to this function we can compute the stereo
disparities of many pairs of synchronized images.
This function is generic: the two cameras may have any lens models, any
resolution and any geometry. They don't even have to match. As long as there's
some non-zero baseline and some overlapping views, we can set up stereo matching
using this function. The input images are tranformed into a "rectified" space.
Geometrically, the rectified coordinate system sits at the origin of camera0,
with a rotation. The axes of the rectified coordinate system:
- x: from the origin of camera0 to the origin of camera1 (the baseline direction)
- y: completes the system from x,z
- z: the "forward" direction of the two cameras, with the component parallel to
the baseline subtracted off
In a nominal geometry (the two cameras are square with each other, camera1
strictly to the right of camera0), the rectified coordinate system exactly
matches the coordinate system of camera0. The above formulation supports any
geometry, however, including vertical and/or forward/backward shifts. Vertical
stereo is supported.
Rectified images represent 3D planes intersecting the origins of the two
cameras. The tilt of each plane is the "elevation". While the left/right
direction inside each plane is the "azimuth". We generate rectified images where
each pixel coordinate represents (x = azimuth, y = elevation). Thus each row
scans the azimuths in a particular elevation, and thus each row in the two
rectified images represents the same plane in 3D, and matching features in each
row can produce a stereo disparity and a range.
In the rectified system, elevation is a rotation along the x axis, while azimuth
is a rotation normal to the resulting tilted plane.
We produce rectified images whose pixel coordinates are linear with azimuths and
elevations. This means that the azimuth angular resolution is constant
everywhere, even at the edges of a wide-angle image.
We return a set of transformation maps and a cookie. The maps can be used to
generate rectified images. These rectified images can be processed by any
stereo-matching routine to generate a disparity image. To interpret the
disparity image, call stereo_unproject() or stereo_range() with the cookie
returned here.
The cookie is a Python dict that describes the rectified space. It is guaranteed
to have the following keys:
- Rt_cam0_stereo: an Rt transformation to map a representation of points in the
rectified coordinate system to a representation in the camera0 coordinate system
- baseline: the distance between the two cameras
- az_row: an array of shape (Naz,) describing the azimuths in each row of the
disparity image
- el_col: an array of shape (Nel,1) describing the elevations in each column of
the disparity image
ARGUMENTS
- models: an iterable of two mrcal.cameramodel objects representing the cameras
in the stereo system. Any sane combination of lens models and resolutions and
geometries is valid
- az_fov_deg: required value for the azimuth (along-the-baseline) field-of-view
of the desired rectified view, in pixels
- el_fov_deg: required value for the elevation (across-the-baseline)
field-of-view of the desired rectified view, in pixels
- az0_deg: optional value for the azimuth center of the rectified images. This
is especially significant in a camera system with some forward/backward shift.
That causes the baseline to no longer be perpendicular with the view axis of
the cameras, and thus azimuth = 0 is no longer at the center of the input
images. If omitted, we compute the center azimuth that aligns with the center
of the cameras' view
- el0_deg: optional value for the elevation center of the rectified system.
Defaults to 0.
- pixels_per_deg_az: optional value for the azimuth resolution of the rectified
image. If omitted (or None), we use the resolution of the input image at
(azimuth, elevation) = 0. If a resolution of <0 is requested, we use this as a
scale factor on the resolution of the input image. For instance, to downsample
by a factor of 2, pass pixels_per_deg_az = -0.5
- pixels_per_deg_el: same as pixels_per_deg_az but in the elevation direction
RETURNED VALUES
We return a tuple
- transformation maps: a tuple of length-2 containing transformation maps for
each camera. Each map can be used to mrcal.transform_image() images to the
rectified space
- cookie: a dict describing the rectified space. Intended as input to
stereo_unproject() and stereo_range(). See the description above for more
detail
'''
if len(models) != 2:
raise Exception("I need exactly 2 camera models")
def normalize(v):
v /= nps.mag(v)
return v
def remove_projection(a, proj_base):
r'''Returns the normalized component of a orthogonal to proj_base
proj_base assumed normalized'''
v = a - nps.inner(a,proj_base)*proj_base
return normalize(v)
######## Compute the geometry of the rectified stereo system. This is a
######## rotation, centered at camera0. More or less we have axes:
########
######## x: from camera0 to camera1
######## y: completes the system from x,z
######## z: component of the cameras' viewing direction
######## normal to the baseline
Rt_cam0_ref = models[0].extrinsics_Rt_fromref()
Rt01 = mrcal.compose_Rt( Rt_cam0_ref,
models[1].extrinsics_Rt_toref())
Rt10 = mrcal.invert_Rt(Rt01)
# Rotation relating camera0 coords to the rectified camera coords. I fill in
# each row separately
R_stereo_cam0 = np.zeros((3,3), dtype=float)
right = R_stereo_cam0[0,:]
down = R_stereo_cam0[1,:]
forward = R_stereo_cam0[2,:]
# "right" of the rectified coord system: towards the origin of camera1 from
# camera0, in camera0 coords
right[:] = Rt01[3,:]
baseline = nps.mag(right)
right /= baseline
# "forward" for each of the two cameras, in the cam0 coord system
forward0 = np.array((0,0,1.))
forward1 = Rt01[:3,2]
# "forward" of the rectified coord system, in camera0 coords. The mean of
# the two non-right "forward" directions
forward[:] = normalize( ( remove_projection(forward0,right) +
remove_projection(forward1,right) ) / 2. )
# "down" of the rectified coord system, in camera0 coords. Completes the
# right,down,forward coordinate system
down[:] = np.cross(forward,right)
R_cam0_stereo = nps.transpose(R_stereo_cam0)
######## Done with the geometry! Now to get the az/el grid. I need to figure
######## out the resolution and the extents
if az0_deg is not None:
az0 = az0_deg * np.pi/180.
else:
# In the rectified system az=0 sits perpendicular to the baseline.
# Normally the cameras are looking out perpendicular to the baseline
# also, so I center my azimuth samples around 0 to match the cameras'
# field of view. But what if the geometry isn't square, and one camera
# is behind the other? Like this:
#
# camera
# view
# ^
# |
# \ | /
# \_/
# . /
# . /az=0
# ./
# .
# baseline .
# .
# \ /
# \_/
#
# Here the center-of-view axis of each camera is not at all
# perpendicular to the baseline. Thus I compute the mean "forward"
# direction of the cameras in the rectified system, and set that as the
# center azimuth az0.
v0 = nps.matmult( forward0, R_cam0_stereo ).ravel()
v1 = nps.matmult( forward1, R_cam0_stereo ).ravel()
v0[1] = 0.0
v1[1] = 0.0
normalize(v0)
normalize(v1)
v = v0 + v1
az0 = np.arctan2(v[0],v[2])
el0 = el0_deg * np.pi/180.
####### Rectified image resolution
if pixels_per_deg_az is None or pixels_per_deg_az < 0 or \
pixels_per_deg_el is None or pixels_per_deg_el < 0:
# I need to compute the resolution of the rectified images. I try to
# match the resolution of the cameras. I just look at camera0. If you
# have different cameras, pass in pixels_per_deg yourself :)
#
# I look at the center of the stereo field of view. There I have q =
# project(v) where v is a unit projection vector. I compute dq/dth where
# th is an angular perturbation applied to v.
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)
v, dv_dazel = stereo_unproject(az0, el0, get_gradients = True)
v0 = mrcal.rotate_point_R(R_cam0_stereo, v)
dv0_dazel = nps.matmult(R_cam0_stereo, dv_dazel)
_,dq_dv0,_ = mrcal.project(v0, *models[0].intrinsics(), get_gradients = True)
# I rotate my v to a coordinate system where u = rotate(v) is [0,0,1].
# Then u = [a,b,0] are all orthogonal to v. So du/dth = [cos, sin, 0].
# I then have dq/dth = dq/dv dv/du [cos, sin, 0]t
# ---> dq/dth = dq/dv dv/du[:,:2] [cos, sin]t = M [cos,sin]t
#
# norm2(dq/dth) = [cos,sin] MtM [cos,sin]t is then an ellipse with the
# eigenvalues of MtM giving me the best and worst sensitivities. I can
# use mrcal.worst_direction_stdev() to find the densest direction. But I
# actually know the directions I care about, so I evaluate them
# independently for the az and el directions
# Ruv = rotation_any_v_to_z(v0)
# M = nps.matmult(dq_dv0, nps.transpose(Ruv[:2,:]))
# # I pick the densest direction: highest |dq/dth|
# pixels_per_rad = mrcal.worst_direction_stdev( nps.matmult( nps.transpose(M),M) )
if pixels_per_deg_az is None or pixels_per_deg_az < 0:
dq_daz = nps.inner( dq_dv0, dv0_dazel[:,0] )
pixels_per_rad_az_have = nps.mag(dq_daz)
if pixels_per_deg_az is not None:
# negative pixels_per_deg_az requested means I use the requested
# value as a scaling
pixels_per_deg_az = -pixels_per_deg_az * pixels_per_rad_az_have*np.pi/180.
else:
pixels_per_deg_az = pixels_per_rad_az_have*np.pi/180.
if pixels_per_deg_el is None or pixels_per_deg_el < 0:
dq_del = nps.inner( dq_dv0, dv0_dazel[:,1] )
pixels_per_rad_el_have = nps.mag(dq_del)
if pixels_per_deg_el is not None:
# negative pixels_per_deg_el requested means I use the requested
# value as a scaling
pixels_per_deg_el = -pixels_per_deg_el * pixels_per_rad_el_have*np.pi/180.
else:
pixels_per_deg_el = pixels_per_rad_el_have*np.pi/180.
Naz = round(az_fov_deg*pixels_per_deg_az)
Nel = round(el_fov_deg*pixels_per_deg_el)
# Adjust the fov to keep the requested resolution and pixel counts
az_fov_radius_deg = Naz / (2.*pixels_per_deg_az)
el_fov_radius_deg = Nel / (2.*pixels_per_deg_el)
# shape (Naz,)
az = np.linspace(az0 - az_fov_radius_deg*np.pi/180.,
az0 + az_fov_radius_deg*np.pi/180.,
Naz)
# shape (Nel,1)
el = nps.dummy( np.linspace(el0 - el_fov_radius_deg*np.pi/180.,
el0 + el_fov_radius_deg*np.pi/180.,
Nel),
-1 )
# v has shape (Nel,Naz,3)
v = stereo_unproject(az, el)
v0 = nps.matmult( nps.dummy(v, -2), R_stereo_cam0 )[...,0,:]
v1 = nps.matmult( nps.dummy(v0, -2), Rt01[:3,:] )[...,0,:]
cookie = \
dict( Rt_cam0_stereo = nps.glue(R_cam0_stereo, np.zeros((3,)), axis=-2),
baseline = baseline,
az_row = az,
el_col = el,
# The caller should NOT assume these are available in the cookie:
# some other rectification scheme may not produce linear az/el
# maps
pixels_per_deg_az = pixels_per_deg_az,
pixels_per_deg_el = pixels_per_deg_el,
)
return \
(mrcal.project( v0, *models[0].intrinsics()).astype(np.float32), \
mrcal.project( v1, *models[1].intrinsics()).astype(np.float32)), \
cookie
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)
2.043015135,1.211903685,2.05291186,1.188092787,2.09486724,1.179277314,
2.078230124,1.186273023,2.077743945,1.148028845,2.081634186,1.131207467,
2.112936851,1.126412871,2.113220553,1.114991063,2.017901873,1.244588667,
2.051238803,1.201855728,2.043256406,1.216674722,2.035286046,1.178380907,
2.08028318,1.178783085,2.051214271,1.173560417,2.059298121,1.182414688,
2.094607679,1.177960959,2.086998287,1.147371259,2.12029442,1.138197348,
2.138994213, 1.114846113,])))
# a few points, some wide, some not. None behind the camera
p = np.array(((1.0, 2.0, 10.0), (-1.1, 0.3, 1.0), (-0.9, -1.5, 1.0)))
delta = 1e-6
for i in intrinsics:
q, dq_dp, dq_di = mrcal.project(p, *i, get_gradients=True)
Nintrinsics = mrcal.lensmodel_num_params(i[0])
testutils.confirm_equal(dq_di.shape[-1],
Nintrinsics,
msg=f"{i[0]}: Nintrinsics match for {i[0]}")
if Nintrinsics != dq_di.shape[-1]:
continue
for ivar in range(dq_dp.shape[-1]):
# center differences
p1 = p.copy()
p1[..., ivar] = p[..., ivar] - delta / 2
q1 = mrcal.project(p1, *i, get_gradients=False)
p1[..., ivar] += delta
(-35., 14., 77.), (5., -0., 110.), (1., 50., 50.)))
# The points are all somewhere at +z. So the Camera poses are all ~ identity
ref_extrinsics_rt_fromref = np.array(
((-0.1, -0.07, 0.01, 10.0, 4.0, -7.0), (-0.01, 0.05, -0.02, 30.0, -8.0,
-8.0), (-0.1, 0.03, -0.03, 10.0,
-9.0, 20.0),
(0.04, -0.04, 0.03, -20.0, 2.0, -11.0), (0.01, 0.05, -0.05, -10.0, 3.0,
9.0)))
# shape (Ncamposes, Npoints, 3)
ref_p_cam = mrcal.transform_point_rt(nps.mv(ref_extrinsics_rt_fromref, -2, -3),
ref_p)
# shape (Ncamposes, Npoints, 2)
ref_q_cam = mrcal.project(ref_p_cam, lensmodel, intrinsics_data)
# Observations are incomplete. Not all points are observed from everywhere
indices_point_camintrinsics_camextrinsics = \
np.array(((0, 0, 1),
(0, 0, 2),
(0, 0, 4),
(1, 0, 0),
(1, 0, 1),
(1, 0, 4),
(2, 0, 0),
(2, 0, 1),
(2, 0, 2),
(3, 0, 1),
(3, 0, 2),
(3, 0, 3),
else:
raise Exception(
"Unknown projection type. Currently I support 'lonlat','stereographic'"
)
intrinsics = (lensmodel, np.array((fx, fy, cx, cy)))
q_projected = func_project(p, fx, fy, cx, cy)
testutils.confirm_equal(q_projected,
q_projected_ref,
msg=f"project_{name}()",
worstcase=True,
relative=True)
testutils.confirm_equal(
mrcal.project(p, *intrinsics),
q_projected,
msg=f"project({name}) returns the same as project_{name}()",
worstcase=True,
relative=True)
v_unprojected = func_unproject(q_projected, fx, fy, cx, cy)
if unproject_is_normalized:
testutils.confirm_equal(
nps.mag(v_unprojected),
1.,
msg=f"unproject_{name}() returns normalized vectors",
worstcase=True,
relative=True)
testutils.confirm_equal(v_unprojected,
p / nps.dummy(nps.mag(p), axis=-1),
def cost_no_gradients(vxy, *args, **kwargs):
'''Optimization functions'''
return \
mrcal.project(np.array((vxy[0],vxy[1],1.)), lensmodel, intrinsics_data) - \
q0
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
