def _validateIntrinsics(imagersize, i, optimization_inputs = None, icam_intrinsics = None): r'''Raises an exception if given components of the intrinsics is invalid''' # need two integers in the imager size try: N = len(imagersize) if N != 2: raise Exception("The imagersize must be an iterable of two positive integers") if imagersize[0] <= 0 or imagersize[1] <= 0: raise Exception("The imagersize must be an iterable of two positive integers") if imagersize[0] != int(imagersize[0]) or imagersize[1] != int(imagersize[1]): raise Exception("The imagersize must be an iterable of two positive integers") except: raise Exception("The imagersize must be an iterable of two positive integers") try: N = len(i) except: raise Exception("Valid intrinsics are an iterable of len 2") if N != 2: raise Exception("Valid intrinsics are an iterable of len 2") lensmodel = i[0] intrinsics = i[1] # If this fails, I keep the exception and let it fall through Nintrinsics_want = mrcal.lensmodel_num_params(lensmodel) try: Nintrinsics_have = len(intrinsics) except: raise Exception("Valid intrinsics are (lensmodel, intrinsics) where 'intrinsics' is an iterable with a length") if Nintrinsics_want != Nintrinsics_have: raise Exception("Mismatched Nintrinsics. Got {}, but model {} must have {}".format(Nintrinsics_have,lensmodel,Nintrinsics_want)) for x in intrinsics: if not isinstance(x, numbers.Number): raise Exception("All intrinsics elements should be numeric, but '{}' isn't".format(x)) if optimization_inputs is not None: # Currently this is only checked when we set the optimization_inputs by # calling intrinsics(). We do NOT check this if reading a file from # disk. This is done as an optimization: we store the unprocessed # _optimization_inputs_string bytes and only decode them as needed. # Perhaps I should expand this if not isinstance(optimization_inputs, dict): raise Exception(f"'optimization_inputs' must be a dict. Instead got type {type(optimization_inputs)}") if icam_intrinsics is None: raise Exception(f"optimization_inputs is given, so icam_intrinsics MUST be given too") if not isinstance(icam_intrinsics, int): raise Exception(f"icam_intrinsics not an int. This must be an int >= 0") if icam_intrinsics < 0: raise Exception(f"icam_intrinsics < 0. This must be an int >= 0")
def ingest_packed_state(p_packed, **optimization_inputs): r'''Read a given packed state into optimization_inputs SYNOPSIS # A simple gradient check model = mrcal.cameramodel('xxx.cameramodel') optimization_inputs = model.optimization_inputs() p0,x0,J = mrcal.optimizer_callback(no_factorization = True, **optimization_inputs)[:3] dp = np.random.randn(len(p0)) * 1e-9 mrcal.ingest_packed_state(p0 + dp, **optimization_inputs) x1 = mrcal.optimizer_callback(no_factorization = True, no_jacobian = True, **optimization_inputs)[1] dx_observed = x1 - x0 dx_predicted = nps.inner(J, dp_packed) This is the converse of mrcal.optimizer_callback(). One thing mrcal.optimizer_callback() does is to convert the expanded (intrinsics, extrinsics, ...) arrays into a 1-dimensional scaled optimization vector p_packed. mrcal.ingest_packed_state() allows updates to p_packed to be absorbed back into the (intrinsics, extrinsics, ...) arrays for further evaluation with mrcal.optimizer_callback() and others. ARGUMENTS - p_packed: a numpy array of shape (Nstate,) containing the input packed state - **optimization_inputs: a dict() of arguments passable to mrcal.optimize() and mrcal.optimizer_callback(). The arrays in this dict are updated RETURNED VALUE None ''' intrinsics = optimization_inputs.get("intrinsics") extrinsics = optimization_inputs.get("extrinsics_rt_fromref") frames = optimization_inputs.get("frames_rt_toref") points = optimization_inputs.get("points") calobject_warp = optimization_inputs.get("calobject_warp") Npoints_fixed = optimization_inputs.get('Npoints_fixed', 0) Nvars_intrinsics = mrcal.num_states_intrinsics(**optimization_inputs) Nvars_extrinsics = mrcal.num_states_extrinsics(**optimization_inputs) Nvars_frames = mrcal.num_states_frames(**optimization_inputs) Nvars_points = mrcal.num_states_points(**optimization_inputs) Nvars_calobject_warp = mrcal.num_states_calobject_warp( **optimization_inputs) Nvars_expected = \ Nvars_intrinsics + \ Nvars_extrinsics + \ Nvars_frames + \ Nvars_points + \ Nvars_calobject_warp # Defaults MUST match those in OPTIMIZER_ARGUMENTS_OPTIONAL in # mrcal-pywrap.c. Or better yet, this whole function should # come from the C code instead of being reimplemented here in Python do_optimize_intrinsics_core = optimization_inputs.get( 'do_optimize_intrinsics_core', True) do_optimize_intrinsics_distortions = optimization_inputs.get( 'do_optimize_intrinsics_distortions', True) do_optimize_extrinsics = optimization_inputs.get('do_optimize_extrinsics', True) do_optimize_frames = optimization_inputs.get('do_optimize_frames', True) do_optimize_calobject_warp = optimization_inputs.get( 'do_optimize_calobject_warp', True) if p_packed.ravel().size != Nvars_expected: raise Exception( f"Mismatched array size: p_packed.size={p_packed.ravel().size} while the optimization problem expects {Nvars_expected}" ) p = p_packed.copy() mrcal.unpack_state(p, **optimization_inputs) if do_optimize_intrinsics_core or \ do_optimize_intrinsics_distortions: ivar0 = mrcal.state_index_intrinsics(0, **optimization_inputs) if ivar0 is not None: iunpacked0, iunpacked1 = None, None # everything by default lensmodel = optimization_inputs['lensmodel'] has_core = mrcal.lensmodel_metadata_and_config( lensmodel)['has_core'] Ncore = 4 if has_core else 0 Ndistortions = mrcal.lensmodel_num_params(lensmodel) - Ncore if not do_optimize_intrinsics_core: iunpacked0 = Ncore if not do_optimize_intrinsics_distortions: iunpacked1 = -Ndistortions intrinsics[:, iunpacked0:iunpacked1].ravel()[:] = \ p[ ivar0:Nvars_intrinsics ] if do_optimize_extrinsics: ivar0 = mrcal.state_index_extrinsics(0, **optimization_inputs) if ivar0 is not None: extrinsics.ravel()[:] = p[ivar0:ivar0 + Nvars_extrinsics] if do_optimize_frames: ivar0 = mrcal.state_index_frames(0, **optimization_inputs) if ivar0 is not None: frames.ravel()[:] = p[ivar0:ivar0 + Nvars_frames] if do_optimize_frames: ivar0 = mrcal.state_index_points(0, **optimization_inputs) if ivar0 is not None: points.ravel()[:-Npoints_fixed * 3] = p[ivar0:ivar0 + Nvars_points] if do_optimize_calobject_warp: ivar0 = mrcal.state_index_calobject_warp(**optimization_inputs) if ivar0 is not None: calobject_warp.ravel()[:] = p[ivar0:ivar0 + Nvars_calobject_warp]
############# Set up my world, and compute all the perfect positions, pixel ############# observations of everything models_ref = (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")) imagersizes = nps.cat(*[m.imagersize() for m in models_ref]) lensmodel = models_ref[0].intrinsics()[0] # I have opencv8 models_ref, but let me truncate to opencv4 models_ref to keep this # simple and fast lensmodel = 'LENSMODEL_OPENCV4' for m in models_ref: m.intrinsics(intrinsics=(lensmodel, m.intrinsics()[1][:8])) Nintrinsics = mrcal.lensmodel_num_params(lensmodel) Ncameras = len(models_ref) Ncameras_extrinsics = Ncameras - 1 Nframes = 50 models_ref[0].extrinsics_rt_fromref(np.zeros((6, ), dtype=float)) models_ref[1].extrinsics_rt_fromref(np.array((0.08, 0.2, 0.02, 1., 0.9, 0.1))) models_ref[2].extrinsics_rt_fromref(np.array((0.01, 0.07, 0.2, 2.1, 0.4, 0.2))) models_ref[3].extrinsics_rt_fromref(np.array( (-0.1, 0.08, 0.08, 4.4, 0.2, 0.1))) pixel_uncertainty_stdev = 1.5 object_spacing = 0.1 object_width_n = 10
def _write(f, m, note=None): r'''Writes a cameramodel as a .cahvor to a writeable file object''' if note is not None: for l in note.splitlines(): f.write('# ' + l + '\n') d = m.imagersize() f.write('Dimensions = {} {}\n'.format(int(d[0]), int(d[1]))) lensmodel,intrinsics = m.intrinsics() if lensmodel == 'LENSMODEL_CAHVOR': f.write("Model = CAHVOR = perspective, distortion\n") elif re.match('LENSMODEL_(OPENCV.*|PINHOLE)', lensmodel): f.write("Model = CAHV = perspective, linear\n") else: match = re.match('^LENSMODEL_CAHVORE_linearity=([0-9\.]+)$', lensmodel) if match is not None: f.write("Model = CAHVORE3,{} = general\n".format(match.group(1))) else: raise Exception("Don't know how to handle lens model '{}'".format(lensmodel)) fx,fy,cx,cy = intrinsics[:4] Rt_toref = m.extrinsics_Rt_toref() R_toref = Rt_toref[:3,:] t_toref = Rt_toref[ 3,:] C = t_toref A = R_toref[:,2] Hp = R_toref[:,0] Vp = R_toref[:,1] H = fx*Hp + A*cx V = fy*Vp + A*cy f.write(("{} =" + (" {:15.10f}" * 3) + "\n").format('C', *C)) f.write(("{} =" + (" {:15.10f}" * 3) + "\n").format('A', *A)) f.write(("{} =" + (" {:15.10f}" * 3) + "\n").format('H', *H)) f.write(("{} =" + (" {:15.10f}" * 3) + "\n").format('V', *V)) if re.match('^LENSMODEL_CAHVOR', lensmodel): # CAHVOR(E) alpha,beta,R0,R1,R2 = intrinsics[4:9] s_al,c_al,s_be,c_be = np.sin(alpha),np.cos(alpha),np.sin(beta),np.cos(beta) O = nps.matmult( R_toref, nps.transpose(np.array(( s_al*c_be, s_be, c_al*c_be ), dtype=float)) ).ravel() R = np.array((R0, R1, R2), dtype=float) f.write(("{} =" + (" {:15.10f}" * 3) + "\n").format('O', *O)) f.write(("{} =" + (" {:15.10f}" * 3) + "\n").format('R', *R)) if re.match('^LENSMODEL_CAHVORE', lensmodel): E = intrinsics[9:] f.write(("{} =" + (" {:15.10f}" * 3) + "\n").format('E', *E)) elif re.match('LENSMODEL_OPENCV*', lensmodel): Ndistortions = mrcal.lensmodel_num_params(lensmodel) - 4 f.write(("{} =" + (" {:15.10f}" * Ndistortions) + "\n").format(lensmodel, *intrinsics[4:])) elif lensmodel == 'LENSMODEL_PINHOLE': # the CAHV values we already wrote are all that's needed pass else: raise Exception(f"Cannot write lens model '{lensmodel}' to a .cahvor file. I only support PINHOLE, CAHVOR(E) and OPENCV model") c = m.valid_intrinsics_region() if c is not None: f.write("VALID_INTRINSICS_REGION = ") np.savetxt(f, c.ravel(), fmt='%.2f', newline=' ') f.write('\n') Hs,Vs,Hc,Vc = intrinsics[:4] f.write("Hs = {}\n".format(Hs)) f.write("Hc = {}\n".format(Hc)) f.write("Vs = {}\n".format(Vs)) f.write("Vc = {}\n".format(Vc)) f.write("# this is hard-coded\nTheta = {} (-90.0 deg)\n".format(-np.pi/2)) return True
2.112936851,1.126412871,2.113220553,1.114991063,2.017901873,1.244588667, 2.051238803,1.201855728,2.043256406,1.216674722,2.035286046,1.178380907, 2.08028318,1.178783085,2.051214271,1.173560417,2.059298121,1.182414688, 2.094607679,1.177960959,2.086998287,1.147371259,2.12029442,1.138197348, 2.138994213, 1.114846113,]))) # a few points, some wide, some not. None behind the camera p = np.array(((1.0, 2.0, 10.0), (-1.1, 0.3, 1.0), (-0.9, -1.5, 1.0))) delta = 1e-6 for i in intrinsics: q, dq_dp, dq_di = mrcal.project(p, *i, get_gradients=True) Nintrinsics = mrcal.lensmodel_num_params(i[0]) testutils.confirm_equal(dq_di.shape[-1], Nintrinsics, msg=f"{i[0]}: Nintrinsics match for {i[0]}") if Nintrinsics != dq_di.shape[-1]: continue for ivar in range(dq_dp.shape[-1]): # center differences p1 = p.copy() p1[..., ivar] = p[..., ivar] - delta / 2 q1 = mrcal.project(p1, *i, get_gradients=False) p1[..., ivar] += delta q2 = mrcal.project(p1, *i, get_gradients=False)
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
def calibration_baseline(model, Ncameras, Nframes, extra_observation_at, pixel_uncertainty_stdev, object_width_n, object_height_n, object_spacing, extrinsics_rt_fromref_true, calobject_warp_true, fixedframes, testdir, cull_left_of_center=False): r'''Compute a calibration baseline as a starting point for experiments This is a perfect, noiseless solve. Regularization IS enabled, and the returned model is at the optimization optimum. So the returned models will not sit exactly at the ground-truth ARGUMENTS - model: string. 'opencv4' or 'opencv8' or 'splined' - ... ''' if re.match('opencv', model): models_true = ( mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel"), mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel"), mrcal.cameramodel(f"{testdir}/data/cam1.opencv8.cameramodel"), mrcal.cameramodel(f"{testdir}/data/cam1.opencv8.cameramodel")) if model == 'opencv4': # I have opencv8 models_true, but I truncate to opencv4 models_true for m in models_true: m.intrinsics(intrinsics=('LENSMODEL_OPENCV4', m.intrinsics()[1][:8])) elif model == 'splined': models_true = ( mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel"), mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel"), mrcal.cameramodel(f"{testdir}/data/cam1.splined.cameramodel"), mrcal.cameramodel(f"{testdir}/data/cam1.splined.cameramodel")) else: raise Exception("Unknown lens being tested") models_true = models_true[:Ncameras] lensmodel = models_true[0].intrinsics()[0] Nintrinsics = mrcal.lensmodel_num_params(lensmodel) for i in range(Ncameras): models_true[i].extrinsics_rt_fromref(extrinsics_rt_fromref_true[i]) imagersizes = nps.cat(*[m.imagersize() for m in models_true]) # These are perfect intrinsics_true = nps.cat(*[m.intrinsics()[1] for m in models_true]) extrinsics_true_mounted = nps.cat( *[m.extrinsics_rt_fromref() for m in models_true]) x_center = -(Ncameras - 1) / 2. # shapes (Nframes, Ncameras, Nh, Nw, 2), # (Nframes, 4,3) q_true,Rt_cam0_board_true = \ mrcal.synthesize_board_observations(models_true, object_width_n, object_height_n, object_spacing, calobject_warp_true, np.array((0., 0., 0., x_center, 0, 4.0)), np.array((np.pi/180.*30., np.pi/180.*30., np.pi/180.*20., 2.5, 2.5, 2.0)), Nframes) if extra_observation_at: c = mrcal.ref_calibration_object(object_width_n, object_height_n, object_spacing, calobject_warp_true) Rt_cam0_board_true_far = \ nps.glue( np.eye(3), np.array((0,0,extra_observation_at)), axis=-2) q_true_far = \ mrcal.project(mrcal.transform_point_Rt(Rt_cam0_board_true_far, c), *models_true[0].intrinsics()) q_true = nps.glue(q_true_far, q_true, axis=-5) Rt_cam0_board_true = nps.glue(Rt_cam0_board_true_far, Rt_cam0_board_true, axis=-3) Nframes += 1 frames_true = mrcal.rt_from_Rt(Rt_cam0_board_true) ############# I have perfect observations in q_true. I corrupt them by noise # weight has shape (Nframes, Ncameras, Nh, Nw), weight01 = (np.random.rand(*q_true.shape[:-1]) + 1.) / 2. # in [0,1] weight0 = 0.2 weight1 = 1.0 weight = weight0 + (weight1 - weight0) * weight01 if cull_left_of_center: imagersize = models_true[0].imagersize() for m in models_true[1:]: if np.any(m.imagersize() - imagersize): raise Exception( "I'm assuming all cameras have the same imager size, but this is false" ) weight[q_true[..., 0] < imagersize[0] / 2.] /= 1000. # I want observations of shape (Nframes*Ncameras, Nh, Nw, 3) where each row is # (x,y,weight) observations_true = nps.clump(nps.glue(q_true, nps.dummy(weight, -1), axis=-1), n=2) # Dense observations. All the cameras see all the boards indices_frame_camera = np.zeros((Nframes * Ncameras, 2), dtype=np.int32) indices_frame = indices_frame_camera[:, 0].reshape(Nframes, Ncameras) indices_frame.setfield(nps.outer(np.arange(Nframes, dtype=np.int32), np.ones((Ncameras, ), dtype=np.int32)), dtype=np.int32) indices_camera = indices_frame_camera[:, 1].reshape(Nframes, Ncameras) indices_camera.setfield(nps.outer(np.ones((Nframes, ), dtype=np.int32), np.arange(Ncameras, dtype=np.int32)), dtype=np.int32) indices_frame_camintrinsics_camextrinsics = \ nps.glue(indices_frame_camera, indices_frame_camera[:,(1,)], axis=-1) if not fixedframes: indices_frame_camintrinsics_camextrinsics[:, 2] -= 1 ########################################################################### # Now I apply pixel noise, and look at the effects on the resulting calibration. # p = mrcal.show_geometry(models_true, # frames = frames_true, # object_width_n = object_width_n, # object_height_n = object_height_n, # object_spacing = object_spacing) # sys.exit() # I now reoptimize the perfect-observations problem. Without regularization, # this is a no-op: I'm already at the optimum. With regularization, this will # move us a certain amount (that the test will evaluate). Then I look at # noise-induced motions off this optimization optimum optimization_inputs_baseline = \ dict( intrinsics = copy.deepcopy(intrinsics_true), extrinsics_rt_fromref = copy.deepcopy(extrinsics_true_mounted if fixedframes else extrinsics_true_mounted[1:,:]), frames_rt_toref = copy.deepcopy(frames_true), points = None, observations_board = observations_true, indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics, observations_point = None, indices_point_camintrinsics_camextrinsics = None, lensmodel = lensmodel, calobject_warp = copy.deepcopy(calobject_warp_true), imagersizes = imagersizes, calibration_object_spacing = object_spacing, verbose = False, observed_pixel_uncertainty = pixel_uncertainty_stdev, do_optimize_frames = not fixedframes, do_optimize_intrinsics_core = False if model =='splined' else True, do_optimize_intrinsics_distortions = True, do_optimize_extrinsics = True, do_optimize_calobject_warp = True, do_apply_regularization = True, do_apply_outlier_rejection = False) mrcal.optimize(**optimization_inputs_baseline) models_baseline = \ [ mrcal.cameramodel( optimization_inputs = optimization_inputs_baseline, icam_intrinsics = i) \ for i in range(Ncameras) ] return \ optimization_inputs_baseline, \ models_true, models_baseline, \ indices_frame_camintrinsics_camextrinsics, \ lensmodel, Nintrinsics, imagersizes, \ intrinsics_true, extrinsics_true_mounted, frames_true, \ observations_true, \ Nframes