def _noisy_observation_vectors_for_triangulation(p, Rt01, intrinsics0, intrinsics1, Nsamples, sigma): # p has shape (...,3) # shape (..., 2) q0 = mrcal.project(p, *intrinsics0) q1 = mrcal.project(mrcal.transform_point_Rt(mrcal.invert_Rt(Rt01), p), *intrinsics1) # shape (..., 1,2). Each has x,y q0 = nps.dummy(q0, -2) q1 = nps.dummy(q1, -2) q_noise = np.random.randn(*p.shape[:-1], Nsamples, 2, 2) * sigma # shape (..., Nsamples,2). Each has x,y q0_noise = q_noise[..., :, 0, :] q1_noise = q_noise[..., :, 1, :] q0_noisy = q0 + q0_noise q1_noisy = q1 + q1_noise # shape (..., Nsamples, 3) v0local_noisy = mrcal.unproject(q0_noisy, *intrinsics0) v1local_noisy = mrcal.unproject(q1_noisy, *intrinsics1) v0_noisy = v0local_noisy v1_noisy = mrcal.rotate_point_R(Rt01[:3, :], v1local_noisy) # All have shape (..., Nsamples,3) return \ v0local_noisy, v1local_noisy, v0_noisy,v1_noisy, \ q0,q1, q0_noisy, q1_noisy
def reproject_perturbed__diff( q, distance, # shape (Ncameras, Nintrinsics) baseline_intrinsics, # shape (Ncameras, 6) baseline_rt_cam_ref, # shape (Nframes, 6) baseline_rt_ref_frame, # shape (2) baseline_calobject_warp, # shape (Ncameras, Nintrinsics) query_intrinsics, # shape (Ncameras, 6) query_rt_cam_ref, # shape (Nframes, 6) query_rt_ref_frame, # shape (2) query_calobject_warp): r'''Reproject by using the "diff" method to compute a rotation ''' # shape (Ncameras, 3) p_cam_baseline = mrcal.unproject( q, lensmodel, baseline_intrinsics, normalize=True) * distance p_cam_query = np.zeros((args.Ncameras, 3), dtype=float) for icam in range(args.Ncameras): # This method only cares about the intrinsics model_baseline = \ mrcal.cameramodel( intrinsics = (lensmodel, baseline_intrinsics[icam]), imagersize = imagersizes[0] ) model_query = \ mrcal.cameramodel( intrinsics = (lensmodel, query_intrinsics[icam]), imagersize = imagersizes[0] ) implied_Rt10_query = \ mrcal.projection_diff( (model_baseline, model_query), distance = distance, use_uncertainties = False, focus_center = None, focus_radius = 1000.)[3] mrcal.transform_point_Rt(implied_Rt10_query, p_cam_baseline[icam], out=p_cam_query[icam]) # shape (Ncameras, 2) return \ mrcal.project( p_cam_query, lensmodel, query_intrinsics)
def check(intrinsics, p_ref, q_ref, unproject=True): q_projected = mrcal.project(p_ref, *intrinsics) testutils.confirm_equal(q_projected, q_ref, msg=f"Projecting {intrinsics[0]}", eps=1e-2) if not unproject: return v_unprojected = mrcal.unproject(q_projected, *intrinsics, normalize=True) testutils.confirm_equal(nps.norm2(v_unprojected), np.ones((p_ref.shape[0], ), dtype=float), msg=f"Unprojected v are normalized", eps=1e-6) cos = nps.inner(v_unprojected, p_ref) / nps.mag(p_ref) cos = np.clip(cos, -1, 1) testutils.confirm_equal(np.arccos(cos), np.zeros((p_ref.shape[0], ), dtype=float), msg=f"Unprojecting {intrinsics[0]}", eps=1e-6)
def reproject_perturbed__fit_boards_ref( q, distance, # shape (Ncameras, Nintrinsics) baseline_intrinsics, # shape (Ncameras, 6) baseline_rt_cam_ref, # shape (Nframes, 6) baseline_rt_ref_frame, # shape (2) baseline_calobject_warp, # shape (..., Ncameras, Nintrinsics) query_intrinsics, # shape (..., Ncameras, 6) query_rt_cam_ref, # shape (..., Nframes, 6) query_rt_ref_frame, # shape (..., 2) query_calobject_warp): r'''Reproject by explicitly computing a procrustes fit to align the reference coordinate systems of the two solves. We match up the two sets of chessboard points ''' calobject_height, calobject_width = optimization_inputs_baseline[ 'observations_board'].shape[1:3] # shape (Nsamples, Nh, Nw, 3) if query_calobject_warp.ndim > 1: calibration_object_query = \ nps.cat(*[ mrcal.ref_calibration_object(calobject_width, calobject_height, optimization_inputs_baseline['calibration_object_spacing'], calobject_warp=calobject_warp) \ for calobject_warp in query_calobject_warp] ) else: calibration_object_query = \ mrcal.ref_calibration_object(calobject_width, calobject_height, optimization_inputs_baseline['calibration_object_spacing'], calobject_warp=query_calobject_warp) # shape (Nsamples, Nframes, Nh, Nw, 3) pcorners_ref_query = \ mrcal.transform_point_rt( nps.dummy(query_rt_ref_frame, -2, -2), nps.dummy(calibration_object_query, -4)) # shape (Nh, Nw, 3) calibration_object_baseline = \ mrcal.ref_calibration_object(calobject_width, calobject_height, optimization_inputs_baseline['calibration_object_spacing'], calobject_warp=baseline_calobject_warp) # frames_ref.shape is (Nframes, 6) # shape (Nframes, Nh, Nw, 3) pcorners_ref_baseline = \ mrcal.transform_point_rt( nps.dummy(baseline_rt_ref_frame, -2, -2), calibration_object_baseline) # shape (Nsamples,4,3) Rt_refq_refb = \ mrcal.align_procrustes_points_Rt01( \ # shape (Nsamples,N,3) nps.mv(nps.clump(nps.mv(pcorners_ref_query, -1,0),n=-3),0,-1), # shape (N,3) nps.clump(pcorners_ref_baseline, n=3)) # shape (Ncameras, 3) p_cam_baseline = mrcal.unproject( q, lensmodel, baseline_intrinsics, normalize=True) * distance # shape (Ncameras, 3). In the ref coord system p_ref_baseline = \ mrcal.transform_point_rt( mrcal.invert_rt(baseline_rt_cam_ref), p_cam_baseline ) # shape (Nsamples,Ncameras,3) p_ref_query = \ mrcal.transform_point_Rt(nps.mv(Rt_refq_refb,-3,-4), p_ref_baseline) # shape (..., Ncameras, 3) p_cam_query = \ mrcal.transform_point_rt(query_rt_cam_ref, p_ref_query) # shape (..., Ncameras, 2) q1 = mrcal.project(p_cam_query, lensmodel, query_intrinsics) if q1.shape[-3] == 1: q1 = q1[0, :, :] return q1
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))
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)
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))
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))
def image_transformation_map(model_from, model_to, use_rotation = False, plane_n = None, plane_d = None, mask_valid_intrinsics_region_from = False): r'''Compute a reprojection map between two models SYNOPSIS model_orig = mrcal.cameramodel("xxx.cameramodel") image_orig = cv2.imread("image.jpg") model_pinhole = mrcal.pinhole_model_for_reprojection(model_orig, fit = "corners") mapxy = mrcal.image_transformation_map(model_orig, model_pinhole) image_undistorted = mrcal.transform_image(image_orig, mapxy) # image_undistorted is now a pinhole-reprojected version of image_orig Returns the transformation that describes a mapping - from pixel coordinates of an image of a scene observed by model_to - to pixel coordinates of an image of the same scene observed by model_from This transformation can then be applied to a whole image by calling mrcal.transform_image(). This function returns a transformation map in an (Nheight,Nwidth,2) array. The image made by model_to will have shape (Nheight,Nwidth). Each pixel (x,y) in this image corresponds to a pixel mapxy[y,x,:] in the image made by model_from. This function has 3 modes of operation: - intrinsics-only This is the default. Selected if - use_rotation = False - plane_n = None - plane_d = None All of the extrinsics are ignored. If the two cameras have the same orientation, then their observations of infinitely-far-away objects will line up exactly - rotation This can be selected explicitly with - use_rotation = True - plane_n = None - plane_d = None Here we use the rotation component of the relative extrinsics. The relative translation is impossible to use without knowing what we're looking at, so IT IS ALWAYS IGNORED. If the relative orientation in the models matches reality, then the two cameras' observations of infinitely-far-away objects will line up exactly - plane This is selected if - use_rotation = True - plane_n is not None - plane_d is not None We map observations of a given plane in camera FROM coordinates coordinates to where this plane would be observed by camera TO. This uses ALL the intrinsics, extrinsics and the plane representation. If all of these are correct, the observations of this plane would line up exactly in the remapped-camera-fromimage and the camera-to image. The plane is represented in camera-from coordinates by a normal vector plane_n, and the distance to the normal plane_d. The plane is all points p such that inner(p,plane_n) = plane_d. plane_n does not need to be normalized; any scaling is compensated in plane_d. ARGUMENTS - model_from: the mrcal.cameramodel object describing the camera used to capture the input image - model_to: the mrcal.cameramodel object describing the camera that would have captured the image we're producing - use_rotation: optional boolean, defaulting to False. If True: we respect the relative rotation in the extrinsics of the camera models. - plane_n: optional numpy array of shape (3,); None by default. If given, we produce a transformation to map observations of a given plane to the same pixels in the source and target images. This argument describes the normal vector in the coordinate system of model_from. The plane is all points p such that inner(p,plane_n) = plane_d. plane_n does not need to be normalized; any scaling is compensated in plane_d. If given, plane_d should be given also, and use_rotation should be True. if given, we use the full intrinsics and extrinsics of both camera models - plane_d: optional floating-point value; None by default. If given, we produce a transformation to map observations of a given plane to the same pixels in the source and target images. The plane is all points p such that inner(p,plane_n) = plane_d. plane_n does not need to be normalized; any scaling is compensated in plane_d. If given, plane_n should be given also, and use_rotation should be True. if given, we use the full intrinsics and extrinsics of both camera models - mask_valid_intrinsics_region_from: optional boolean defaulting to False. If True, points outside the valid-intrinsics region in the FROM image are set to black, and thus do not appear in the output image RETURNED VALUE A numpy array of shape (Nheight,Nwidth,2) where Nheight and Nwidth represent the imager dimensions of model_to. This array contains 32-bit floats, as required by cv2.remap() (the function providing the internals of mrcal.transform_image()). This array can be passed to mrcal.transform_image() ''' if (plane_n is None and plane_d is not None) or \ (plane_n is not None and plane_d is None): raise Exception("plane_n and plane_d should both be None or neither should be None") if plane_n is not None and plane_d is not None and \ not use_rotation: raise Exception("We're looking at remapping a plane (plane_d, plane_n are not None), so use_rotation should be True") Rt_to_from = None if use_rotation: Rt_to_r = model_to. extrinsics_Rt_fromref() Rt_r_from = model_from.extrinsics_Rt_toref() Rt_to_from = mrcal.compose_Rt(Rt_to_r, Rt_r_from) lensmodel_from,intrinsics_data_from = model_from.intrinsics() lensmodel_to, intrinsics_data_to = model_to. intrinsics() if re.match("LENSMODEL_OPENCV",lensmodel_from) and \ lensmodel_to == "LENSMODEL_PINHOLE" and \ plane_n is None and \ not mask_valid_intrinsics_region_from: # This is a common special case. This branch works identically to the # other path (the other side of this "if" can always be used instead), # but the opencv-specific code is optimized and at one point ran faster # than the code on the other side. # # The mask_valid_intrinsics_region_from isn't implemented in this path. # It COULD be, then this faster path could be used fxy_from = intrinsics_data_from[0:2] cxy_from = intrinsics_data_from[2:4] cameraMatrix_from = np.array(((fxy_from[0], 0, cxy_from[0]), ( 0, fxy_from[1], cxy_from[1]), ( 0, 0, 1))) fxy_to = intrinsics_data_to[0:2] cxy_to = intrinsics_data_to[2:4] cameraMatrix_to = np.array(((fxy_to[0], 0, cxy_to[0]), ( 0, fxy_to[1], cxy_to[1]), ( 0, 0, 1))) output_shape = model_to.imagersize() distortion_coeffs = intrinsics_data_from[4: ] if Rt_to_from is not None: R_to_from = Rt_to_from[:3,:] if np.trace(R_to_from) > 3. - 1e-12: R_to_from = None # identity, so I pass None else: R_to_from = None return nps.glue( *[ nps.dummy(arr,-1) for arr in \ cv2.initUndistortRectifyMap(cameraMatrix_from, distortion_coeffs, R_to_from, cameraMatrix_to, tuple(output_shape), cv2.CV_32FC1)], axis = -1) W_from,H_from = model_from.imagersize() W_to, H_to = model_to. imagersize() # shape: (Nheight,Nwidth,2). Contains (x,y) rows grid = np.ascontiguousarray(nps.mv(nps.cat(*np.meshgrid(np.arange(W_to), np.arange(H_to))), 0,-1), dtype = float) if lensmodel_to == "LENSMODEL_PINHOLE": # Faster path for the unproject. Nice, simple closed-form solution fxy_to = intrinsics_data_to[0:2] cxy_to = intrinsics_data_to[2:4] v = np.zeros( (grid.shape[0], grid.shape[1], 3), dtype=float) v[..., :2] = (grid-cxy_to)/fxy_to v[..., 2] = 1 elif lensmodel_to == "LENSMODEL_STEREOGRAPHIC": # Faster path for the unproject. Nice, simple closed-form solution v = mrcal.unproject_stereographic(grid, *intrinsics_data_to[:4]) else: v = mrcal.unproject(grid, lensmodel_to, intrinsics_data_to) if plane_n is not None: R_to_from = Rt_to_from[:3,:] t_to_from = Rt_to_from[ 3,:] # The homography definition. Derived in many places. For instance in # "Motion and structure from motion in a piecewise planar environment" # by Olivier Faugeras, F. Lustman. A_to_from = plane_d * R_to_from + nps.outer(t_to_from, plane_n) A_from_to = np.linalg.inv(A_to_from) v = nps.matmult( v, nps.transpose(A_from_to) ) else: if Rt_to_from is not None: R_to_from = Rt_to_from[:3,:] if np.trace(R_to_from) < 3. - 1e-12: # rotation isn't identity. apply v = nps.matmult(v, R_to_from) mapxy = mrcal.project( v, lensmodel_from, intrinsics_data_from ) if mask_valid_intrinsics_region_from: # Using matplotlib to compute the out-of-bounds points. It doesn't # support broadcasting, so I do that manually with a clump/reshape from matplotlib.path import Path region = Path(model_from.valid_intrinsics_region()) is_inside = region.contains_points(nps.clump(mapxy,n=2)).reshape(mapxy.shape[:2]) mapxy[ ~is_inside, :] = -1 return mapxy.astype(np.float32)
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) if lensmodel == 'LENSMODEL_LATLON': v0_rect = mrcal.unproject_latlon(np.array((az0, el0))) # already normalized testutils.confirm_equal( v0_rect, v0, msg=f'vanilla stereo: az0,el0 represent the same point ({lensmodel})') else: v0_rect = mrcal.unproject_pinhole(np.array((np.tan(az0), np.tan(el0)))) v0_rect /= nps.mag(v0_rect) testutils.confirm_equal( v0_rect, v0, msg=f'vanilla stereo: az0,el0 represent the same point ({lensmodel})', eps = 1e-3) dq0x = np.array((1e-1, 0)) dq0y = np.array((0, 1e-1))
print(f"Couldn't read '{args.model}' as a camera model", file=sys.stderr) sys.exit(1) W, H = m.imagersize() Nw = 40 Nh = 30 # shape (Nh,Nw,2) xy = \ nps.mv(nps.cat(*np.meshgrid( np.linspace(0,W-1,Nw), np.linspace(0,H-1,Nh) )), 0,-1) fxy = m.intrinsics()[1][0:2] cxy = m.intrinsics()[1][2:4] # shape (Nh,Nw,2) v = mrcal.unproject(np.ascontiguousarray(xy), *m.intrinsics()) v0 = mrcal.unproject(cxy, *m.intrinsics()) # shape (Nh,Nw) costh = nps.inner(v, v0) / (nps.mag(v) * nps.mag(v0)) th = np.arccos(costh) # shape (Nh,Nw,2) xy_rel = xy - cxy # shape (Nh,Nw) az = np.arctan2(xy_rel[..., 1], xy_rel[..., 0]) if args.scheme == 'stereographic': r = np.tan(th / 2.) * 2. elif args.scheme == 'equidistant': r = th elif args.scheme == 'equisolidangle': r = np.sin(th / 2.) * 2. elif args.scheme == 'orthographic': r = np.sin(th)
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
def reproject_perturbed__mean_frames( q, distance, # shape (Ncameras, Nintrinsics) baseline_intrinsics, # shape (Ncameras, 6) baseline_rt_cam_ref, # shape (Nframes, 6) baseline_rt_ref_frame, # shape (2) baseline_calobject_warp, # shape (..., Ncameras, Nintrinsics) query_intrinsics, # shape (..., Ncameras, 6) query_rt_cam_ref, # shape (..., Nframes, 6) query_rt_ref_frame, # shape (..., 2) query_calobject_warp): r'''Reproject by computing the mean in the space of frames This is what the uncertainty computation does (as of 2020/10/26). The implied rotation here is aphysical (it is a mean of multiple rotation matrices) ''' # shape (Ncameras, 3) p_cam_baseline = mrcal.unproject( q, lensmodel, baseline_intrinsics, normalize=True) * distance # shape (Ncameras, 3) p_ref_baseline = \ mrcal.transform_point_rt( mrcal.invert_rt(baseline_rt_cam_ref), p_cam_baseline ) if fixedframes: p_ref_query = p_ref_baseline else: # shape (Nframes, Ncameras, 3) # The point in the coord system of all the frames p_frames = mrcal.transform_point_rt( \ nps.dummy(mrcal.invert_rt(baseline_rt_ref_frame),-2), p_ref_baseline) # shape (..., Nframes, Ncameras, 3) p_ref_query_allframes = \ mrcal.transform_point_rt( nps.dummy(query_rt_ref_frame, -2), p_frames ) if args.reproject_perturbed == 'mean-frames': # "Normal" path: I take the mean of all the frame-coord-system # representations of my point if not fixedframes: # shape (..., Ncameras, 3) p_ref_query = np.mean(p_ref_query_allframes, axis=-3) # shape (..., Ncameras, 3) p_cam_query = \ mrcal.transform_point_rt(query_rt_cam_ref, p_ref_query) # shape (..., Ncameras, 2) return mrcal.project(p_cam_query, lensmodel, query_intrinsics) else: # Experimental path: I take the mean of the projections, not the points # in the reference frame # guaranteed that not fixedframes: I asserted this above # shape (..., Nframes, Ncameras, 3) p_cam_query_allframes = \ mrcal.transform_point_rt(nps.dummy(query_rt_cam_ref, -3), p_ref_query_allframes) # shape (..., Nframes, Ncameras, 2) q_reprojected = mrcal.project(p_cam_query_allframes, lensmodel, nps.dummy(query_intrinsics, -3)) if args.reproject_perturbed != 'mean-frames-using-meanq-penalize-big-shifts': return np.mean(q_reprojected, axis=-3) else: # Experiment. Weighted mean to de-emphasize points with huge shifts w = 1. / nps.mag(q_reprojected - q) w = nps.mv(nps.cat(w, w), 0, -1) return \ np.sum(q_reprojected*w, axis=-3) / \ np.sum(w, axis=-3)
def triangulate_nograd(intrinsics_data0, intrinsics_data1, rt_cam0_ref, rt_cam0_ref_baseline, rt_cam1_ref, rt_ref_frame, rt_ref_frame_baseline, q, lensmodel, stabilize_coords=True): q = nps.atleast_dims(q, -3) rt01 = mrcal.compose_rt(rt_cam0_ref, mrcal.invert_rt(rt_cam1_ref)) # all the v have shape (...,3) vlocal0 = \ mrcal.unproject(q[...,0,:], lensmodel, intrinsics_data0) vlocal1 = \ mrcal.unproject(q[...,1,:], lensmodel, intrinsics_data1) v0 = vlocal0 v1 = \ mrcal.rotate_point_r(rt01[:3], vlocal1) # The triangulated point in the perturbed camera-0 coordinate system. # Calibration-time perturbations move this coordinate system, so to get # a better estimate of the triangulation uncertainty, we try to # transform this to the original camera-0 coordinate system; the # stabilization path below does that. # # shape (..., 3) p_triangulated0 = \ mrcal.triangulate_leecivera_mid2(v0, v1, rt01[3:]) if not stabilize_coords: return p_triangulated0 # Stabilization path. This uses the "true" solution, so I cannot do # this in the field. But I CAN do this in the randomized trials in # the test. And I can use the gradients to propagate the uncertainty # of this computation in the field # # Data flow: # point_cam_perturbed -> point_ref_perturbed -> point_frames # point_frames -> point_ref_baseline -> point_cam_baseline p_cam0_perturbed = p_triangulated0 p_ref_perturbed = mrcal.transform_point_rt(rt_cam0_ref, p_cam0_perturbed, inverted=True) # shape (..., Nframes, 3) p_frames = \ mrcal.transform_point_rt(rt_ref_frame, nps.dummy(p_ref_perturbed,-2), inverted = True) # shape (..., Nframes, 3) p_ref_baseline_all = mrcal.transform_point_rt(rt_ref_frame_baseline, p_frames) # shape (..., 3) p_ref_baseline = np.mean(p_ref_baseline_all, axis=-2) # shape (..., 3) return mrcal.transform_point_rt(rt_cam0_ref_baseline, p_ref_baseline)
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') icam_intrinsics_read = model_read.icam_intrinsics() icam_extrinsics_read = mrcal.corresponding_icam_extrinsics( icam_intrinsics_read, **model_read.optimization_inputs()) testutils.confirm_equal( icam if fixedframes else icam - 1, icam_extrinsics_read, msg= f"corresponding icam_extrinsics reported correctly for camera {icam}") p_cam_baseline = mrcal.unproject(q0_baseline, *models_baseline[icam].intrinsics(), normalize=True) Var_dq_ref = \ mrcal.projection_uncertainty( p_cam_baseline * 1.0, model = models_baseline[icam] ) Var_dq_moved_written_read = \ mrcal.projection_uncertainty( p_cam_baseline * 1.0, model = model_read ) testutils.confirm_equal( Var_dq_moved_written_read, Var_dq_ref, eps=0.001, worstcase=True, relative=True, msg=
def image_transformation_map(model_from, model_to, intrinsics_only=False, distance=None, plane_n=None, plane_d=None, mask_valid_intrinsics_region_from=False): r'''Compute a reprojection map between two models SYNOPSIS model_orig = mrcal.cameramodel("xxx.cameramodel") image_orig = cv2.imread("image.jpg") model_pinhole = mrcal.pinhole_model_for_reprojection(model_orig, fit = "corners") mapxy = mrcal.image_transformation_map(model_orig, model_pinhole, intrinsics_only = True) image_undistorted = mrcal.transform_image(image_orig, mapxy) # image_undistorted is now a pinhole-reprojected version of image_orig Returns the transformation that describes a mapping - from pixel coordinates of an image of a scene observed by model_to - to pixel coordinates of an image of the same scene observed by model_from This transformation can then be applied to a whole image by calling mrcal.transform_image(). This function returns a transformation map in an (Nheight,Nwidth,2) array. The image made by model_to will have shape (Nheight,Nwidth). Each pixel (x,y) in this image corresponds to a pixel mapxy[y,x,:] in the image made by model_from. One application of this function is to validate the models in a stereo pair. For instance, reprojecting one camera's image at distance=infinity should produce exactly the same image that is observed by the other camera when looking at very far objects, IF the intrinsics and rotation are correct. If the images don't line up well, we know that some part of the models is off. Similarly, we can use big planes (such as observations of the ground) and plane_n, plane_d to validate. This function has several modes of operation: - intrinsics, extrinsics Used if not intrinsics_only and \ plane_n is None and \ plane_d is None This is the default. For each pixel in the output, we use the full model to unproject a given distance out, and then use the full model to project back into the other camera. - intrinsics only Used if intrinsics_only and \ plane_n is None and \ plane_d is None Similar, but the extrinsics are ignored. We unproject the pixels in one model, and project the into the other camera. The two camera coordinate systems are assumed to line up perfectly - plane Used if plane_n is not None and plane_d is not None We map observations of a given plane in camera FROM coordinates coordinates to where this plane would be observed by camera TO. We unproject each pixel in one camera, compute the intersection point with the plane, and project that intersection point back to the other camera. This uses ALL the intrinsics, extrinsics and the plane representation. The plane is represented by a normal vector plane_n, and the distance to the normal plane_d. The plane is all points p such that inner(p,plane_n) = plane_d. plane_n does not need to be normalized; any scaling is compensated in plane_d. ARGUMENTS - model_from: the mrcal.cameramodel object describing the camera used to capture the input image. We always use the intrinsics. if not intrinsics_only: we use the extrinsics also - model_to: the mrcal.cameramodel object describing the camera that would have captured the image we're producing. We always use the intrinsics. if not intrinsics_only: we use the extrinsics also - intrinsics_only: optional boolean, defaulting to False. If False: we respect the relative transformation in the extrinsics of the camera models. - distance: optional value, defaulting to None. Used only if not intrinsics_only. We reproject points in space a given distance out. If distance is None (the default), we look out to infinity. This is equivalent to using only the rotation component of the extrinsics, ignoring the translation. - plane_n: optional numpy array of shape (3,); None by default. If given, we produce a transformation to map observations of a given plane to the same pixels in the source and target images. This argument describes the normal vector in the coordinate system of model_from. The plane is all points p such that inner(p,plane_n) = plane_d. plane_n does not need to be normalized; any scaling is compensated in plane_d. If given, plane_d should be given also, and intrinsics_only should be False. if given, we use the full intrinsics and extrinsics of both camera models - plane_d: optional floating-point value; None by default. If given, we produce a transformation to map observations of a given plane to the same pixels in the source and target images. The plane is all points p such that inner(p,plane_n) = plane_d. plane_n does not need to be normalized; any scaling is compensated in plane_d. If given, plane_n should be given also, and intrinsics_only should be False. if given, we use the full intrinsics and extrinsics of both camera models - mask_valid_intrinsics_region_from: optional boolean defaulting to False. If True, points outside the valid-intrinsics region in the FROM image are set to black, and thus do not appear in the output image RETURNED VALUE A numpy array of shape (Nheight,Nwidth,2) where Nheight and Nwidth represent the imager dimensions of model_to. This array contains 32-bit floats, as required by cv2.remap() (the function providing the internals of mrcal.transform_image()). This array can be passed to mrcal.transform_image() ''' if (plane_n is None and plane_d is not None) or \ (plane_n is not None and plane_d is None): raise Exception( "plane_n and plane_d should both be None or neither should be None" ) if plane_n is not None and \ intrinsics_only: raise Exception( "We're looking at remapping a plane (plane_d, plane_n are not None), so intrinsics_only should be False" ) if distance is not None and \ (plane_n is not None or intrinsics_only): raise Exception( "'distance' makes sense only without plane_n/plane_d and without intrinsics_only" ) if intrinsics_only: Rt_to_from = None else: Rt_to_from = mrcal.compose_Rt(model_to.extrinsics_Rt_fromref(), model_from.extrinsics_Rt_toref()) lensmodel_from, intrinsics_data_from = model_from.intrinsics() lensmodel_to, intrinsics_data_to = model_to.intrinsics() if re.match("LENSMODEL_OPENCV",lensmodel_from) and \ lensmodel_to == "LENSMODEL_PINHOLE" and \ plane_n is None and \ not mask_valid_intrinsics_region_from and \ distance is None: # This is a common special case. This branch works identically to the # other path (the other side of this "if" can always be used instead), # but the opencv-specific code is optimized and at one point ran faster # than the code on the other side. # # The mask_valid_intrinsics_region_from isn't implemented in this path. # It COULD be, then this faster path could be used import cv2 fxy_from = intrinsics_data_from[0:2] cxy_from = intrinsics_data_from[2:4] cameraMatrix_from = np.array( ((fxy_from[0], 0, cxy_from[0]), (0, fxy_from[1], cxy_from[1]), (0, 0, 1))) fxy_to = intrinsics_data_to[0:2] cxy_to = intrinsics_data_to[2:4] cameraMatrix_to = np.array( ((fxy_to[0], 0, cxy_to[0]), (0, fxy_to[1], cxy_to[1]), (0, 0, 1))) output_shape = model_to.imagersize() distortion_coeffs = intrinsics_data_from[4:] if Rt_to_from is not None: R_to_from = Rt_to_from[:3, :] if np.trace(R_to_from) > 3. - 1e-12: R_to_from = None # identity, so I pass None else: R_to_from = None return nps.glue( *[ nps.dummy(arr,-1) for arr in \ cv2.initUndistortRectifyMap(cameraMatrix_from, distortion_coeffs, R_to_from, cameraMatrix_to, tuple(output_shape), cv2.CV_32FC1)], axis = -1) W_from, H_from = model_from.imagersize() W_to, H_to = model_to.imagersize() # shape: (Nheight,Nwidth,2). Contains (x,y) rows grid = np.ascontiguousarray(nps.mv( nps.cat(*np.meshgrid(np.arange(W_to), np.arange(H_to))), 0, -1), dtype=float) v = mrcal.unproject(grid, lensmodel_to, intrinsics_data_to) if plane_n is not None: R_to_from = Rt_to_from[:3, :] t_to_from = Rt_to_from[3, :] # The homography definition. Derived in many places. For instance in # "Motion and structure from motion in a piecewise planar environment" # by Olivier Faugeras, F. Lustman. A_to_from = plane_d * R_to_from + nps.outer(t_to_from, plane_n) A_from_to = np.linalg.inv(A_to_from) v = nps.matmult(v, nps.transpose(A_from_to)) else: if Rt_to_from is not None: if distance is not None: v = mrcal.transform_point_Rt( mrcal.invert_Rt(Rt_to_from), v / nps.dummy(nps.mag(v), -1) * distance) else: R_to_from = Rt_to_from[:3, :] v = nps.matmult(v, R_to_from) mapxy = mrcal.project(v, lensmodel_from, intrinsics_data_from) if mask_valid_intrinsics_region_from: # Using matplotlib to compute the out-of-bounds points. It doesn't # support broadcasting, so I do that manually with a clump/reshape from matplotlib.path import Path region = Path(model_from.valid_intrinsics_region()) is_inside = region.contains_points(nps.clump(mapxy, n=2)).reshape( mapxy.shape[:2]) mapxy[~is_inside, :] = -1 return mapxy.astype(np.float32)
def scale_focal__best_pinhole_fit(model, fit): r'''Compute the optimal focal-length scale for reprojection to a pinhole lens SYNOPSIS model = mrcal.cameramodel('from.cameramodel') lensmodel,intrinsics_data = model.intrinsics() scale_focal = mrcal.scale_focal__best_pinhole_fit(model, 'centers-horizontal') intrinsics_data[:2] *= scale_focal model_pinhole = \ mrcal.cameramodel(intrinsics = ('LENSMODEL_PINHOLE', intrinsics_data[:4]), imagersize = model.imagersize(), extrinsics_rt_fromref = model.extrinsics_rt_fromref() ) Many algorithms work with images assumed to have been captured with a pinhole camera, even though real-world lenses never fit a pinhole model. mrcal provides several functions to remap images captured with non-pinhole lenses into images of the same scene as if they were observed by a pinhole lens. When doing this, we're free to choose all of the parameters of this pinhole lens model, and this function allows us to pick the best scaling on the focal-length parameter. The focal length parameters serve as a "zoom" factor: changing these parameters can increase the resolution of the center of the image, at the expense of cutting off the edges. This function computes the best focal-length scaling, for several possible meanings of "best". I assume an output pinhole model that has pinhole parameters (k*fx, k*fy, cx, cy) where (fx, fy, cx, cy) are the parameters from the input model, and k is the scaling we compute. This function looks at some points on the edge of the input image. I choose k so that all of these points end up inside the pinhole-reprojected image, leaving the worst one at the edge. The set of points I look at are specified in the "fit" argument. ARGUMENTS - model: a mrcal.cameramodel object for the input lens model - fit: which pixel coordinates in the input image must project into the output pinhole-projected image. The 'fit' argument must be one of - a numpy array of shape (N,2) where each row is a pixel coordinate in the input image - "corners": each of the 4 corners of the input image must project into the output image - "centers-horizontal": the two points at the left and right edges of the input image, half-way vertically, must both project into the output image - "centers-vertical": the two points at the top and bottom edges of the input image, half-way horizontally, must both project into the output image RETURNED VALUE A scalar scale_focal that can be passed to pinhole_model_for_reprojection() ''' if fit is None: return 1.0 WH = np.array(model.imagersize(), dtype=float) W, H = WH if type(fit) is np.ndarray: q_edges = fit elif type(fit) is str: if fit == 'corners': q_edges = np.array( ((0., 0.), (0., H - 1.), (W - 1., H - 1.), (W - 1., 0.))) elif fit == 'centers-horizontal': q_edges = np.array(((0, (H - 1.) / 2.), (W - 1., (H - 1.) / 2.))) elif fit == 'centers-vertical': q_edges = np.array((( (W - 1.) / 2., 0, ), ( (W - 1.) / 2., H - 1., ))) else: raise Exception( "fit must be either None or a numpy array or one of ('corners','centers-horizontal','centers-vertical')" ) else: raise Exception( "fit must be either None or a numpy array or one of ('corners','centers-horizontal','centers-vertical')" ) lensmodel, intrinsics_data = model.intrinsics() v_edges = mrcal.unproject(q_edges, lensmodel, intrinsics_data) if not mrcal.lensmodel_metadata_and_config(lensmodel)['has_core']: raise Exception( "This currently works only with models that have an fxfycxcy core") fxy = intrinsics_data[:2] cxy = intrinsics_data[2:4] # I have points in space now. My scaled pinhole camera would map these to # (k*fx*x/z+cx, k*fy*y/z+cy). I pick a k to make sure that this is in-bounds # for all my points, and that the points occupy as large a chunk of the # imager as possible. I can look at just the normalized x and y. Just one of # the query points should land on the edge; the rest should be in-bounds normxy_edges = v_edges[:, :2] / v_edges[:, (2, )] normxy_min = (-cxy) / fxy normxy_max = (WH - 1. - cxy) / fxy # Each query point will imply a scale to just fit into the imager I take the # most conservative of these. For each point I look at the normalization sign to # decide if I should be looking at the min or max edge. And for each I pick the # more conservative scale # start out at an unreasonably high scale. The data will cut this down scale = 1e6 for p in normxy_edges: for ixy in range(2): if p[ixy] > 0: scale = np.min((scale, normxy_max[ixy] / p[ixy])) else: scale = np.min((scale, normxy_min[ixy] / p[ixy])) return scale
p / nps.dummy(nps.mag(p), axis=-1), msg=f"unproject_{name}()", worstcase=True, relative=True) else: cos = nps.inner(v_unprojected, p) / (nps.mag(p) * nps.mag(v_unprojected)) cos = np.clip(cos, -1, 1) testutils.confirm_equal(np.arccos(cos), np.zeros((p.shape[0], ), dtype=float), msg=f"unproject_{name}()", worstcase=True) # Not normalized by default. Make sure that if I ask for it to be # normalized, that it is testutils.confirm_equal( nps.mag(mrcal.unproject(q_projected, *intrinsics, normalize=True)), 1., msg=f"unproject({name},normalize = True) returns normalized vectors", worstcase=True, relative=True) testutils.confirm_equal( nps.mag( mrcal.unproject(q_projected, *intrinsics, normalize=True, get_gradients=True)[0]), 1., msg= f"unproject({name},normalize = True, get_gradients=True) returns normalized vectors", worstcase=True, relative=True)
any(on_left_edge) or \ any(on_top_edge) or \ any(on_right_edge) or \ any(on_bottom_edge) ): return ": No points lie on the edge" # all good return '' err_msg = \ fit_check( mrcal.scale_focal__best_pinhole_fit(m, 'corners'), intrinsics_core, mrcal.unproject( np.array((( 0 , 0), (W-1, 0), ( 0, H-1), (W-1, H-1)), dtype=float), *m.intrinsics()),) testutils.confirm( err_msg == '', msg = 'scale_focal__best_pinhole_fit' + err_msg) err_msg = \ fit_check( mrcal.scale_focal__best_pinhole_fit(m, 'centers-horizontal'), intrinsics_core, mrcal.unproject( np.array((( 0, (H-1.)/2.), (W-1, (H-1.)/2.)), dtype=float), *m.intrinsics()),) testutils.confirm( err_msg == '', msg = 'scale_focal__best_pinhole_fit' + err_msg) err_msg = \
def sample_imager_unproject(gridn_width, gridn_height, imager_width, imager_height, lensmodel, intrinsics_data, normalize=False): r'''Reports 3D observation vectors that regularly sample the imager SYNOPSIS import gnuplotlib as gp import mrcal ... Nwidth = 60 Nheight = 40 # shape (Nheight,Nwidth,3) v,q = \ mrcal.sample_imager_unproject(Nw, Nh, *model.imagersize(), *model.intrinsics()) # shape (Nheight,Nwidth) f = interesting_quantity(v) gp.plot(f, tuplesize = 3, ascii = True, using = mrcal.imagergrid_using(model.imagersize, Nw, Nh), square = True, _with = 'image') This is a utility function used by functions that evalute some interesting quantity for various locations across the imager. Grid dimensions and lens parameters are passed in, and the grid points and corresponding unprojected vectors are returned. The unprojected vectors are unique only up-to-length, and the returned vectors aren't normalized by default. If we want them to be normalized, pass normalize=True. This function has two modes of operation: - One camera. lensmodel is a string, and intrinsics_data is a 1-dimensions numpy array. With a mrcal.cameramodel object together these are *model.intrinsics(). We return (v,q) where v is a shape (Nheight,Nwidth,3) array of observation vectors, and q is a (Nheight,Nwidth,2) array of corresponding pixel coordinates (the grid returned by sample_imager()) - Multiple cameras. lensmodel is a list or tuple of strings; intrinsics_data is an iterable of 1-dimensional numpy arrays (a list/tuple or a 2D array). We return the same q as before (only one camera is gridded), but the unprojected array v has shape (Ncameras,Nheight,Nwidth,3) where Ncameras is the leading dimension of lensmodel. The gridded imager appears in camera0: v[0,...] = unproject(q) ARGUMENTS - gridn_width: how many points along the horizontal gridding dimension - gridn_height: how many points along the vertical gridding dimension. If None, we compute an integer gridn_height to maintain a square-ish grid: gridn_height/gridn_width ~ imager_height/imager_width - imager_width,imager_height: the width, height of the imager. With a mrcal.cameramodel object this is *model.imagersize() - lensmodel, intrinsics_data: the lens parameters. With a single camera, lensmodel is a string, and intrinsics_data is a 1-dimensions numpy array; with a mrcal.cameramodel object together these are *model.intrinsics(). With multiple cameras, lensmodel is a list/tuple of strings. And intrinsics_data is an iterable of 1-dimensional numpy arrays (a list/tuple or a 2D array). - normalize: optional boolean defaults to False. If True: normalize the output vectors RETURNED VALUES We return a tuple: - v: the unprojected vectors. If we have a single camera this has shape (Nheight,Nwidth,3). With multiple cameras this has shape (Ncameras,Nheight,Nwidth,3). These are NOT normalized by default. To get normalized vectors, pass normalize=True - q: the imager-sampling grid. This has shape (Nheight,Nwidth,2) regardless of how many cameras were given (we always sample just one camera). This is what sample_imager() returns ''' def is_list_or_tuple(l): return isinstance(l, list) or isinstance(l, tuple) # shape: (Nheight,Nwidth,2). Contains (x,y) rows grid = sample_imager(gridn_width, gridn_height, imager_width, imager_height) if is_list_or_tuple(lensmodel): # shape: Ncameras,Nwidth,Nheight,3 return np.array([mrcal.unproject(np.ascontiguousarray(grid), lensmodel[i], intrinsics_data[i], normalize = normalize) \ for i in range(len(lensmodel))]), \ grid else: # shape: Nheight,Nwidth,3 return \ mrcal.unproject(np.ascontiguousarray(grid), lensmodel, intrinsics_data, normalize = normalize), \ grid
def _triangulate(# shape (Ncameras, Nintrinsics) intrinsics_data, # shape (Ncameras, 6) rt_cam_ref, # shape (Nframes,6), rt_ref_frame, rt_ref_frame_true, # shape (..., Ncameras, 2) q, lensmodel, stabilize_coords, get_gradients): if not ( intrinsics_data.ndim == 2 and intrinsics_data.shape[0] == 2 and \ rt_cam_ref.shape == (2,6) and \ rt_ref_frame.ndim == 2 and rt_ref_frame.shape[-1] == 6 and \ q.shape[-2:] == (2,2 ) ): raise Exception("Arguments must have a consistent Ncameras == 2") # I now compute the same triangulation, but just at the un-perturbed baseline, # and keeping track of all the gradients rt0r = rt_cam_ref[0] rt1r = rt_cam_ref[1] if not get_gradients: rtr1 = mrcal.invert_rt(rt1r) rt01_baseline = mrcal.compose_rt(rt0r, rtr1) # all the v have shape (...,3) vlocal0 = \ mrcal.unproject(q[...,0,:], lensmodel, intrinsics_data[0]) vlocal1 = \ mrcal.unproject(q[...,1,:], lensmodel, intrinsics_data[1]) v0 = vlocal0 v1 = \ mrcal.rotate_point_r(rt01_baseline[:3], vlocal1) # p_triangulated has shape (..., 3) p_triangulated = \ mrcal.triangulate_leecivera_mid2(v0, v1, rt01_baseline[3:]) if stabilize_coords: # shape (..., Nframes, 3) p_frames_new = \ mrcal.transform_point_rt(mrcal.invert_rt(rt_ref_frame), nps.dummy(p_triangulated,-2)) # shape (..., Nframes, 3) p_refs = mrcal.transform_point_rt(rt_ref_frame_true, p_frames_new) # shape (..., 3) p_triangulated = np.mean(p_refs, axis=-2) return p_triangulated else: rtr1,drtr1_drt1r = mrcal.invert_rt(rt1r, get_gradients=True) rt01_baseline,drt01_drt0r, drt01_drtr1 = mrcal.compose_rt(rt0r, rtr1, get_gradients=True) # all the v have shape (...,3) vlocal0, dvlocal0_dq0, dvlocal0_dintrinsics0 = \ mrcal.unproject(q[...,0,:], lensmodel, intrinsics_data[0], get_gradients = True) vlocal1, dvlocal1_dq1, dvlocal1_dintrinsics1 = \ mrcal.unproject(q[...,1,:], lensmodel, intrinsics_data[1], get_gradients = True) v0 = vlocal0 v1, dv1_dr01, dv1_dvlocal1 = \ mrcal.rotate_point_r(rt01_baseline[:3], vlocal1, get_gradients=True) # p_triangulated has shape (..., 3) p_triangulated, dp_triangulated_dv0, dp_triangulated_dv1, dp_triangulated_dt01 = \ mrcal.triangulate_leecivera_mid2(v0, v1, rt01_baseline[3:], get_gradients = True) shape_leading = dp_triangulated_dv0.shape[:-2] dp_triangulated_dq = np.zeros(shape_leading + (3,) + q.shape[-2:], dtype=float) nps.matmult( dp_triangulated_dv0, dvlocal0_dq0, out = dp_triangulated_dq[..., 0, :]) nps.matmult( dp_triangulated_dv1, dv1_dvlocal1, dvlocal1_dq1, out = dp_triangulated_dq[..., 1, :]) Nframes = len(rt_ref_frame) if stabilize_coords: # shape (Nframes,6) rt_frame_ref, drtfr_drtrf = \ mrcal.invert_rt(rt_ref_frame, get_gradients=True) # shape (Nframes,6) rt_true_shifted, _, drt_drtfr = \ mrcal.compose_rt(rt_ref_frame_true, rt_frame_ref, get_gradients=True) # shape (..., Nframes, 3) p_refs,dprefs_drt,dprefs_dptriangulated = \ mrcal.transform_point_rt(rt_true_shifted, nps.dummy(p_triangulated,-2), get_gradients = True) # shape (..., 3) p_triangulated = np.mean(p_refs, axis=-2) # I have dpold/dx. dpnew/dx = dpnew/dpold dpold/dx # shape (...,3,3) dpnew_dpold = np.mean(dprefs_dptriangulated, axis=-3) dp_triangulated_dv0 = nps.matmult(dpnew_dpold, dp_triangulated_dv0) dp_triangulated_dv1 = nps.matmult(dpnew_dpold, dp_triangulated_dv1) dp_triangulated_dt01 = nps.matmult(dpnew_dpold, dp_triangulated_dt01) dp_triangulated_dq = nps.xchg(nps.matmult( dpnew_dpold, nps.xchg(dp_triangulated_dq, -2,-3)), -2,-3) # shape (..., Nframes,3,6) dp_triangulated_drtrf = \ nps.matmult(dprefs_drt, drt_drtfr, drtfr_drtrf) / Nframes else: dp_triangulated_drtrf = np.zeros(shape_leading + (Nframes,3,6), dtype=float) return \ p_triangulated, \ 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