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)
# I do this:
# load file
# compare with hardcoded
# save
# load again
# compare with hardcoded
#
# modify with setter
# call getter and compare
m = mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel")
testutils.confirm_equal(m.extrinsics_rt_fromref(), [
2e-2,
-3e-1,
-1e-2,
1.,
2,
-3.,
])
testutils.confirm_equal(m.intrinsics()[0], 'LENSMODEL_OPENCV8')
testutils.confirm_equal(m.intrinsics()[1], [
1761.181055,
1761.250444,
1965.706996,
1087.518797,
-0.01266096516,
0.03590794372,
-0.0002547045941,
0.0005275929652,
0.01968883397,
0.01482863541,
indptr = np.array([0, 2, 3, 6, 8])
indices = np.array([0, 2, 2, 0, 1, 2, 1, 2])
data = np.array([1, 2, 3, 4, 5, 6, 7, 8], dtype=float)
Jsparse = csr_matrix((data, indices, indptr))
Jdense = Jsparse.toarray()
Jdense_ref = \
np.array(((1, 0, 2),
(0, 0, 3),
(4, 5, 6),
(0, 7, 8)), dtype=float)
testutils.confirm_equal(Jdense,
Jdense_ref,
relative = True,
worstcase = True,
eps = 1e-6,
msg = "csr_matrix representation works as expected")
bt = np.array(((1., 5., 3.), (2., -2., -8)))
F = mrcal.CHOLMOD_factorization(Jsparse)
xt = F.solve_xt_JtJ_bt(bt)
JtJ = nps.matmult(nps.transpose(Jdense), Jdense)
xt_ref = nps.transpose(np.linalg.solve(JtJ, nps.transpose(bt)))
testutils.confirm_equal(xt, xt_ref,
relative = True,
worstcase = True,
eps = 1e-6,
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)
testutils.confirm_equal(p_triangulated0,
p_triangulated_true0,
eps=1e-6,
msg="Noiseless triangulation should be perfect")
p_triangulated0 = \
triangulate_nograd(models_baseline[icam0].intrinsics()[1],
models_baseline[icam1].intrinsics()[1],
models_baseline[icam0].extrinsics_rt_fromref(),
models_baseline[icam0].extrinsics_rt_fromref(),
models_baseline[icam1].extrinsics_rt_fromref(),
baseline_rt_ref_frame, baseline_rt_ref_frame,
q_true,
lensmodel,
stabilize_coords = args.stabilize_coords)
testutils.confirm_equal(
p_triangulated0,
import mrcal
import testutils
model_opencv8 = mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel")
model_splined = mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel")
gridn_width = 50
########## Compare the model to itself. I should get 0 diff and identity transform
difflen, diff, q0, implied_Rt10 = \
mrcal.projection_diff( (model_splined,model_splined),
gridn_width = gridn_width,
distance = None,
use_uncertainties = False )
testutils.confirm_equal( difflen.shape[1], gridn_width,
msg = "Expected number of columns" )
testutils.confirm_equal( difflen.shape[0], int(round( model_splined.imagersize()[1] / model_splined.imagersize()[0] * gridn_width)),
msg = "Expected number of rows" )
icenter = np.array(difflen.shape) // 2
testutils.confirm_equal( difflen*0, difflen,
eps = 0.05,
worstcase = True,
relative = False,
msg = "diff(model,model) at infinity should be 0")
testutils.confirm_equal( 0, np.arccos((np.trace(implied_Rt10[:3,:]) - 1) / 2.) * 180./np.pi,
eps = 0.01,
msg = "diff(model,model) at infinity should produce a rotation of 0 deg")
# intrinsics. The test-gradients tool does this much more thoroughly
optimization_inputs = copy.deepcopy(baseline)
dp_packed = np.random.randn(len(p0)) * 1e-9
mrcal.ingest_packed_state(p0 + dp_packed, **optimization_inputs)
x1 = mrcal.optimizer_callback(no_factorization=True,
no_jacobian=True,
**optimization_inputs)[1]
dx_observed = x1 - x0
dx_predicted = nps.inner(J0, dp_packed)
testutils.confirm_equal(dx_predicted,
dx_observed,
eps=1e-1,
worstcase=True,
relative=True,
msg="Trivial, sanity-checking gradient check")
if 0:
import gnuplotlib as gp
gp.plot(
nps.cat(
dx_predicted,
dx_observed,
),
_with='lines',
legend=np.arange(2),
_set=mrcal.plotoptions_measurement_boundaries(**optimization_inputs),
wait=1)
calobject_warp = np.array((1e-3, 2e-3)),
imagersizes = imagersizes,
calibration_object_spacing = 0.1,
point_min_range = 1.0,
point_max_range = 1000.0,
verbose = False,
**kwargs )
x, J = mrcal.optimizer_callback(**optimization_inputs)[1:3]
J = J.toarray()
# let's make sure that pack and unpack work correctly
J2 = J.copy()
mrcal.pack_state(J2, **optimization_inputs)
mrcal.unpack_state(J2, **optimization_inputs)
testutils.confirm_equal(J2, J, "unpack(pack(J)) = J")
J2 = J.copy()
mrcal.unpack_state(J2, **optimization_inputs)
mrcal.pack_state(J2, **optimization_inputs)
testutils.confirm_equal(J2, J, "pack(unpack(J)) = J")
# I compare full-state J so that I can change SCALE_... without breaking the
# test
mrcal.pack_state(J, **optimization_inputs)
if store_current_output_as_reference:
np.save(f"{testdir}/data/test-optimizer-callback-ref-x-{itest}.npy", x)
np.save(f"{testdir}/data/test-optimizer-callback-ref-J-{itest}.npy", J)
else:
x_ref = np.load(
f"{testdir}/data/test-optimizer-callback-ref-x-{itest}.npy")
el0 = 0.
try:
mrcal.stereo._validate_models_rectified(models_rectified)
testutils.confirm(True,
msg=f'Generated models pass validation ({lensmodel})')
except:
testutils.confirm(False,
msg=f'Generated models pass validation ({lensmodel})')
Rt_cam0_rect = mrcal.compose_Rt( model0.extrinsics_Rt_fromref(),
models_rectified[0].extrinsics_Rt_toref())
Rt01_rectified = mrcal.compose_Rt( models_rectified[0].extrinsics_Rt_fromref(),
models_rectified[1].extrinsics_Rt_toref())
testutils.confirm_equal(models_rectified[0].intrinsics()[0], lensmodel,
msg=f'model0 has the right lensmodel ({lensmodel})')
testutils.confirm_equal(models_rectified[1].intrinsics()[0], lensmodel,
msg=f'model1 has the right lensmodel ({lensmodel})')
testutils.confirm_equal(Rt_cam0_rect, mrcal.identity_Rt(),
msg=f'vanilla stereo has a vanilla geometry ({lensmodel})')
testutils.confirm_equal( Rt01_rectified[3,0],
nps.mag(rt01[3:]),
msg=f'vanilla stereo: baseline ({lensmodel})')
Naz,Nel = models_rectified[0].imagersize()
q0 = np.array(((Naz-1.)/2., (Nel-1.)/2.))
v0 = mrcal.unproject(q0, *models_rectified[0].intrinsics(), normalize=True)
# Basic test. Combine intrinsics and extrinsics without fitting any extra
# transform
out = subprocess.check_output( (f"{testdir}/../mrcal-graft-models",
filename0, filename1),
encoding = 'ascii',
stderr = subprocess.DEVNULL)
filename01 = f"{workdir}/model01.cameramodel"
with open(filename01, "w") as f:
print(out, file=f)
model01 = mrcal.cameramodel(filename01)
testutils.confirm_equal(model01.intrinsics()[1], model0.intrinsics()[1],
msg = f"Basic grafted intrinsics match",
eps = 1.0e-6)
testutils.confirm_equal(model01.extrinsics_rt_fromref(), model1.extrinsics_rt_fromref(),
msg = f"Basic grafted extrinsics match",
eps = 1.0e-6)
# More complicated test. I want to compensate for the different intrinsics with
# modified extrinsics such that the old-intrinsics and new-intrinsics project
# world points to the same place
out = subprocess.check_output( (f"{testdir}/../mrcal-graft-models",
'--radius', '-1',
filename0, filename1),
encoding = 'ascii',
stderr = subprocess.DEVNULL)
import os
testdir = os.path.dirname(os.path.realpath(__file__))
# I import the LOCAL mrcal since that's what I'm testing
sys.path[:0] = f"{testdir}/..",
import mrcal
import testutils
m = mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel")
W,H = m.imagersize()
intrinsics_core = m.intrinsics()[1][:4]
testutils.confirm_equal( mrcal.scale_focal__best_pinhole_fit(m, None),
1.0,
msg = 'scale_focal__best_pinhole_fit')
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
rt01 = np.array((0, 0, 0, 3.0, 0, 0))
model1.extrinsics_rt_toref(mrcal.compose_rt(model0.extrinsics_rt_toref(),
rt01))
az_fov_deg = 90
el_fov_deg = 50
rectification_maps,cookie = \
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.,
[0.00063803, 0.00024423, 0.00010871],
[0.00004966, 0.00053377, 0.00018905],
[0.00007708, 0.00023529, 0.0002229],
[0.00090558, 0.00072379, 0.00004062],
[0.00072059, 0.00074467, 0.00044128],
[0.00024228, 0.00058201, 0.00041458],
[0.00018121, 0.00078172, 0.00016128],
[0.00019021, 0.00001371, 0.00096808]])
Tp = nps.matmult(p, nps.transpose(R)) + t
Rt_fit = \
mrcal.align_procrustes_points_Rt01(Tp + noise,
p)
R_fit = Rt_fit[:3, :]
t_fit = Rt_fit[3, :]
testutils.confirm_equal(R_fit, R, eps=1e-2, msg='Procrustes fit R')
testutils.confirm_equal(t_fit, t, eps=1e-2, msg='Procrustes fit t')
R_fit_vectors = \
mrcal.align_procrustes_vectors_R01(nps.matmult( p, nps.transpose(R) ) + noise,
p)
testutils.confirm_equal(R_fit_vectors,
R,
eps=1e-2,
msg='Procrustes fit R (vectors)')
testutils.confirm_equal(mrcal.invert_Rt(
mrcal.Rt_from_rt(mrcal.invert_rt(mrcal.rt_from_Rt(Rt)))),
Rt,
msg='Rt/rt and invert')
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")
testutils.confirm_equal(
Var_p0p1_calibration_big.shape,
(Npoints, Nmodelpairs, 3, Npoints, Nmodelpairs, 3),
msg="Big covariance (calibration) matrix has the right shape")
testutils.confirm_equal(
Var_p0p1_observation_big.shape, (Npoints, Nmodelpairs, 3, 3),
msg="Big covariance (observation) matrix has the right shape")
testutils.confirm_equal(
Var_p0p1_joint_big.shape,
(Npoints, Nmodelpairs, 3, Npoints, Nmodelpairs, 3),
msg="Big covariance (joint) matrix has the right shape")
# Now I check each block in the diagonal individually
for ipt in range(Npoints):
drtr1_drt1r, \
drt01_drt0r, drt01_drtr1, \
dvlocal0_dintrinsics0, dvlocal1_dintrinsics1, \
dv1_dr01, dv1_dvlocal1, \
dp_triangulated_dv0, dp_triangulated_dv1, dp_triangulated_dt01, \
dp_triangulated_drtrf, \
dp_triangulated_dq = \
triangulate_grad([m.intrinsics()[1] for m in models_baseline],
[m.extrinsics_rt_fromref() for m in models_baseline],
optimization_inputs_baseline['frames_rt_toref'], frames_true,
q_true,
lensmodel,
stabilize_coords = args.stabilize_coords)
testutils.confirm_equal(p_triangulated, p_triangulated_true,
worstcase = True,
eps = 1.0,
msg = "Re-optimized triangulation should be close to the reference. This checks the regularization bias")
# I have q0,i0 -> v0
# q1,i1 -> vlocal1
# vlocal1,r0r,r1r -> v1
# r0r,r1r,t0r,t1r -> t01
# v0,v1,t01 -> p_triangulated
ppacked,x,Jpacked,factorization = mrcal.optimizer_callback(**optimization_inputs_baseline)
Nstate = len(ppacked)
# I store dp_triangulated_dp initialy, without worrying about the "packed" part.
# I'll scale the thing when done to pack it
dp_triangulated_dpstate = np.zeros((Npoints,3,Nstate), dtype=float)
istate_i0 = mrcal.state_index_intrinsics(0, **optimization_inputs_baseline)
# Solve this thing incrementally
optimization_inputs['do_optimize_intrinsics_core'] = False
optimization_inputs['do_optimize_intrinsics_distortions'] = False
optimization_inputs['do_optimize_extrinsics'] = True
optimization_inputs['do_optimize_frames'] = True
optimization_inputs['do_optimize_calobject_warp'] = False
mrcal.optimize(**optimization_inputs, do_apply_outlier_rejection=True)
optimization_inputs['do_optimize_intrinsics_core'] = True
optimization_inputs['do_optimize_intrinsics_distortions'] = False
optimization_inputs['do_optimize_extrinsics'] = True
optimization_inputs['do_optimize_frames'] = True
optimization_inputs['do_optimize_calobject_warp'] = False
mrcal.optimize(**optimization_inputs, do_apply_outlier_rejection=True)
testutils.confirm_equal(mrcal.num_states_intrinsics(**optimization_inputs),
4 * Ncameras, "num_states_intrinsics()")
testutils.confirm_equal(mrcal.num_states_extrinsics(**optimization_inputs),
6 * (Ncameras - 1), "num_states_extrinsics()")
testutils.confirm_equal(mrcal.num_states_frames(**optimization_inputs),
6 * Nframes, "num_states_frames()")
testutils.confirm_equal(mrcal.num_states_points(**optimization_inputs), 0,
"num_states_points()")
testutils.confirm_equal(mrcal.num_states_calobject_warp(**optimization_inputs),
0, "num_states_calobject_warp()")
testutils.confirm_equal(
mrcal.num_measurements_boards(**optimization_inputs),
object_width_n * object_height_n * 2 * Nframes * Ncameras,
"num_measurements_boards()")
testutils.confirm_equal(mrcal.num_measurements_points(**optimization_inputs),
0, "num_measurements_points()")
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
q2 = mrcal.project(p1, *i, get_gradients=False)
dq_dp_observed = (q2 - q1) / delta
dq_dp_reported = dq_dp[..., ivar]
def test_ref_calibration_object():
obj = mrcal.ref_calibration_object(10, 9, 5)
testutils.confirm_equal(
obj.shape, (9, 10, 3),
msg="ref_calibration_object() baseline case: shape")
testutils.confirm_equal(obj[0, 1, 0] - obj[0, 0, 0],
5,
msg="ref_calibration_object() baseline case: dx")
testutils.confirm_equal(obj[1, 0, 1] - obj[0, 0, 1],
5,
msg="ref_calibration_object() baseline case: dy")
obj = mrcal.ref_calibration_object(10, 9, (5, 6))
testutils.confirm_equal(
obj.shape, (9, 10, 3),
msg="ref_calibration_object() different x,y spacing: shape")
testutils.confirm_equal(
obj[0, 1, 0] - obj[0, 0, 0],
5,
msg="ref_calibration_object() different x,y spacing: dx")
testutils.confirm_equal(
obj[1, 0, 1] - obj[0, 0, 1],
6,
msg="ref_calibration_object() different x,y spacing: dy")
obj = mrcal.ref_calibration_object(10, 9, np.array(((5, 6), (2, 3))))
testutils.confirm_equal(
obj.shape, (2, 9, 10, 3),
msg="ref_calibration_object() different x,y spacing, broadcasted: shape"
)
testutils.confirm_equal(
obj[0, 0, 1, 0] - obj[0, 0, 0, 0],
5,
msg="ref_calibration_object() different x,y spacing, broadcasted: dx[0]"
)
testutils.confirm_equal(
obj[0, 1, 0, 1] - obj[0, 0, 0, 1],
6,
msg="ref_calibration_object() different x,y spacing, broadcasted: dy[0]"
)
testutils.confirm_equal(
obj[1, 0, 1, 0] - obj[1, 0, 0, 0],
2,
msg="ref_calibration_object() different x,y spacing, broadcasted: dx[1]"
)
testutils.confirm_equal(
obj[1, 1, 0, 1] - obj[1, 0, 0, 1],
3,
msg="ref_calibration_object() different x,y spacing, broadcasted: dy[1]"
)
obj = mrcal.ref_calibration_object(10,
9,
5,
calobject_warp=np.array((3, 4)))
testutils.confirm_equal(
obj.shape, (9, 10, 3),
msg="ref_calibration_object() one calobject_warp: shape")
obj = mrcal.ref_calibration_object(10,
9,
5,
calobject_warp=np.array(
((3, 4), (2, 5))))
testutils.confirm_equal(
obj.shape, (2, 9, 10, 3),
msg="ref_calibration_object() multiple calobject_warp: shape")
obj = mrcal.ref_calibration_object(
10,
9,
nps.dummy(np.array(((5, 6), (2, 3))), -2), # shape (2,1,2)
calobject_warp=np.array(((3, 4), (2, 5), (0.1, 0.2))))
testutils.confirm_equal(
obj.shape, (2, 3, 9, 10, 3),
msg=
"ref_calibration_object() multiple calobject_warp, x,y spacing: shape")
H10_shifted = H10.copy()
H10_shifted[0, 2] += 10.2
H10_shifted[1, 2] -= 20.4
q1_matched, diagnostics = \
mrcal.match_feature( image, image1,
templatesize,
cv2.TM_CCOEFF_NORMED,
50,
q0,
H10_shifted)
testutils.confirm_equal(
q1_matched,
mrcal.apply_homography(H10, q0),
worstcase=True,
eps=0.1,
msg=
f'match_feature(method=TM_CCOEFF_NORMED) reports the correct pixel coordinate'
)
q1_matched, diagnostics = \
mrcal.match_feature( image, image1,
templatesize,
cv2.TM_SQDIFF_NORMED,
50,
q0,
H10_shifted)
testutils.confirm_equal(
q1_matched,
mrcal.apply_homography(H10, q0),
worstcase=True,
[-5939.33490417, 1624.58376866],
[-2181.52681292, -2953.8803086]])
unproject_is_normalized = False
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.,
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.)
text_out_cahvor = \
subprocess.check_output( (f"{testdir}/../mrcal-convert-lensmodel",
"--radius", "800",
"--intrinsics-only",
"--sampled",
"--distance", "3",
"LENSMODEL_CAHVOR",
"-",),
encoding = 'ascii',
input = text_splined,
stderr = subprocess.DEVNULL)
filename_out_cahvor = f"{workdir}/cam0.out.cahvor.cameramodel"
with open(filename_out_cahvor, "w") as f:
print(text_out_cahvor, file=f)
model_out_cahvor = mrcal.cameramodel(filename_out_cahvor)
difflen, diff, q0, implied_Rt10 = \
mrcal.projection_diff( (model_out_cahvor, model_splined),
use_uncertainties = False,
distance = 3)
icenter = np.array(difflen.shape) // 2
testutils.confirm_equal(0,
difflen[icenter[0], icenter[1]],
eps=0.1,
msg="Low-enough diff at the center")
testutils.finish()
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)
[0.00063803, 0.00024423, 0.00010871],
[0.00004966, 0.00053377, 0.00018905],
[0.00007708, 0.00023529, 0.0002229],
[0.00090558, 0.00072379, 0.00004062],
[0.00072059, 0.00074467, 0.00044128],
[0.00024228, 0.00058201, 0.00041458],
[0.00018121, 0.00078172, 0.00016128],
[0.00019021, 0.00001371, 0.00096808]])
Tp = nps.matmult(p, nps.transpose(R)) + t
Rt_fit = \
mrcal.align_procrustes_points_Rt01(Tp + noise,
p)
R_fit = Rt_fit[:3, :]
t_fit = Rt_fit[3, :]
testutils.confirm_equal(R_fit, R, eps=1e-2, msg='Procrustes fit R')
testutils.confirm_equal(t_fit, t, eps=1e-2, msg='Procrustes fit t')
R_fit_vectors = \
mrcal.align_procrustes_vectors_R01(nps.matmult( p, nps.transpose(R) ) + noise,
p)
testutils.confirm_equal(R_fit_vectors,
R,
eps=1e-2,
msg='Procrustes fit R (vectors)')
testutils.confirm_equal(mrcal.invert_Rt(
mrcal.Rt_from_rt(mrcal.invert_rt(mrcal.rt_from_Rt(Rt)))),
Rt,
msg='Rt/rt and invert')
intrinsics_true,
extrinsics_true_mounted,
frames_true,
calobject_warp_true)
# I check the bias for cameras 0,1,2. Camera 3 has q0 outside of the
# observed region, so regularization affects projections there dramatically
# (it's the only contributor to the projection behavior in that area)
for icam in range(args.Ncameras):
if icam == 3:
continue
testutils.confirm_equal(
q0_true[distance][icam],
q0_baseline,
eps=0.1,
worstcase=True,
msg=
f"Regularization bias small-enough for camera {icam} at distance={'infinity' if distance is None else distance}"
)
for icam in (0, 3):
# I move the extrinsics of a model, write it to disk, and make sure the same
# uncertainties come back
if icam >= args.Ncameras: break
model_moved = mrcal.cameramodel(models_baseline[icam])
model_moved.extrinsics_rt_fromref([1., 2., 3., 4., 5., 6.])
model_moved.write(f'{workdir}/out.cameramodel')
model_read = mrcal.cameramodel(f'{workdir}/out.cameramodel')
def check_uncertainties_at(q0_baseline, idistance):
distance = args.distances[idistance]
# distance of "None" means I'll simulate a large distance, but compare
# against a special-case distance of "infinity"
if distance is None:
distance = 1e5
atinfinity = True
distancestr = "infinity"
else:
atinfinity = False
distancestr = str(distance)
# shape (Ncameras,3)
p_cam_baseline = mrcal.unproject(
q0_baseline, lensmodel, intrinsics_baseline, normalize=True) * distance
# shape (Nsamples, Ncameras, 2)
q_sampled = \
reproject_perturbed(q0_baseline,
distance,
intrinsics_baseline,
extrinsics_baseline_mounted,
frames_baseline,
calobject_warp_baseline,
intrinsics_sampled,
extrinsics_sampled_mounted,
frames_sampled,
calobject_warp_sampled)
# shape (Ncameras, 2)
q_sampled_mean = np.mean(q_sampled, axis=-3)
# shape (Ncameras, 2,2)
Var_dq_observed = np.mean(nps.outer(q_sampled - q_sampled_mean,
q_sampled - q_sampled_mean),
axis=-4)
# shape (Ncameras)
worst_direction_stdev_observed = mrcal.worst_direction_stdev(
Var_dq_observed)
# shape (Ncameras, 2,2)
Var_dq = \
nps.cat(*[ mrcal.projection_uncertainty( \
p_cam_baseline[icam],
atinfinity = atinfinity,
model = models_baseline[icam]) \
for icam in range(args.Ncameras) ])
# shape (Ncameras)
worst_direction_stdev_predicted = mrcal.worst_direction_stdev(Var_dq)
# q_sampled should be evenly distributed around q0_baseline. I can make eps
# as tight as I want by increasing Nsamples
testutils.confirm_equal(
nps.mag(q_sampled_mean - q0_baseline),
0,
eps=0.3,
worstcase=True,
msg=
f"Sampled projections cluster around the sample point at distance = {distancestr}"
)
# I accept 20% error. This is plenty good-enough. And I can get tighter matches
# if I grab more samples
testutils.confirm_equal(
worst_direction_stdev_observed,
worst_direction_stdev_predicted,
eps=0.2,
worstcase=True,
relative=True,
msg=
f"Predicted worst-case projections match sampled observations at distance = {distancestr}"
)
# I now compare the variances. The cross terms have lots of apparent error,
# but it's more meaningful to compare the eigenvectors and eigenvalues, so I
# just do that
# First, the thing is symmetric, right?
testutils.confirm_equal(
nps.transpose(Var_dq),
Var_dq,
worstcase=True,
msg=f"Var(dq) is symmetric at distance = {distancestr}")
for icam in range(args.Ncameras):
l_predicted, v = sorted_eig(Var_dq[icam])
v0_predicted = v[:, 0]
l_observed, v = sorted_eig(Var_dq_observed[icam])
v0_observed = v[:, 0]
testutils.confirm_equal(
l_observed,
l_predicted,
eps=0.35, # high error tolerance. Nsamples is too low for better
worstcase=True,
relative=True,
msg=
f"Var(dq) eigenvalues match for camera {icam} at distance = {distancestr}"
)
if icam == 3:
# I only check the eigenvectors for camera 3. The other cameras have
# isotropic covariances, so the eigenvectors aren't well defined. If
# one isn't isotropic for some reason, the eigenvalue check will
# fail
testutils.confirm_equal(
np.arcsin(nps.mag(np.cross(v0_observed, v0_predicted))) *
180. / np.pi,
0,
eps=15, # high error tolerance. Nsamples is too low for better
worstcase=True,
msg=
f"Var(dq) eigenvectors match for camera {icam} at distance = {distancestr}"
)
# I don't bother checking v1. I already made sure the matrix is
# symmetric. Thus the eigenvectors are orthogonal, so any angle offset
# in v0 will be exactly the same in v1
return q_sampled, Var_dq
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()
import numpysane as nps
import os
testdir = os.path.dirname(os.path.realpath(__file__))
# I import the LOCAL mrcal since that's what I'm testing
sys.path[:0] = f"{testdir}/..",
import mrcal
import testutils
model_splined = mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel")
ux, uy = mrcal.knots_for_splined_models(model_splined.intrinsics()[0])
testutils.confirm_equal(ux,
np.array([
-1.33234678, -1.15470054, -0.9770543, -0.79940807,
-0.62176183, -0.44411559, -0.26646936, -0.08882312,
0.08882312, 0.26646936, 0.44411559, 0.62176183,
0.79940807, 0.9770543, 1.15470054, 1.33234678
]),
msg=f"knots_for_splined_models ux")
testutils.confirm_equal(uy,
np.array([
-0.88823118, -0.71058495, -0.53293871, -0.35529247,
-0.17764624, 0., 0.17764624, 0.35529247,
0.53293871, 0.71058495, 0.88823118
]),
msg=f"knots_for_splined_models uy")
meta = mrcal.lensmodel_metadata(model_splined.intrinsics()[0])
meta_ref = {
'has_core': 1,
'can_project_behind_camera': 1,
sys.path[:0] = f"{testdir}/..",
import mrcal
import testutils
fx, fy, cx, cy = 1512., 1112, 500., 333.
# a few points, some wide, some not. Some behind the camera
p = np.array(((1.0, 2.0, 10.0), (-1.1, 0.3, -1.0), (-0.9, -1.5, -1.0)))
q_projected_ref = np.array([[649.35582325, 552.6874014],
[-5939.33490417, 1624.58376866],
[-2181.52681292, -2953.8803086]])
q_projected = mrcal.project_stereographic(p, fx, fy, cx, cy)
testutils.confirm_equal(q_projected,
q_projected_ref,
msg=f"Projecting",
eps=1e-3)
p_unprojected = mrcal.unproject_stereographic(q_projected, fx, fy, cx, cy)
cos = nps.inner(p_unprojected, p) / (nps.mag(p) * nps.mag(p_unprojected))
cos = np.clip(cos, -1, 1)
testutils.confirm_equal(np.arccos(cos),
np.zeros((p.shape[0], ), dtype=float),
msg="Unprojecting",
eps=1e-6)
# Now gradients for project()
delta = 1e-6
q_projected, dq_dp_reported = mrcal.project_stereographic(p,
fx,
fy,
pixels_per_deg_el = -1./4.)
try:
mrcal.stereo._validate_models_rectified(models_rectified)
testutils.confirm(True,
msg='Generated models pass validation')
except:
testutils.confirm(False,
msg='Generated models pass validation')
Rt_cam0_stereo = mrcal.compose_Rt( model0.extrinsics_Rt_fromref(),
models_rectified[0].extrinsics_Rt_toref())
Rt01_rectified = mrcal.compose_Rt( models_rectified[0].extrinsics_Rt_fromref(),
models_rectified[1].extrinsics_Rt_toref())
fxycxy = models_rectified[0].intrinsics()[1]
testutils.confirm_equal(Rt_cam0_stereo, mrcal.identity_Rt(),
msg='vanilla stereo has a vanilla geometry')
testutils.confirm_equal( Rt01_rectified[3,0],
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(fxycxy[0] * np.pi/180. * 8.,
(q0x-q0)[0] / 1e-6 * np.pi/180.,
msg='vanilla stereo: correct az pixel density',
eps = 0.05)
testutils.confirm_equal(fxycxy[1] * np.pi/180. * 4.,
