def get_observation_chunk(): '''Make Nframes observations, and return them all, even the out-of-view ones''' # I compute the full random block in one shot. This is useful for # simulations that want to see identical poses when asking for N-1 # random poses and when asking for the first N-1 of a set of N random # poses # shape (Nframes,6) randomblock = np.random.uniform(low=-1.0, high=1.0, size=(Nframes, 6)) # shape(Nframes,4,3) Rt_ref_boardref = \ mrcal.Rt_from_rt( rt_ref_boardcenter + randomblock * rt_ref_boardcenter__noiseradius ) # shape = (Nframes, Nh,Nw,3) boards_ref = mrcal.transform_point_Rt( # shape (Nframes, 1,1,4,3) nps.mv(Rt_ref_boardref, 0, -5), # shape ( Nh,Nw,3) board_reference) # I project full_board. Shape: (Nframes,Ncameras,Nh,Nw,2) q = \ nps.mv( \ nps.cat( \ *[ mrcal.project( mrcal.transform_point_Rt(models[i].extrinsics_Rt_fromref(), boards_ref), *models[i].intrinsics()) \ for i in range(Ncameras) ]), 0,1 ) return q, Rt_ref_boardref
def grad(f, x, step=1e-6): r'''Computes df/dx at x f is a function of one argument. If the input has shape Si and the output has shape So, the returned gradient has shape So+Si. This applies central differences ''' d = np.zeros(x.shape, dtype=float) dflat = d.ravel() def df_dxi(i, d, dflat): dflat[i] = step fplus = f(x + d) fminus = f(x - d) j = (fplus - fminus) / (2. * step) dflat[i] = 0 return j # grad variable is in first dim Jflat = nps.cat(*[df_dxi(i, d, dflat) for i in range(len(dflat))]) # grad variable is in last dim Jflat = nps.mv(Jflat, 0, -1) return Jflat.reshape(Jflat.shape[:-1] + d.shape)
def _splined_stereographic_domain(lensmodel): r'''Return the stereographic domain for splined-stereographic lens models SYNOPSIS model = mrcal.cameramodel(model_filename) lensmodel = model.intrinsics()[0] domain_contour = mrcal._splined_stereographic_domain(lensmodel) Splined stereographic models are defined by a splined surface. This surface is indexed by normalized stereographic-projected points. This surface is defined in some finite area, and this function reports a piecewise linear contour reporting this region. This function only makes sense for splined stereographic models. RETURNED VALUE An array of shape (N,2) containing a contour representing the projection domain. ''' if not re.match('LENSMODEL_SPLINED_STEREOGRAPHIC', lensmodel): raise Exception(f"This only makes sense with splined models. Input uses {lensmodel}") ux,uy = mrcal.knots_for_splined_models(lensmodel) # shape (Ny,Nx,2) u = np.ascontiguousarray(nps.mv(nps.cat(*np.meshgrid(ux,uy)), 0, -1)) meta = mrcal.lensmodel_metadata(lensmodel) if meta['order'] == 2: # spline order is 3. The valid region is 1/2 segments inwards from the # outer contour return \ nps.glue( (u[0,1:-2] + u[1,1:-2]) / 2., (u[0,-2] + u[1,-2] + u[0,-1] + u[1,-1]) / 4., (u[1:-2, -2] + u[1:-2, -1]) / 2., (u[-2,-2] + u[-1,-2] + u[-2,-1] + u[-1,-1]) / 4., (u[-2, -2:1:-1] + u[-1, -2:1:-1]) / 2., (u[-2, 1] + u[-1, 1] + u[-2, 0] + u[-1, 0]) / 4., (u[-2:0:-1, 0] +u[-2:0:-1, 1]) / 2., (u[0, 0] +u[0, 1] + u[1, 0] +u[1, 1]) / 4., (u[0,1] + u[1,1]) / 2., axis = -2 ) elif meta['order'] == 3: # spline order is 3. The valid region is the outer contour, leaving one # knot out return \ nps.glue( u[1,1:-2], u[1:-2, -2], u[-2, -2:1:-1], u[-2:0:-1, 1], axis=-2 ) else: raise Exception("I only support cubic (order==3) and quadratic (order==2) models")
def sample_imager(gridn_width, gridn_height, imager_width, imager_height): r'''Returns regularly-sampled, gridded pixels coordinates across the imager SYNOPSIS q = sample_imager( 60, 40, *model.imagersize() ) print(q.shape) ===> (40,60,2) Note that the arguments are given in width,height order, as is customary when generally talking about images and indexing. However, the output is in height,width order, as is customary when talking about matrices and numpy arrays. If we ask for gridding dimensions (gridn_width, gridn_height), the output has shape (gridn_height,gridn_width,2) where each row is an (x,y) pixel coordinate. The top-left corner is at [0,0,:]: sample_imager(...)[0,0] = [0,0] The the bottom-right corner is at [-1,-1,:]: sample_imager(...)[ -1, -1,:] = sample_imager(...)[gridn_height-1,gridn_width-1,:] = (imager_width-1,imager_height-1) When making plots you probably want to call mrcal.imagergrid_using(). See the that docstring for details. 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() RETURNED VALUES We return an array of shape (gridn_height,gridn_width,2). Each row is an (x,y) pixel coordinate. ''' if gridn_height is None: gridn_height = int(round(imager_height/imager_width*gridn_width)) w = np.linspace(0,imager_width -1,gridn_width) h = np.linspace(0,imager_height-1,gridn_height) return np.ascontiguousarray(nps.mv(nps.cat(*np.meshgrid(w,h)), 0,-1))
def sample_dqref(observations, pixel_uncertainty_stdev, make_outliers=False): # Outliers have weight < 0. The code will adjust the outlier observations # also. But that shouldn't matter: they're outliers so those observations # should be ignored weight = observations[..., -1] q_noise = np.random.randn(*observations.shape[:-1], 2) * pixel_uncertainty_stdev / nps.mv( nps.cat(weight, weight), 0, -1) if make_outliers: if not hasattr(sample_dqref, 'idx_outliers_ref_flat'): NobservedPoints = observations.size // 3 sample_dqref.idx_outliers_ref_flat = \ np.random.choice( NobservedPoints, (NobservedPoints//100,), # 1% outliers replace = False ) nps.clump(q_noise, n=3)[sample_dqref.idx_outliers_ref_flat, :] *= 20 observations_perturbed = observations.copy() observations_perturbed[..., :2] += q_noise return q_noise, observations_perturbed
lensmodel, intrinsics_data = m.intrinsics() ref_p = np.array( ((10., 20., 100.), (25., 30., 90.), (5., 10., 94.), (-45., -20., 95.), (-35., 14., 77.), (5., -0., 110.), (1., 50., 50.))) # The points are all somewhere at +z. So the Camera poses are all ~ identity ref_extrinsics_rt_fromref = np.array( ((-0.1, -0.07, 0.01, 10.0, 4.0, -7.0), (-0.01, 0.05, -0.02, 30.0, -8.0, -8.0), (-0.1, 0.03, -0.03, 10.0, -9.0, 20.0), (0.04, -0.04, 0.03, -20.0, 2.0, -11.0), (0.01, 0.05, -0.05, -10.0, 3.0, 9.0))) # shape (Ncamposes, Npoints, 3) ref_p_cam = mrcal.transform_point_rt(nps.mv(ref_extrinsics_rt_fromref, -2, -3), ref_p) # shape (Ncamposes, Npoints, 2) ref_q_cam = mrcal.project(ref_p_cam, lensmodel, intrinsics_data) # Observations are incomplete. Not all points are observed from everywhere indices_point_camintrinsics_camextrinsics = \ np.array(((0, 0, 1), (0, 0, 2), (0, 0, 4), (1, 0, 0), (1, 0, 1), (1, 0, 4), (2, 0, 0), (2, 0, 1),
def hypothesis_board_corner_positions(icam_intrinsics=None, idx_inliers=None, **optimization_inputs): r'''Reports the 3D chessboard points observed by a camera at calibration time SYNOPSIS model = mrcal.cameramodel("xxx.cameramodel") optimization_inputs = model.optimization_inputs() # shape (Nobservations, Nheight, Nwidth, 3) pcam = mrcal.hypothesis_board_corner_positions(**optimization_inputs)[0] i_intrinsics = \ optimization_inputs['indices_frame_camintrinsics_camextrinsics'][:,1] # shape (Nobservations,1,1,Nintrinsics) intrinsics = nps.mv(optimization_inputs['intrinsics'][i_intrinsics],-2,-4) optimization_inputs['observations_board'][...,:2] = \ mrcal.project( pcam, optimization_inputs['lensmodel'], intrinsics ) # optimization_inputs now contains perfect, noiseless board observations x = mrcal.optimizer_callback(**optimization_inputs)[1] print(nps.norm2(x[:mrcal.num_measurements_boards(**optimization_inputs)])) ==> 0 The optimization routine generates hypothetical observations from a set of parameters being evaluated, trying to match these hypothetical observations to real observations. To facilitate analysis, this routine returns these hypothetical coordinates of the chessboard corners being observed. This routine reports the 3D points in the coordinate system of the observing camera. The hypothetical points are constructed from - The calibration object geometry - The calibration object-reference transformation in optimization_inputs['frames_rt_toref'] - The camera extrinsics (reference-camera transformation) in optimization_inputs['extrinsics_rt_fromref'] - The table selecting the camera and calibration object frame for each observation in optimization_inputs['indices_frame_camintrinsics_camextrinsics'] ARGUMENTS - icam_intrinsics: optional integer specifying which intrinsic camera in the optimization_inputs we're looking at. If omitted (or None), we report camera-coordinate points for all the cameras - idx_inliers: optional numpy array of booleans of shape (Nobservations,object_height,object_width) to select the outliers manually. If omitted (or None), the outliers are selected automatically: idx_inliers = observations_board[...,2] > 0. This argument is available to pick common inliers from two separate solves. - **optimization_inputs: a dict() of arguments passable to mrcal.optimize() and mrcal.optimizer_callback(). We use the geometric data. This dict is obtainable from a cameramodel object by calling cameramodel.optimization_inputs() RETURNED VALUE - An array of shape (Nobservations, Nheight, Nwidth, 3) containing the coordinates (in the coordinate system of each camera) of the chessboard corners, for ALL the cameras. These correspond to the observations in optimization_inputs['observations_board'], which also have shape (Nobservations, Nheight, Nwidth, 3) - An array of shape (Nobservations_thiscamera, Nheight, Nwidth, 3) containing the coordinates (in the camera coordinate system) of the chessboard corners, for the particular camera requested in icam_intrinsics. If icam_intrinsics is None: this is the same array as the previous returned value - an (N,3) array containing camera-frame 3D points observed at calibration time, and accepted by the solver as inliers. This is a subset of the 2nd returned array. - an (N,3) array containing camera-frame 3D points observed at calibration time, but rejected by the solver as outliers. This is a subset of the 2nd returned array. ''' observations_board = optimization_inputs.get('observations_board') if observations_board is None: return Exception("No board observations available") indices_frame_camintrinsics_camextrinsics = \ optimization_inputs['indices_frame_camintrinsics_camextrinsics'] object_width_n = observations_board.shape[-2] object_height_n = observations_board.shape[-3] object_spacing = optimization_inputs['calibration_object_spacing'] calobject_warp = optimization_inputs.get('calobject_warp') # shape (Nh,Nw,3) full_object = mrcal.ref_calibration_object(object_width_n, object_height_n, object_spacing, calobject_warp) frames_Rt_toref = \ mrcal.Rt_from_rt( optimization_inputs['frames_rt_toref'] )\ [ indices_frame_camintrinsics_camextrinsics[:,0] ] extrinsics_Rt_fromref = \ nps.glue( mrcal.identity_Rt(), mrcal.Rt_from_rt(optimization_inputs['extrinsics_rt_fromref']), axis = -3 ) \ [ indices_frame_camintrinsics_camextrinsics[:,2]+1 ] Rt_cam_frame = mrcal.compose_Rt(extrinsics_Rt_fromref, frames_Rt_toref) p_cam_calobjects = \ mrcal.transform_point_Rt(nps.mv(Rt_cam_frame,-3,-5), full_object) # shape (Nobservations,Nheight,Nwidth) if idx_inliers is None: idx_inliers = observations_board[..., 2] > 0 idx_outliers = ~idx_inliers if icam_intrinsics is None: return \ p_cam_calobjects, \ p_cam_calobjects, \ p_cam_calobjects[idx_inliers, ...], \ p_cam_calobjects[idx_outliers, ...] # The user asked for a specific camera. Separate out its data # shape (Nobservations,) idx_observations = indices_frame_camintrinsics_camextrinsics[:, 1] == icam_intrinsics idx_inliers[~idx_observations] = False idx_outliers[~idx_observations] = False return \ p_cam_calobjects, \ p_cam_calobjects[idx_observations, ...], \ p_cam_calobjects[idx_inliers, ...], \ p_cam_calobjects[idx_outliers, ...]
noise_for_gradients = 1e-3 dqref, observations_perturbed = sample_dqref(baseline['observations_board'], noise_for_gradients) optimization_inputs = copy.deepcopy(baseline) optimization_inputs['observations_board'] = observations_perturbed mrcal.optimize(**optimization_inputs, do_apply_outlier_rejection=False) p1, x1, J1 = mrcal.optimizer_callback(no_factorization=True, **optimization_inputs)[:3] J1 = J1.toarray() dx_observed = x1 - x0 dp_observed = p1 - p0 w = observations_perturbed[..., 2] w[w < 0] = 0 # outliers have weight=0 w = np.ravel(nps.mv(nps.cat(w, w), 0, -1)) # each weight controls x,y xtJ1 = nps.inner(nps.transpose(J1), x1) mrcal.pack_state(xtJ0, **optimization_inputs) testutils.confirm_equal(xtJ1, 0, eps=1e-2, worstcase=True, msg="dE/dp = 0 at the optimum: perturbed") # I added noise reoptimized, did dx do the expected thing? # I should have # x(p+dp, qref+dqref) = x + J dp + dx/dqref dqref # -> x1-x0 ~ J dp + dx/dqref dqref # x[measurements] = (q - qref) * weight # -> dx/dqref = -diag(weight)
def ref_calibration_object(W, H, object_spacing, calobject_warp=None): r'''Return the geometry of the calibration object SYNOPSIS import gnuplotlib as gp import numpysane as nps obj = mrcal.ref_calibration_object( 10,6, 0.1 ) print(obj.shape) ===> (6, 10, 3) gp.plot( nps.clump( obj[...,:2], n=2), tuplesize = -2, _with = 'points', _xrange = (-0.1,1.0), _yrange = (-0.1,0.6), unset = 'grid', square = True, terminal = 'dumb 74,45') 0.6 +---------------------------------------------------------------+ | + + + + + | | | 0.5 |-+ A A A A A A A A A A +-| | | | | 0.4 |-+ A A A A A A A A A A +-| | | | | 0.3 |-+ A A A A A A A A A A +-| | | | | 0.2 |-+ A A A A A A A A A A +-| | | | | 0.1 |-+ A A A A A A A A A A +-| | | | | 0 |-+ A A A A A A A A A A +-| | | | + + + + + | -0.1 +---------------------------------------------------------------+ 0 0.2 0.4 0.6 0.8 1 Returns the geometry of a calibration object in its own reference coordinate system in a (H,W,3) array. Only a grid-of-points calibration object is supported, possibly with some bowing (i.e. what the internal mrcal solver supports). Each row of the output is an (x,y,z) point. The origin is at the corner of the grid, so ref_calibration_object(...)[0,0,:] is np.array((0,0,0)). The grid spans x and y, with z representing the depth: z=0 for a flat calibration object. A simple parabolic board warping model is supported by passing a (2,) array in calobject_warp. These 2 values describe additive flex along the x axis and along the y axis, in that order. In each direction the flex is a parabola, with the parameter k describing the max deflection at the center. If the edges were at +-1 we'd have z = k*(1 - x^2) The edges we DO have are at (0,N-1), so the equivalent expression is xr = x / (N-1) z = k*( 1 - 4*xr^2 + 4*xr - 1 ) = 4*k*(xr - xr^2) = ARGUMENTS - W: how many points we have in the horizontal direction - H: how many points we have in the vertical direction - object_spacing: the distance between adjacent points in the calibration object. If a scalar is given, a square object is assumed, and the vertical and horizontal distances are assumed to be identical. An array of shape (..., 2) can be given: the last dimension is (spacing_h, spacing_w), and the preceding dimensions are used for broadcasting - calobject_warp: optional array of shape (2,) defaults to None. Describes the warping of the calibration object. If None, the object is flat. If an array is given, the values describe the maximum additive deflection along the x and y axes. Extended array can be given for broadcasting This function supports broadcasting across object_spacing and calobject_warp RETURNED VALUES The calibration object geometry in a (..., H,W,3) array, with the leading dimensions set by the broadcasting rules ''' # shape (H,W) xx, yy = np.meshgrid(np.arange(W, dtype=float), np.arange(H, dtype=float)) # shape (H,W,3) full_object = nps.glue(nps.mv(nps.cat(xx, yy), 0, -1), np.zeros((H, W, 1)), axis=-1) # object_spacing has shape (..., 2) object_spacing = np.array(object_spacing) if object_spacing.ndim == 0: object_spacing = np.array((1, 1)) * object_spacing object_spacing = nps.dummy(object_spacing, -2, -2) # object_spacing now has shape (..., 1,1,2) if object_spacing.ndim > 3: # extend full_object to the output shape I want full_object = full_object * np.ones(object_spacing.shape[:-3] + (1, 1, 1)) full_object[..., :2] *= object_spacing if calobject_warp is not None: xr = xx / (W - 1) yr = yy / (H - 1) dx = 4. * xr * (1. - xr) dy = 4. * yr * (1. - yr) # To allow broadcasting over calobject_warp if calobject_warp.ndim > 1: # shape (..., 1,1,2) calobject_warp = nps.dummy(calobject_warp, -2, -2) # extend full_object to the output shape I want full_object = full_object * np.ones(calobject_warp.shape[:-3] + (1, 1, 1)) full_object[..., 2] += calobject_warp[..., 0] * dx full_object[..., 2] += calobject_warp[..., 1] * dy return full_object
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 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 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 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)
1 + k[5] * r2 + k[6] * r4 + k[7] * r6)) try: m = mrcal.cameramodel(args.model) except: 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)