def callback_linf_angle(p, v0, v1, t01):
costh0 = nps.inner(p, v0) / np.sqrt(nps.norm2(p) * nps.norm2(v0))
costh1 = nps.inner(p - t01, v1) / np.sqrt(
nps.norm2(p - t01) * nps.norm2(v1))
# Simpler function that has the same min
return (1 - min(costh0, costh1)) * 1e9
def callback_l1_angle(p, v0, v1, t01):
costh0 = nps.inner(p, v0) / np.sqrt(nps.norm2(p) * nps.norm2(v0))
costh1 = nps.inner(p - t01, v1) / np.sqrt(
nps.norm2(p - t01) * nps.norm2(v1))
th0 = np.arccos(min(costh0, 1.0))
th1 = np.arccos(min(costh1, 1.0))
return np.abs(th0) + np.abs(th1)
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 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 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 _validateValidIntrinsicsRegion(valid_intrinsics_region):
r'''Raises an exception if the given valid_intrinsics_region is illegal'''
if valid_intrinsics_region is None:
return
try:
# valid intrinsics region is a closed contour, so I need at least 4
# points to be valid. Or as a special case, a (0,2) array is legal, and
# means "intrinsics are valid nowhere"
if not (valid_intrinsics_region.ndim == 2 and \
valid_intrinsics_region.shape[1] == 2 and \
(valid_intrinsics_region.shape[0] >= 4 or \
valid_intrinsics_region.shape[0] == 0)):
raise Exception("The valid extrinsics region must be a numpy array of shape (N,2) with N >= 4 or N == 0")
except:
raise Exception("The valid extrinsics region must be a numpy array of shape (N,2) with N >= 4. Instead got type {} of shape {}". \
format(type(valid_intrinsics_region), valid_intrinsics_region.shape if type(valid_intrinsics_region) is np.ndarray else None))
if valid_intrinsics_region.size > 0 and \
nps.norm2(valid_intrinsics_region[0] - valid_intrinsics_region[-1]) > 1e-6:
raise Exception("The valid extrinsics region must be a closed contour: the first and last points must be identical")
def r_from_R(R):
r'''Rodrigues vector from a Rotation matrix
Simple reference implementation from wikipedia:
https://en.wikipedia.org/wiki/Rotation_matrix#Conversion_from_and_to_axis%E2%80%93angle
I assume the input is a valid rotation matrix
'''
axis = np.array((R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]))
if nps.norm2(axis) > 1e-20:
# normal path
costh = (np.trace(R) - 1.) / 2.
th = np.arccos(costh)
return axis / nps.mag(axis) * th
# small mag(axis). Can't divide by it. But I can look at the limit.
#
# axis / (2 sinth)*th = axis/2 *th/sinth ~ axis/2
return axis / 2.
def R_from_r(r):
r'''Rotation matrix from a Rodrigues vector
Simple reference implementation from wikipedia:
https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
'''
th_sq = nps.norm2(r)
Kth = np.array(((0, -r[2], r[1]), (r[2], 0, -r[0]), (-r[1], r[0], 0)))
if th_sq > 1e-20:
# normal path
th = np.sqrt(th_sq)
s = np.sin(th)
c = np.cos(th)
K = Kth / th
return np.eye(3) + s * K + (1. - c) * nps.matmult(K, K)
# small th. Can't divide by it. But I can look at the limit.
#
# s*K = Kth * sin(th)/th -> Kth
#
# (1-c)*nps.matmult(K,K) = (1-c) Kth^2/th^2 -> Kth^2 s / 2th -> Kth^2/2
return np.eye(3) + Kth + nps.matmult(Kth, Kth) / 2.
stats = mrcal.optimize(nps.atleast_dims(intrinsics_data, -2),
extrinsics_rt_fromref,
None,
points,
None,
None,
observations,
indices_point_camintrinsics_camextrinsics,
lensmodel,
imagersizes=nps.atleast_dims(imagersize, -2),
Npoints_fixed=Npoints_fixed,
point_min_range=1.0,
point_max_range=1000.0,
do_optimize_intrinsics_core=False,
do_optimize_intrinsics_distortions=False,
do_optimize_extrinsics=True,
do_optimize_frames=True,
do_apply_outlier_rejection=False,
do_apply_regularization=True,
verbose=False)
# Got a solution. How well do they fit?
fit_rms = np.sqrt(np.mean(nps.norm2(points - ref_p)))
testutils.confirm_equal(fit_rms,
0,
msg=f"Solved at ref coords with known-position points",
eps=1.0)
testutils.finish()
ranges_true = nps.mag(p_triangulated_true0)
ranges_sampled = nps.transpose(nps.mag(p_triangulated_sampled0))
mean_ranges_sampled = ranges_sampled.mean(axis=-1)
Var_ranges_sampled = ranges_sampled.var(axis=-1)
# r = np.mag(p)
# dr_dp = p/r
# Var(r) = dr_dp var(p) dr_dpT
# = p var(p) pT / norm2(p)
Var_ranges_joint = np.zeros((Npoints, ), dtype=float)
Var_ranges_calibration = np.zeros((Npoints, ), dtype=float)
Var_ranges_observations = np.zeros((Npoints, ), dtype=float)
for ipt in range(Npoints):
Var_ranges_joint[ipt] = \
nps.matmult(p_triangulated0[ipt],
Var_p_joint[ipt,:,ipt,:],
nps.transpose(p_triangulated0[ipt]))[0] / nps.norm2(p_triangulated0[ipt])
Var_ranges_calibration[ipt] = \
nps.matmult(p_triangulated0[ipt],
Var_p_calibration[ipt,:,ipt,:],
nps.transpose(p_triangulated0[ipt]))[0] / nps.norm2(p_triangulated0[ipt])
Var_ranges_observations[ipt] = \
nps.matmult(p_triangulated0[ipt],
Var_p_observation[ipt,:,:],
nps.transpose(p_triangulated0[ipt]))[0] / nps.norm2(p_triangulated0[ipt])
diff = p_triangulated0[1] - p_triangulated0[0]
distance = nps.mag(diff)
distance_true = nps.mag(p_triangulated_true0[:, 0] -
p_triangulated_true0[:, 1])
distance_sampled = nps.mag(p_triangulated_sampled0[:, 1, :] -
p_triangulated_sampled0[:, 0, :])
def _compose_r(r0, r1, assume_r0_tiny=False, get_gradients=False):
r'''Axis/angle rotation composition
THIS IS TEMPORARY. WILL BE REDONE IN C, WITH DOCS AND TESTS
Described here:
Altmann, Simon L. "Hamilton, Rodrigues, and the Quaternion Scandal."
Mathematics Magazine, vol. 62, no. 5, 1989, pp. 291–308
Available here:
https://www.jstor.org/stable/2689481
I use Equations (19) and (20) on page 302 of this paper. These equations say
that
R(angle=gamma, axis=n) =
compose( R(angle=alpha, axis=l), R(angle=beta, axis=m) )
I need to compute gamma*n, and these are given as solutions to:
cos(gamma/2) =
cos(alpha/2)*cos(beta/2) -
sin(alpha/2)*sin(beta/2) * inner(l,m)
sin(gamma/2) n =
sin(alpha/2)*cos(beta/2)*l +
cos(alpha/2)*sin(beta/2)*m +
sin(alpha/2)*sin(beta/2) * cross(l,m)
For nicer notation, I define
A = alpha/2
B = beta /2
C = gamma/2
l = r0 /(2A)
m = r1 /(2B)
n = r01/(2C)
I rewrite:
cos(C) =
cos(A)*cos(B) -
sin(A)*sin(B) * inner(r0,r1) / 4AB
sin(C) r01 / 2C =
sin(A)*cos(B)*r0 / 2A +
cos(A)*sin(B)*r1 / 2B +
sin(A)*sin(B) * cross(r0,r1) / 4AB
If alpha ~ 0, I have A ~ 0, and I can simplify:
cos(C) ~
cos(B) -
A*sin(B) * inner(r0,r1) / 4AB
sin(C) r01 / 2C ~
A*cos(B)* r0 / 2A +
sin(B) * r1 / 2B +
A*sin(B) * cross(r0,r1) / 4AB
I have C = B + dB where dB ~ 0, so
cos(C) ~ cos(B + dB) ~ cos(B) - dB sin(B)
-> dB = A * inner(r0,r1) / 4AB = inner(r0,r1) / 4B
-> C = B + inner(r0,r1) / 4B
Now let's look at the axis expression. Assuming A ~ 0
sin(C) r01 / 2C ~
A*cos(B) r0 / 2A +
sin(B) r1 / 2B +
A*sin(B) * cross(r0,r1) / 4AB
->
sin(C)/C * r01 ~
cos(B) r0 +
sin(B) r1 / B +
sin(B) * cross(r0,r1) / 2B
I linearize the left-hand side:
sin(C)/C = sin(B+dB)/(B+dB) ~
~ sin(B)/B + d( sin(B)/B )/de dB =
= sin(B)/B + dB (B cos(B) - sin(B)) / B^2
So
(sin(B)/B + dB (B cos(B) - sin(B)) / B^2) r01 ~
cos(B) r0 +
sin(B) r1 / B +
sin(B) * cross(r0,r1) / 2B
->
(sin(B) + dB (B cos(B) - sin(B)) / B) r01 ~
sin(B) r1 +
cos(B)*B r0 +
sin(B) * cross(r0,r1) / 2
I want to find the perturbation to give me r01 ~ r1 + deltar ->
( dB (B cos(B) - sin(B)) / B) r1 + (sin(B) + dB (B cos(B) - sin(B)) / B) deltar ~
cos(B)*B r0 +
sin(B) * cross(r0,r1) / 2
All terms here are linear or quadratic in r0. For tiny r0, I can ignore the
quadratic terms:
( dB (B cos(B) - sin(B)) / B) r1 + sin(B) deltar ~
cos(B)*B r0 +
sin(B) * cross(r0,r1) / 2
I solve for deltar:
deltar ~
cos(B)/sin(B)*B r0 +
cross(r0,r1) / 2 -
( dB (B cos(B)/sin(B) - 1) / B) r1
I substitute in the dB from above, and I simplify:
deltar ~
B/tan(B) r0 +
cross(r0,r1) / 2 -
( inner(r0,r1) / 4B * (1/tan(B) - 1/B)) r1
And I differentiate:
dr01/dr0 = ddeltar/dr0 =
B/tan(B) I +
-skew_symmetric(r1) / 2 -
outer(r1,r1) / 4B * (1/tan(B) - 1/B)
'''
if not assume_r0_tiny:
if get_gradients:
raise Exception("Not implemented")
A = nps.mag(r0) / 2
B = nps.mag(r1) / 2
cos_C = np.cos(A) * np.cos(B) - np.sin(A) * np.sin(B) * nps.inner(
r0, r1) / (4 * A * B)
sin_C = np.sqrt(1. - cos_C * cos_C)
th01 = np.arctan2(sin_C, cos_C) * 2.
sin_C__a01 = np.sin(A) * np.cos(B) * r0 / (
2 * A) + np.cos(A) * np.sin(B) * r1 / (
2 * B) + np.sin(A) * np.sin(B) * np.cross(r0, r1) / (4 * A * B)
a01 = sin_C__a01 / sin_C
return a01 * th01
B = nps.mag(r1) / 2
if nps.norm2(r0) != 0:
# for broadcasting
if isinstance(B, np.ndarray): Bs = nps.dummy(B, -1)
else: Bs = B
r01 = r1 + \
Bs/np.tan(Bs) * r0 + \
np.cross(r0,r1) / 2 - \
( nps.inner(r0,r1) / (4*Bs) * (1/np.tan(Bs) - 1/Bs))* r1
else:
r01 = r1
if not get_gradients:
return r01
# for broadcasting
if isinstance(B, np.ndarray): Bs = nps.dummy(B, -1, -1)
else: Bs = B
dr01_dr0 = \
Bs/np.tan(Bs) * np.eye(3) + \
-mrcal.utils._skew_symmetric(r1) / 2 - \
nps.outer(r1,r1) / (4*Bs) * (1/np.tan(Bs) - 1/Bs)
return r01, dr01_dr0
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)
nps.transpose( nps.clump(dp_triangulated_dq[ipt], n=-2) ) )
range0 = nps.mag(p_triangulated[0])
range0_true = nps.mag(p_triangulated_true[0])
range0_sampled = nps.mag(p_triangulated_sampled[:,0,:])
Mean_range0_sampled = nps.mag(range0_sampled).mean()
Var_range0_sampled = nps.mag(range0_sampled).var()
# r = np.mag(p)
# dr_dp = p/r
# Var(r) = dr_dp var(p) dr_dpT
# = p var(p) pT / norm2(p)
Var_range0 = nps.matmult(p_triangulated[0],
Var_p0p1_triangulated[:3,:3],
nps.transpose(p_triangulated[0]))[0] / nps.norm2(p_triangulated[0])
diff = p_triangulated[1] - p_triangulated[0]
distance = nps.mag(diff)
distance_true = nps.mag(p_triangulated_true[:,0] - p_triangulated_true[:,1])
distance_sampled = nps.mag(p_triangulated_sampled[:,1,:] - p_triangulated_sampled[:,0,:])
Mean_distance_sampled = nps.mag(distance_sampled).mean()
Var_distance_sampled = nps.mag(distance_sampled).var()
# diff = p1-p0
# dist = np.mag(diff)
# ddist_dp01 = [-diff diff] / dist
# Var(dist) = ddist_dp01 var(p01) ddist_dp01T
# = [-diff diff] var(p01) [-diff diff]T / norm2(diff)
Var_distance = nps.matmult(nps.glue( -diff, diff, axis=-1),
Var_p0p1_triangulated,
gp.plot(
nps.cat(
dx_predicted,
dx_observed,
),
_with='lines',
legend=np.arange(2),
_set=mrcal.plotoptions_measurement_boundaries(**optimization_inputs),
wait=1)
###########################################################################
# We're supposed to be at the optimum. E = norm2(x) ~ norm2(x0 + J dp) =
# norm2(x0) + 2 dpt Jt x0 + norm2(J dp). At the optimum Jt x0 = 0 -> E =
# norm2(x0) + norm2(J dp). dE = norm2(J dp) = norm2(dx_predicted)
x_predicted = x0 + dx_predicted
dE = nps.norm2(x1) - nps.norm2(x0)
dE_predicted = nps.norm2(dx_predicted)
testutils.confirm_equal(dE_predicted,
dE,
eps=1e-3,
relative=True,
msg="diff(E) predicted")
# At the optimum dE/dp = 0 -> xtJ = 0
xtJ0 = nps.inner(nps.transpose(J0), x0)
mrcal.pack_state(xtJ0, **optimization_inputs)
testutils.confirm_equal(xtJ0,
0,
eps=1.5e-2,
worstcase=True,
msg="dE/dp = 0 at the optimum: original")
def confirm_equal(x,
xref,
msg='',
eps=1e-6,
relative=False,
worstcase=False,
percentile=None):
r'''If x is equal to xref, report test success.
msg identifies this check. eps sets the RMS equality tolerance. The x,xref
arguments can be given as many different types. This function tries to do
the right thing.
if relative: I look at a relative error:
err = (a-b) / ((abs(a)+abs(b))/2 + eps)
else: I look at absolute error:
err = a-b
if worstcase: I look at the worst-case error
error = np.max(np.abs(err))
elif percentile is not None: I look at the given point in the error distribution
error = np.percentile(np.abs(err), percentile)
else: RMS error
error = np.sqrt(nps.norm2(err) / len(err))
'''
global Nchecks
global NchecksFailed
Nchecks = Nchecks + 1
# strip all trailing whitespace in each line, in case these are strings
if isinstance(x, str):
x = re.sub('[ \t]+(\n|$)', '\\1', x)
if isinstance(xref, str):
xref = re.sub('[ \t]+(\n|$)', '\\1', xref)
# convert data to numpy if possible
try:
xref = np.array(xref)
except:
pass
try:
x = np.array(x)
except:
pass
try: # flatten array if possible
x = x.ravel()
xref = xref.ravel()
except:
pass
try:
N = x.shape[0]
except:
N = 1
try:
Nref = xref.shape[0]
except:
Nref = 1
if N != Nref:
# Comparing an array to a scalar reference is allowed
if Nref == 1:
xref = np.ones((N, ), dtype=float) * xref
Nref = N
else:
print_red((
"FAILED{}: mismatched array sizes: N = {} but Nref = {}. Arrays: \n"
+ "x = {}\n" + "xref = {}").format((': ' + msg) if msg else '',
N, Nref, x, xref))
NchecksFailed = NchecksFailed + 1
return False
if N != 0:
try: # I I can subtract, get the error that way
if relative:
diff = relative_diff(x, xref)
else:
diff = x - xref
if worstcase:
what = 'worst-case'
err = np.max(np.abs(diff))
elif percentile is not None:
what = f'{percentile}%-percentile'
err = np.percentile(np.abs(diff),
percentile,
interpolation='higher')
else:
what = 'RMS'
err = np.sqrt(nps.norm2(diff) / len(diff))
if not np.all(np.isfinite(err)):
print_red(
f"FAILED{(': ' + msg) if msg else ''}: Some comparison results are NaN or Inf. {what}. error_x_xref =\n{nps.cat(err,x,xref)}"
)
NchecksFailed = NchecksFailed + 1
return False
if err > eps:
print_red(
f"FAILED{(': ' + msg) if msg else ''}: {what} error = {err}. x_xref_err =\n{nps.cat(x,xref,diff)}"
)
NchecksFailed = NchecksFailed + 1
return False
except: # Can't subtract. Do == instead
if not np.array_equal(x, xref):
print_red(
f"FAILED{(': ' + msg) if msg else ''}: x_xref =\n{nps.cat(x,xref)}"
)
NchecksFailed = NchecksFailed + 1
return False
print_green("OK" + (': ' + msg) if msg else '')
return True
def calibration_baseline(model,
Ncameras,
Nframes,
extra_observation_at,
object_width_n,
object_height_n,
object_spacing,
extrinsics_rt_fromref_true,
calobject_warp_true,
fixedframes,
testdir,
cull_left_of_center=False,
allow_nonidentity_cam0_transform=False,
range_to_boards=4.0):
r'''Compute a calibration baseline as a starting point for experiments
This is a perfect, noiseless solve. Regularization IS enabled, and the returned
model is at the optimization optimum. So the returned models will not sit
exactly at the ground-truth.
NOTE: if not fixedframes: the ref frame in the returned
optimization_inputs_baseline is NOT the ref frame used by the returned
extrinsics and frames arrays. The arrays in optimization_inputs_baseline had to
be transformed to reference off camera 0. If the extrinsics of camera 0 are the
identity, then the two ref coord systems are the same. To avoid accidental bugs,
we have a kwarg allow_nonidentity_cam0_transform, which defaults to False. if
not allow_nonidentity_cam0_transform and norm(extrinsics_rt_fromref_true[0]) >
0: raise
This logic is here purely for safety. A caller that handles non-identity cam0
transforms has to explicitly say that
ARGUMENTS
- model: string. 'opencv4' or 'opencv8' or 'splined'
- ...
'''
if re.match('opencv', model):
models_true = (
mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.opencv8.cameramodel"))
if model == 'opencv4':
# I have opencv8 models_true, but I truncate to opencv4 models_true
for m in models_true:
m.intrinsics(intrinsics=('LENSMODEL_OPENCV4',
m.intrinsics()[1][:8]))
elif model == 'splined':
models_true = (
mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.splined.cameramodel"))
else:
raise Exception("Unknown lens being tested")
models_true = models_true[:Ncameras]
lensmodel = models_true[0].intrinsics()[0]
Nintrinsics = mrcal.lensmodel_num_params(lensmodel)
for i in range(Ncameras):
models_true[i].extrinsics_rt_fromref(extrinsics_rt_fromref_true[i])
if not allow_nonidentity_cam0_transform and \
nps.norm2(extrinsics_rt_fromref_true[0]) > 0:
raise Exception(
"A non-identity cam0 transform was given, but the caller didn't explicitly say that they support this"
)
imagersizes = nps.cat(*[m.imagersize() for m in models_true])
# These are perfect
intrinsics_true = nps.cat(*[m.intrinsics()[1] for m in models_true])
extrinsics_true_mounted = nps.cat(
*[m.extrinsics_rt_fromref() for m in models_true])
x_center = -(Ncameras - 1) / 2.
# shapes (Nframes, Ncameras, Nh, Nw, 2),
# (Nframes, 4,3)
q_true,Rt_ref_board_true = \
mrcal.synthesize_board_observations(models_true,
object_width_n, object_height_n, object_spacing,
calobject_warp_true,
np.array((0., 0., 0., x_center, 0, range_to_boards)),
np.array((np.pi/180.*30., np.pi/180.*30., np.pi/180.*20., 2.5, 2.5, range_to_boards/2.0)),
Nframes)
if extra_observation_at is not None:
q_true_extra,Rt_ref_board_true_extra = \
mrcal.synthesize_board_observations(models_true,
object_width_n, object_height_n, object_spacing,
calobject_warp_true,
np.array((0., 0., 0., x_center, 0, extra_observation_at)),
np.array((np.pi/180.*30., np.pi/180.*30., np.pi/180.*20., 2.5, 2.5, extra_observation_at/10.0)),
Nframes = 1)
q_true = nps.glue(q_true, q_true_extra, axis=-5)
Rt_ref_board_true = nps.glue(Rt_ref_board_true,
Rt_ref_board_true_extra,
axis=-3)
Nframes += 1
frames_true = mrcal.rt_from_Rt(Rt_ref_board_true)
############# I have perfect observations in q_true.
# weight has shape (Nframes, Ncameras, Nh, Nw),
weight01 = (np.random.rand(*q_true.shape[:-1]) + 1.) / 2. # in [0,1]
weight0 = 0.2
weight1 = 1.0
weight = weight0 + (weight1 - weight0) * weight01
if cull_left_of_center:
imagersize = models_true[0].imagersize()
for m in models_true[1:]:
if np.any(m.imagersize() - imagersize):
raise Exception(
"I'm assuming all cameras have the same imager size, but this is false"
)
weight[q_true[..., 0] < imagersize[0] / 2.] /= 1000.
# I want observations of shape (Nframes*Ncameras, Nh, Nw, 3) where each row is
# (x,y,weight)
observations_true = nps.clump(nps.glue(q_true,
nps.dummy(weight, -1),
axis=-1),
n=2)
# Dense observations. All the cameras see all the boards
indices_frame_camera = np.zeros((Nframes * Ncameras, 2), dtype=np.int32)
indices_frame = indices_frame_camera[:, 0].reshape(Nframes, Ncameras)
indices_frame.setfield(nps.outer(np.arange(Nframes, dtype=np.int32),
np.ones((Ncameras, ), dtype=np.int32)),
dtype=np.int32)
indices_camera = indices_frame_camera[:, 1].reshape(Nframes, Ncameras)
indices_camera.setfield(nps.outer(np.ones((Nframes, ), dtype=np.int32),
np.arange(Ncameras, dtype=np.int32)),
dtype=np.int32)
indices_frame_camintrinsics_camextrinsics = \
nps.glue(indices_frame_camera,
indices_frame_camera[:,(1,)],
axis=-1)
if not fixedframes:
indices_frame_camintrinsics_camextrinsics[:, 2] -= 1
###########################################################################
# p = mrcal.show_geometry(models_true,
# frames = frames_true,
# object_width_n = object_width_n,
# object_height_n = object_height_n,
# object_spacing = object_spacing)
# sys.exit()
# I now reoptimize the perfect-observations problem. Without regularization,
# this is a no-op: I'm already at the optimum. With regularization, this will
# move us a certain amount (that the test will evaluate). Then I look at
# noise-induced motions off this optimization optimum
optimization_inputs_baseline = \
dict( intrinsics = copy.deepcopy(intrinsics_true),
points = None,
observations_board = observations_true,
indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics,
observations_point = None,
indices_point_camintrinsics_camextrinsics = None,
lensmodel = lensmodel,
calobject_warp = copy.deepcopy(calobject_warp_true),
imagersizes = imagersizes,
calibration_object_spacing = object_spacing,
verbose = False,
do_optimize_frames = not fixedframes,
do_optimize_intrinsics_core = False if model =='splined' else True,
do_optimize_intrinsics_distortions = True,
do_optimize_extrinsics = True,
do_optimize_calobject_warp = True,
do_apply_regularization = True,
do_apply_outlier_rejection = False)
if fixedframes:
# Frames are fixed: each camera has an independent pose
optimization_inputs_baseline['extrinsics_rt_fromref'] = \
copy.deepcopy(extrinsics_true_mounted)
optimization_inputs_baseline['frames_rt_toref'] = copy.deepcopy(
frames_true)
else:
# Frames are NOT fixed: cam0 is fixed as the reference coord system. I
# transform each optimization extrinsics vector to be relative to cam0
optimization_inputs_baseline['extrinsics_rt_fromref'] = \
mrcal.compose_rt(extrinsics_true_mounted[1:,:],
mrcal.invert_rt(extrinsics_true_mounted[0,:]))
optimization_inputs_baseline['frames_rt_toref'] = \
mrcal.compose_rt(extrinsics_true_mounted[0,:], frames_true)
mrcal.optimize(**optimization_inputs_baseline)
models_baseline = \
[ mrcal.cameramodel( optimization_inputs = optimization_inputs_baseline,
icam_intrinsics = i) \
for i in range(Ncameras) ]
return \
optimization_inputs_baseline, \
models_true, models_baseline, \
indices_frame_camintrinsics_camextrinsics, \
lensmodel, Nintrinsics, imagersizes, \
intrinsics_true, extrinsics_true_mounted, frames_true, \
observations_true, \
Nframes
v0_rect = mrcal.unproject_latlon(np.array((az0, el0)))
# already normalized
testutils.confirm_equal( v0_rect, v0,
msg=f'vanilla stereo: az0,el0 represent the same point ({lensmodel})')
else:
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,
def callback_l2_geometric(p, v0, v1, t01):
if p[2] < 0: return 1e6
distance_p_v0 = nps.mag(p - nps.inner(p, v0) / nps.norm2(v0) * v0)
distance_p_v1 = nps.mag(p - t01 -
nps.inner(p - t01, v1) / nps.norm2(v1) * v1)
return np.abs(distance_p_v0) + np.abs(distance_p_v1)
