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.,