def calibration_sample(Nsamples, Ncameras, Nframes, Nintrinsics, optimization_inputs_baseline, observations_true, pixel_uncertainty_stdev, fixedframes): intrinsics_sampled = np.zeros((Nsamples, Ncameras, Nintrinsics), dtype=float) extrinsics_sampled_mounted = np.zeros((Nsamples, Ncameras, 6), dtype=float) frames_sampled = np.zeros((Nsamples, Nframes, 6), dtype=float) calobject_warp_sampled = np.zeros((Nsamples, 2), dtype=float) optimization_inputs_sampled = [None] * Nsamples for isample in range(Nsamples): if (isample + 1) % 20 == 0: print(f"Sampling {isample+1}/{Nsamples}") optimization_inputs_sampled[isample] = copy.deepcopy( optimization_inputs_baseline) optimization_inputs = optimization_inputs_sampled[isample] optimization_inputs['observations_board'] = \ sample_dqref(observations_true, pixel_uncertainty_stdev)[1] mrcal.optimize(**optimization_inputs) intrinsics_sampled[isample, ...] = optimization_inputs['intrinsics'] frames_sampled[isample, ...] = optimization_inputs['frames_rt_toref'] calobject_warp_sampled[isample, ...] = optimization_inputs['calobject_warp'] if fixedframes: extrinsics_sampled_mounted[ isample, ...] = optimization_inputs['extrinsics_rt_fromref'] else: # the remaining row is already 0 extrinsics_sampled_mounted[ isample, 1:, ...] = optimization_inputs['extrinsics_rt_fromref'] return \ ( intrinsics_sampled, \ extrinsics_sampled_mounted, \ frames_sampled, \ calobject_warp_sampled, optimization_inputs_sampled)
extrinsics_rt_fromref, points, observations = make_noisy_inputs() # De-noise the fixed points. We know where they are exactly. And correctly Npoints_fixed = 3 points[-Npoints_fixed:, ...] = ref_p[-Npoints_fixed:, ...] 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,
points = None, observations_board = observations_baseline, indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics, observations_point = None, indices_point_camintrinsics_camextrinsics = None, lensmodel = lensmodel, do_optimize_calobject_warp = True, calobject_warp = calobject_warp_ref, do_optimize_intrinsics_core = True, do_optimize_intrinsics_distortions = True, do_optimize_extrinsics = True, imagersizes = imagersizes, calibration_object_spacing = object_spacing, do_apply_regularization = True) mrcal.optimize(**baseline, do_apply_outlier_rejection=False) # Done setting up. I'll be looking at tiny motions off the baseline Nframes = len(frames_ref) Ncameras = len(intrinsics_ref) lensmodel = baseline['lensmodel'] Nintrinsics = mrcal.lensmodel_num_params(lensmodel) Nmeasurements_boards = mrcal.num_measurements_boards(**baseline) Nmeasurements_regularization = mrcal.num_measurements_regularization( **baseline) p0, x0, J0 = mrcal.optimizer_callback(no_factorization=True, **baseline)[:3] J0 = J0.toarray() ###########################################################################
indices_point_camintrinsics_camextrinsics = None, lensmodel = lensmodel, calobject_warp = None, imagersizes = imagersizes, calibration_object_spacing = object_spacing, verbose = False, observed_pixel_uncertainty = pixel_uncertainty_stdev, do_apply_regularization = True) # 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,
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 args.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(args.Ncameras) ] # I evaluate the projection uncertainty of this vector. In each camera. I'd like # it to be center-ish, but not AT the center. So I look at 1/3 (w,h). I want # this to represent a point in a globally-consistent coordinate system. Here I # have fixed frames, so using the reference coordinate system gives me that # consistency. Note that I look at q0 for each camera separately, so I'm going # to evaluate a different world point for each camera q0_baseline = imagersizes[0] / 3. if args.make_documentation_plots is not None:
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