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, ...]
out333 = base[:3, :3, :3, 3]
out343 = base[:3, 3:7, :3, 3]
out43 = base[2, 4:8, :3, 2]
out33 = base[2, 8:11, :3, 2]
out33a = base[3, :3, :3, 2]
out33b = base[3, 3:6, :3, 2]
out33c = base[3, 6:9, :3, 2]
out33d = base[4, :3, :3, 2]
out36 = base[6, :3, :6, 2]
out3 = base[1, 3, 6:9, 1]
out6 = base[1, 4, :6, 1]
out66 = base[5, 3:9, 3:9, 2]
out66a = base[6, 3:9, 3:9, 2]
confirm_equal(mrcal.identity_R(out=out33), np.eye(3), msg='identity_R')
confirm_equal(mrcal.identity_Rt(out=out43),
nps.glue(np.eye(3), np.zeros((3, ), ), axis=-2),
msg='identity_Rt')
confirm_equal(mrcal.identity_r(out=out3), np.zeros((3, )), msg='identity_r')
confirm_equal(mrcal.identity_rt(out=out6), np.zeros((6, )), msg='identity_rt')
################# rotate_point_R
y = \
mrcal.rotate_point_R(R0_ref, x, out = out3)
confirm_equal(y,
nps.matmult(x, nps.transpose(R0_ref)),
msg='rotate_point_R result')
y, J_R, J_x = \
mrcal.rotate_point_R(R0_ref, x, get_gradients=True,
out = (out3,out333,out33))
msg=f'Generated models pass validation ({lensmodel})')
except:
testutils.confirm(False,
msg=f'Generated models pass validation ({lensmodel})')
Rt_cam0_rect = mrcal.compose_Rt( model0.extrinsics_Rt_fromref(),
models_rectified[0].extrinsics_Rt_toref())
Rt01_rectified = mrcal.compose_Rt( models_rectified[0].extrinsics_Rt_fromref(),
models_rectified[1].extrinsics_Rt_toref())
testutils.confirm_equal(models_rectified[0].intrinsics()[0], lensmodel,
msg=f'model0 has the right lensmodel ({lensmodel})')
testutils.confirm_equal(models_rectified[1].intrinsics()[0], lensmodel,
msg=f'model1 has the right lensmodel ({lensmodel})')
testutils.confirm_equal(Rt_cam0_rect, mrcal.identity_Rt(),
msg=f'vanilla stereo has a vanilla geometry ({lensmodel})')
testutils.confirm_equal( Rt01_rectified[3,0],
nps.mag(rt01[3:]),
msg=f'vanilla stereo: baseline ({lensmodel})')
Naz,Nel = models_rectified[0].imagersize()
q0 = np.array(((Naz-1.)/2., (Nel-1.)/2.))
v0 = mrcal.unproject(q0, *models_rectified[0].intrinsics(), normalize=True)
if lensmodel == 'LENSMODEL_LATLON':
v0_rect = mrcal.unproject_latlon(np.array((az0, el0)))
# already normalized
testutils.confirm_equal( v0_rect, v0,
def synthesize_board_observations(models,
object_width_n,
object_height_n,
object_spacing,
calobject_warp,
rt_ref_boardcenter,
rt_ref_boardcenter__noiseradius,
Nframes,
which='all-cameras-must-see-full-board'):
r'''Produce synthetic chessboard observations
SYNOPSIS
models = [mrcal.cameramodel("0.cameramodel"),
mrcal.cameramodel("1.cameramodel"),]
# shapes (Nframes, Ncameras, object_height_n, object_width_n, 2) and
# (Nframes, 4, 3)
q,Rt_ref_boardref = \
mrcal.synthesize_board_observations( \
models,
# board geometry
10,12,0.1,None,
# mean board pose and the radius of the added uniform noise
rt_ref_boardcenter,
rt_ref_boardcenter__noiseradius,
# How many frames we want
100,
which = 'some-cameras-must-see-half-board')
# q now contains the synthetic pixel observations, but some of them will be
# out of view. I construct an (x,y,weight) observations array, as expected
# by the optimizer, and I set the weight for the out-of-view points to -1 to
# tell the optimizer to ignore those points
# Set the weights to 1 initially
# shape (Nframes, Ncameras, object_height_n, object_width_n, 3)
observations = nps.glue(q,
np.ones( q.shape[:-1] + (1,) ),
axis = -1)
# shape (Ncameras, 1, 1, 2)
imagersizes = nps.mv( nps.cat(*[ m.imagersize() for m in models ]),
-2, -4 )
observations[ np.any( q < 0, axis=-1 ), 2 ] = -1.
observations[ np.any( q-imagersizes >= 0, axis=-1 ), 2 ] = -1.
Given a description of a calibration object and of the cameras observing it,
produces perfect pixel observations of the objects by those cameras. We return a
dense observation array: every corner observation from every chessboard pose
will be reported for every camera. Some of these observations MAY be
out-of-view, depending on the value of the 'which' argument; see description
below. The example above demonstrates how to mark such out-of-bounds
observations as outliers to tell the optimization to ignore these.
The "models" provides the intrinsics and extrinsics.
The calibration objects are nominally have pose rt_ref_boardcenter in the
reference coordinate system, with each pose perturbed uniformly with radius
rt_ref_boardcenter__noiseradius. This is nonstandard since here I'm placing the
board origin at its center instead of the corner (as
mrcal.ref_calibration_object() does). But this is more appropriate to the usage
of this function. The returned Rt_ref_boardref transformation DOES use the
normal corner-referenced board geometry
Returns the point observations and the chessboard poses that produced these
observations.
ARGUMENTS
- models: an array of mrcal.cameramodel objects, one for each camera we're
simulating. This is the intrinsics and the extrinsics. Ncameras = len(models)
- object_width_n: the number of horizontal points in the calibration object grid
- object_height_n: the number of vertical points in the calibration object grid
- object_spacing: the distance between adjacent points in the calibration
object. A square object is assumed, so the vertical and horizontal distances
are assumed to be identical.
- calobject_warp: a description of the calibration board warping. None means "no
warping": the object is flat. Otherwise this is an array of shape (2,). See
the docs for ref_calibration_object() for the meaning of the values in this
array.
- rt_ref_boardcenter: the nominal pose of the calibration object, in the
reference coordinate system. This is an rt transformation from a
center-referenced calibration object to the reference coordinate system
- rt_ref_boardcenter__noiseradius: the deviation-from-nominal for the chessboard
pose for each frame. I add uniform noise to rt_ref_boardcenter, with each
element sampled independently, with the radius given here.
- Nframes: how many frames of observations to return
- which: a string, defaulting to 'all-cameras-must-see-full-board'. Controls the
requirements on the visibility of the returned points. Valid values:
- 'all-cameras-must-see-full-board': We return only those chessboard poses
that produce observations that are FULLY visible by ALL the cameras.
- 'some-cameras-must-see-full-board': We return only those chessboard poses
that produce observations that are FULLY visible by AT LEAST ONE camera.
- 'all-cameras-must-see-half-board': We return only those chessboard poses
that produce observations that are AT LEAST HALF visible by ALL the cameras.
- 'some-cameras-must-see-half-board': We return only those chessboard poses
that produce observations that are AT LEAST HALF visible by AT LEAST ONE
camera.
RETURNED VALUES
We return a tuple:
- q: an array of shape (Nframes, Ncameras, object_height, object_width, 2)
containing the pixel coordinates of the generated observations
- Rt_ref_boardref: an array of shape (Nframes, 4,3) containing the poses of the
chessboards. This transforms the object returned by ref_calibration_object()
to the pose that was projected, in the ref coord system
'''
# Can visualize results with this script:
r'''
r = np.array((30, 0, 0,), dtype=float) * np.pi/180.
model = mrcal.cameramodel( intrinsics = ('LENSMODEL_PINHOLE',
np.array((1000., 1000., 1000., 1000.,))),
imagersize = np.array((2000,2000)) )
Rt_ref_boardref = \
mrcal.synthesize_board_observations([model],
5,20,0.1,None,
nps.glue(r, np.array((0,0,3.)), axis=-1),
np.array((0,0,0., 0,0,0)),
1) [1]
mrcal.show_geometry( models_or_extrinsics_rt_fromref = np.zeros((1,1,6), dtype=float),
frames_rt_toref = mrcal.rt_from_Rt(Rt_ref_boardref),
object_width_n = 20,
object_height_n = 5,
object_spacing = 0.1,
_set = 'xyplane 0',
wait = 1 )
'''
which_valid = (
'all-cameras-must-see-full-board',
'some-cameras-must-see-full-board',
'all-cameras-must-see-half-board',
'some-cameras-must-see-half-board',
)
if not which in which_valid:
raise Exception(f"'which' argument must be one of {which_valid}")
Ncameras = len(models)
# I move the board, and keep the cameras stationary.
#
# Camera coords: x,y with pixels, z forward
# Board coords: x,y in-plane. z forward (i.e. back towards the camera)
# The center of the board is at the origin (ignoring warping)
board_center = \
np.array(( (object_width_n -1)*object_spacing/2.,
(object_height_n-1)*object_spacing/2.,
0 ))
# shape: (Nh,Nw,3)
board_reference = \
mrcal.ref_calibration_object(object_width_n,object_height_n,
object_spacing,calobject_warp) - \
board_center
# Transformation from the board returned by ref_calibration_object() to
# the one I use here. It's a shift to move the origin to the center of the
# board
Rt_boardref_origboardref = mrcal.identity_Rt()
Rt_boardref_origboardref[3, :] = -board_center
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 cull_out_of_view(q, Rt_ref_boardref, which):
# q has shape (Nframes,Ncameras,Nh,Nw,2)
# Rt_ref_boardref has shape (Nframes,4,3)
# I pick only those frames where at least one cameras sees the whole
# board
# shape (Nframes,Ncameras,Nh,Nw)
mask_visible = (q[..., 0] >= 0) * (q[..., 1] >= 0)
for i in range(Ncameras):
W, H = models[i].imagersize()
mask_visible[:,i,...] *= \
(q[:,i,:,:,0] <= W-1) * \
(q[:,i,:,:,1] <= H-1)
# shape (Nframes, Ncameras)
Nvisible = np.count_nonzero(mask_visible, axis=(-1, -2))
Nh, Nw = q.shape[2:4]
if which == 'all-cameras-must-see-full-board':
iframe = np.all(Nvisible == Nh * Nw, axis=-1)
elif which == 'some-cameras-must-see-full-board':
iframe = np.any(Nvisible == Nh * Nw, axis=-1)
elif which == 'all-cameras-must-see-half-board':
iframe = np.all(Nvisible > Nh * Nw // 2, axis=-1)
elif which == 'some-cameras-must-see-half-board':
iframe = np.any(Nvisible > Nh * Nw // 2, axis=-1)
else:
raise Exception(
"Unknown 'which' argument. This is a bug. I checked for the valid options at the top of this function"
)
# q has shape (Nframes_inview,Ncameras,Nh*Nw,2)
# Rt_ref_boardref has shape (Nframes_inview,4,3)
return q[iframe, ...], Rt_ref_boardref[iframe, ...]
# shape (Nframes_sofar,Ncameras,Nh,Nw,2)
q = np.zeros((0, Ncameras, object_height_n, object_width_n, 2),
dtype=float)
# shape (Nframes_sofar,4,3)
Rt_ref_boardref = np.zeros((0, 4, 3), dtype=float)
# I keep creating data, until I get Nframes-worth of in-view observations
while True:
q_here, Rt_ref_boardref_here = get_observation_chunk()
q_here, Rt_ref_boardref_here = \
cull_out_of_view(q_here, Rt_ref_boardref_here,
which)
q = nps.glue(q, q_here, axis=-5)
Rt_ref_boardref = nps.glue(Rt_ref_boardref,
Rt_ref_boardref_here,
axis=-3)
if q.shape[0] >= Nframes:
q = q[:Nframes, ...]
Rt_ref_boardref = Rt_ref_boardref[:Nframes, ...]
break
return q, mrcal.compose_Rt(Rt_ref_boardref, Rt_boardref_origboardref)
testutils.confirm_equal(mrcal.invert_Rt(
mrcal.Rt_from_rt(mrcal.invert_rt(mrcal.rt_from_Rt(Rt)))),
Rt,
msg='Rt/rt and invert')
testutils.confirm_equal(mrcal.compose_Rt(Rt, mrcal.invert_Rt(Rt)),
nps.glue(np.eye(3), np.zeros((3, )), axis=-2),
msg='compose_Rt')
testutils.confirm_equal(mrcal.compose_rt(mrcal.rt_from_Rt(Rt),
mrcal.invert_rt(
mrcal.rt_from_Rt(Rt))),
np.zeros((6, )),
msg='compose_rt')
testutils.confirm_equal(mrcal.identity_Rt(),
nps.glue(np.eye(3), np.zeros((3, )), axis=-2),
msg='identity_Rt')
testutils.confirm_equal(mrcal.identity_rt(),
np.zeros((6, )),
msg='identity_rt')
testutils.confirm_equal(mrcal.transform_point_Rt(Rt, p),
Tp,
msg='transform_point_Rt')
testutils.confirm_equal(mrcal.transform_point_rt(mrcal.rt_from_Rt(Rt), p),
Tp,
msg='transform_point_rt')
rt01 = np.array((0, 0, 0, 3.0, 0, 0))
model1.extrinsics_rt_toref(mrcal.compose_rt(model0.extrinsics_rt_toref(),
rt01))
az_fov_deg = 90
el_fov_deg = 50
rectification_maps,cookie = \
mrcal.stereo_rectify_prepare( (model0, model1),
az_fov_deg = az_fov_deg,
el_fov_deg = el_fov_deg,
pixels_per_deg_az = -1./8.,
pixels_per_deg_el = -1./4.)
Rt_cam0_stereo = cookie['Rt_cam0_stereo']
testutils.confirm_equal(Rt_cam0_stereo,
mrcal.identity_Rt(),
msg='vanilla stereo has a vanilla geometry')
testutils.confirm_equal(cookie['baseline'],
nps.mag(rt01[3:]),
msg='vanilla stereo: baseline')
q0, q0x, q0y = mrcal.project(
np.array(((0, 0, 1.), (1e-6, 0, 1.), (0, 1e-6, 1.))), *model0.intrinsics())
testutils.confirm_equal(cookie['pixels_per_deg_az'] * 8.,
(q0x - q0)[0] / 1e-6 * np.pi / 180.,
msg='vanilla stereo: correct az pixel density')
testutils.confirm_equal(cookie['pixels_per_deg_el'] * 4.,
(q0y - q0)[1] / 1e-6 * np.pi / 180.,
pixels_per_deg_el = -1./4.)
try:
mrcal.stereo._validate_models_rectified(models_rectified)
testutils.confirm(True,
msg='Generated models pass validation')
except:
testutils.confirm(False,
msg='Generated models pass validation')
Rt_cam0_stereo = mrcal.compose_Rt( model0.extrinsics_Rt_fromref(),
models_rectified[0].extrinsics_Rt_toref())
Rt01_rectified = mrcal.compose_Rt( models_rectified[0].extrinsics_Rt_fromref(),
models_rectified[1].extrinsics_Rt_toref())
fxycxy = models_rectified[0].intrinsics()[1]
testutils.confirm_equal(Rt_cam0_stereo, mrcal.identity_Rt(),
msg='vanilla stereo has a vanilla geometry')
testutils.confirm_equal( Rt01_rectified[3,0],
nps.mag(rt01[3:]),
msg='vanilla stereo: baseline')
q0,q0x,q0y = mrcal.project( np.array(((0, 0,1.),
(1e-6, 0,1.),
(0, 1e-6, 1.))), *model0.intrinsics() )
testutils.confirm_equal(fxycxy[0] * np.pi/180. * 8.,
(q0x-q0)[0] / 1e-6 * np.pi/180.,
msg='vanilla stereo: correct az pixel density',
eps = 0.05)