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 _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 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
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(),
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.)
el_fov_deg,
msg=f'complex stereo: el_fov ({lensmodel})',
eps = 0.5)
# I examine points somewhere in space. I make sure the rectification maps
# transform it properly. And I compute what its az,el and disparity would have
# been, and I check the geometric functions
pcam0 = np.array((( 1., 2., 12.),
(-4., 3., 12.)))
qcam0 = mrcal.project( pcam0, *model0.intrinsics() )
pcam1 = mrcal.transform_point_rt(mrcal.invert_rt(rt01), pcam0)
qcam1 = mrcal.project( pcam1, *model1.intrinsics() )
prect0 = mrcal.transform_point_Rt( mrcal.invert_Rt(Rt_cam0_rect), pcam0)
prect1 = prect0 - Rt01_rectified[3,:]
qrect0 = mrcal.project(prect0, *models_rectified[0].intrinsics())
qrect1 = mrcal.project(prect1, *models_rectified[1].intrinsics())
Naz,Nel = models_rectified[0].imagersize()
row = np.arange(Naz, dtype=float)
col = np.arange(Nel, dtype=float)
rectification_maps = mrcal.rectification_maps((model0,model1),
models_rectified)
interp_rectification_map0x = \
scipy.interpolate.RectBivariateSpline(row, col,
nps.transpose(rectification_maps[0][...,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 )
testutils.confirm_equal(p.shape, (Npoints, Nmodelpairs, 3),
msg="point array has the right shape")
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
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
testutils.confirm_equal(mrcal.compose_rt(mrcal.rt_from_Rt(Rt),
mrcal.invert_rt(
mrcal.rt_from_Rt(Rt))),
np.zeros((6, )),
msg='compose_rt')
testutils.confirm_equal(mrcal.identity_Rt(),
nps.glue(np.eye(3), np.zeros((3, )), axis=-2),
msg='identity_Rt')
testutils.confirm_equal(mrcal.identity_rt(),
np.zeros((6, )),
msg='identity_rt')
testutils.confirm_equal(mrcal.transform_point_Rt(Rt, p),
Tp,
msg='transform_point_Rt')
testutils.confirm_equal(mrcal.transform_point_rt(mrcal.rt_from_Rt(Rt), p),
Tp,
msg='transform_point_rt')
testutils.confirm_equal(mrcal.transform_point_Rt(mrcal.invert_Rt(Rt), Tp),
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')
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)
msg='funny stereo geometry')
testutils.confirm_equal(cookie['baseline'],
nps.mag(rt01[3:]),
msg='funny stereo: baseline')
# I examine points somewhere in space. I make sure the rectification maps
# transform it properly. And I compute what its az,el and disparity would have
# been, and I check the geometric functions
pcam0 = np.array(((1., 2., 10.), (-4., 3., 10.)))
qcam0 = mrcal.project(pcam0, *model0.intrinsics())
pcam1 = mrcal.transform_point_rt(mrcal.invert_rt(rt01), pcam0)
qcam1 = mrcal.project(pcam1, *model1.intrinsics())
pstereo0 = mrcal.transform_point_Rt(mrcal.invert_Rt(Rt_cam0_stereo), pcam0)
el0 = np.arctan2(pstereo0[:, 1], pstereo0[:, 2])
az0 = np.arctan2(pstereo0[:, 0], pstereo0[:, 2] / np.cos(el0))
pstereo1 = pstereo0 - np.array((cookie['baseline'], 0, 0))
el1 = np.arctan2(pstereo1[:, 1], pstereo1[:, 2])
az1 = np.arctan2(pstereo1[:, 0], pstereo1[:, 2] / np.cos(el1))
disparity = az0 - az1
interp_rectification_map0x = \
scipy.interpolate.RectBivariateSpline(cookie['az_row'].ravel(),
cookie['el_col'].ravel(),
nps.transpose(rectification_maps[0][...,0]))
interp_rectification_map0y = \
msg='rotate_point_r(inverted) result')
y, J_r, J_x = mrcal.rotate_point_r(r0_ref,
x,
get_gradients=True,
out=(out3, out33, out33a),
inverted=True)
J_r_ref = grad(lambda r: nps.matmult(x, R_from_r(r)), r0_ref)
J_x_ref = grad(lambda x: nps.matmult(x, R_from_r(r0_ref)), x)
confirm_equal(y,
nps.matmult(x, R_from_r(r0_ref)),
msg='rotate_point_r(inverted) result')
confirm_equal(J_r, J_r_ref, msg='rotate_point_r(inverted) J_r')
confirm_equal(J_x, J_x_ref, msg='rotate_point_r(inverted) J_x')
y = mrcal.transform_point_Rt(Rt0_ref, x, out=out3)
confirm_equal(y,
nps.matmult(x, nps.transpose(R0_ref)) + t0_ref,
msg='transform_point_Rt result')
y, J_Rt, J_x = mrcal.transform_point_Rt(Rt0_ref,
x,
get_gradients=True,
out=(out3, out343, out33))
J_Rt_ref = grad(lambda Rt: nps.matmult(x, nps.transpose(Rt[:3, :])) + Rt[3, :],
Rt0_ref)
J_x_ref = R0_ref
confirm_equal(y,
nps.matmult(x, nps.transpose(R0_ref)) + t0_ref,
msg='transform_point_Rt result')
confirm_equal(J_Rt, J_Rt_ref, msg='transform_point_Rt J_Rt')
def hypothesis_board_corner_positions(icam_intrinsics=None,
idx_inliers=None,
**optimization_inputs):
r'''Reports the 3D chessboard points observed by a camera at calibration time
SYNOPSIS
model = mrcal.cameramodel("xxx.cameramodel")
optimization_inputs = model.optimization_inputs()
# shape (Nobservations, Nheight, Nwidth, 3)
pcam = mrcal.hypothesis_board_corner_positions(**optimization_inputs)[0]
i_intrinsics = \
optimization_inputs['indices_frame_camintrinsics_camextrinsics'][:,1]
# shape (Nobservations,1,1,Nintrinsics)
intrinsics = nps.mv(optimization_inputs['intrinsics'][i_intrinsics],-2,-4)
optimization_inputs['observations_board'][...,:2] = \
mrcal.project( pcam,
optimization_inputs['lensmodel'],
intrinsics )
# optimization_inputs now contains perfect, noiseless board observations
x = mrcal.optimizer_callback(**optimization_inputs)[1]
print(nps.norm2(x[:mrcal.num_measurements_boards(**optimization_inputs)]))
==>
0
The optimization routine generates hypothetical observations from a set of
parameters being evaluated, trying to match these hypothetical observations to
real observations. To facilitate analysis, this routine returns these
hypothetical coordinates of the chessboard corners being observed. This routine
reports the 3D points in the coordinate system of the observing camera.
The hypothetical points are constructed from
- The calibration object geometry
- The calibration object-reference transformation in
optimization_inputs['frames_rt_toref']
- The camera extrinsics (reference-camera transformation) in
optimization_inputs['extrinsics_rt_fromref']
- The table selecting the camera and calibration object frame for each
observation in
optimization_inputs['indices_frame_camintrinsics_camextrinsics']
ARGUMENTS
- icam_intrinsics: optional integer specifying which intrinsic camera in the
optimization_inputs we're looking at. If omitted (or None), we report
camera-coordinate points for all the cameras
- idx_inliers: optional numpy array of booleans of shape
(Nobservations,object_height,object_width) to select the outliers manually. If
omitted (or None), the outliers are selected automatically: idx_inliers =
observations_board[...,2] > 0. This argument is available to pick common
inliers from two separate solves.
- **optimization_inputs: a dict() of arguments passable to mrcal.optimize() and
mrcal.optimizer_callback(). We use the geometric data. This dict is obtainable
from a cameramodel object by calling cameramodel.optimization_inputs()
RETURNED VALUE
- An array of shape (Nobservations, Nheight, Nwidth, 3) containing the
coordinates (in the coordinate system of each camera) of the chessboard
corners, for ALL the cameras. These correspond to the observations in
optimization_inputs['observations_board'], which also have shape
(Nobservations, Nheight, Nwidth, 3)
- An array of shape (Nobservations_thiscamera, Nheight, Nwidth, 3) containing
the coordinates (in the camera coordinate system) of the chessboard corners,
for the particular camera requested in icam_intrinsics. If icam_intrinsics is
None: this is the same array as the previous returned value
- an (N,3) array containing camera-frame 3D points observed at calibration time,
and accepted by the solver as inliers. This is a subset of the 2nd returned
array.
- an (N,3) array containing camera-frame 3D points observed at calibration time,
but rejected by the solver as outliers. This is a subset of the 2nd returned
array.
'''
observations_board = optimization_inputs.get('observations_board')
if observations_board is None:
return Exception("No board observations available")
indices_frame_camintrinsics_camextrinsics = \
optimization_inputs['indices_frame_camintrinsics_camextrinsics']
object_width_n = observations_board.shape[-2]
object_height_n = observations_board.shape[-3]
object_spacing = optimization_inputs['calibration_object_spacing']
calobject_warp = optimization_inputs.get('calobject_warp')
# shape (Nh,Nw,3)
full_object = mrcal.ref_calibration_object(object_width_n, object_height_n,
object_spacing, calobject_warp)
frames_Rt_toref = \
mrcal.Rt_from_rt( optimization_inputs['frames_rt_toref'] )\
[ indices_frame_camintrinsics_camextrinsics[:,0] ]
extrinsics_Rt_fromref = \
nps.glue( mrcal.identity_Rt(),
mrcal.Rt_from_rt(optimization_inputs['extrinsics_rt_fromref']),
axis = -3 ) \
[ indices_frame_camintrinsics_camextrinsics[:,2]+1 ]
Rt_cam_frame = mrcal.compose_Rt(extrinsics_Rt_fromref, frames_Rt_toref)
p_cam_calobjects = \
mrcal.transform_point_Rt(nps.mv(Rt_cam_frame,-3,-5), full_object)
# shape (Nobservations,Nheight,Nwidth)
if idx_inliers is None:
idx_inliers = observations_board[..., 2] > 0
idx_outliers = ~idx_inliers
if icam_intrinsics is None:
return \
p_cam_calobjects, \
p_cam_calobjects, \
p_cam_calobjects[idx_inliers, ...], \
p_cam_calobjects[idx_outliers, ...]
# The user asked for a specific camera. Separate out its data
# shape (Nobservations,)
idx_observations = indices_frame_camintrinsics_camextrinsics[:,
1] == icam_intrinsics
idx_inliers[~idx_observations] = False
idx_outliers[~idx_observations] = False
return \
p_cam_calobjects, \
p_cam_calobjects[idx_observations, ...], \
p_cam_calobjects[idx_inliers, ...], \
p_cam_calobjects[idx_outliers, ...]
confirm_equal(
J_r,
J_r_ref,
msg=
'rotate_point_r(inverted, with-gradients result written in-place into x) J_r'
)
confirm_equal(
J_x,
J_x_ref,
msg=
'rotate_point_r(inverted, with-gradients result written in-place into x) J_x'
)
################# transform_point_Rt
y = mrcal.transform_point_Rt(Rt0_ref, x, out=out3)
confirm_equal(y,
nps.matmult(x, nps.transpose(R0_ref)) + t0_ref,
msg='transform_point_Rt result')
y, J_Rt, J_x = mrcal.transform_point_Rt(Rt0_ref,
x,
get_gradients=True,
out=(out3, out343, out33))
J_Rt_ref = grad(lambda Rt: nps.matmult(x, nps.transpose(Rt[:3, :])) + Rt[3, :],
Rt0_ref)
J_x_ref = R0_ref
confirm_equal(y,
nps.matmult(x, nps.transpose(R0_ref)) + t0_ref,
msg='transform_point_Rt result')
confirm_equal(J_Rt, J_Rt_ref, msg='transform_point_Rt J_Rt')
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
