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
def make_noisy_inputs():
r'''Construct incomplete, noisy observations to feed to the solver'''
# The seed points array is the true array, but corrupted by noise. All the
# points are observed at some point
#print(repr((np.random.random(points.shape)-0.5)/3))
points_noise = np.array([[-0.16415198, 0.10697666, 0.07137079],
[-0.02353459, 0.07269802, 0.05804911],
[-0.05218085, -0.09302461, -0.16626839],
[0.03649283, -0.04345566, -0.1589429],
[-0.05530528, 0.03942736, -0.02755858],
[-0.16252387, 0.07792151, -0.12200266],
[-0.02611094, -0.13695699, 0.06799326]])
points_noisy = ref_p * (1. + points_noise)
Ncamposes, Npoints = ref_p_cam.shape[:2]
ipoints = indices_point_camintrinsics_camextrinsics[:, 0]
icamposes = indices_point_camintrinsics_camextrinsics[:, 2]
ref_q_cam_indexed = nps.clump(ref_q_cam,
n=2)[icamposes * Npoints + ipoints, :]
#print(repr(np.random.randn(*ref_q_cam_indexed.shape) * 1.0))
q_cam_noise = np.array([[-0.40162837, -0.60884836],
[-0.65186956, -2.23240529],
[0.40217293,
-0.40160168], [2.05376895, -1.47389235],
[-0.01090807, 0.35468639],
[-0.37916168, -1.06052742],
[-0.08546853, -2.69946391],
[0.76133345, -1.38759769],
[-1.05998307,
-0.27779779], [-2.22203688, 1.47809028],
[1.68526798, 0.83635394], [1.26203342, 2.58905488],
[1.18282463, -0.41362789],
[0.41615768, 2.06621809], [0.27271605, 1.19721072],
[-1.48421641,
3.20841776], [1.10563011, 0.38313526],
[0.25591618,
-0.97987565], [-0.2431585, -1.34797656],
[1.57805536,
-0.26467537], [1.23762306, 0.94616712],
[0.29441229, -0.78921128],
[-1.33799634, -1.65173241],
[-0.24854348, -0.14145806]])
q_cam_indexed_noisy = ref_q_cam_indexed + q_cam_noise
observations = nps.glue(q_cam_indexed_noisy,
nps.transpose(
np.ones((q_cam_indexed_noisy.shape[0], ))),
axis=-1)
#print(repr((np.random.random(ref_extrinsics_rt_fromref.shape)-0.5)/10))
extrinsics_rt_fromref_noise = \
np.array([[-0.00781127, -0.04067386, -0.01039731, 0.02057068, -0.0461704 , 0.02112582],
[-0.02466267, -0.01445134, -0.01290107, -0.01956848, 0.04604318, 0.0439563 ],
[-0.02335697, 0.03171099, -0.00900416, -0.0346394 , -0.0392821 , 0.03892269],
[ 0.00229462, -0.01716853, 0.01336239, -0.0228473 , -0.03919978, 0.02671576],
[ 0.03782446, -0.016981 , 0.03949906, -0.03256744, 0.02496247, 0.02924358]])
extrinsics_rt_fromref_noisy = ref_extrinsics_rt_fromref * (
1.0 + extrinsics_rt_fromref_noise)
return extrinsics_rt_fromref_noisy, points_noisy, observations
msg="Gradient check: dp_triangulated_dpstate[extrinsics0]")
if istate_e1 is not None:
testutils.confirm_equal(
dp_triangulated_dpstate[..., istate_e1:istate_e1 + 6],
dp_triangulated_de1_empirical,
relative=True,
worstcase=True,
eps=5e-3,
msg="Gradient check: dp_triangulated_dpstate[extrinsics1]")
if optimization_inputs_baseline.get('do_optimize_frames'):
istate_f0 = mrcal.state_index_frames(0, **optimization_inputs_baseline)
Nstate_frames = mrcal.num_states_frames(**optimization_inputs_baseline)
testutils.confirm_equal(dp_triangulated_dpstate[..., istate_f0:istate_f0 +
Nstate_frames],
nps.clump(dp_triangulated_drtrf_empirical, n=-2),
relative=True,
worstcase=True,
eps=0.1,
msg="Gradient check: dp_triangulated_drtrf")
# dp_triangulated_dq_empirical has shape (Npoints,3, Npoints,Ncameras,2)
# The cross terms (p_triangulated(point=A), q(point=B)) should all be zero
dp_triangulated_dq_empirical_cross_only = dp_triangulated_dq_empirical
dp_triangulated_dq_empirical = np.zeros((Npoints, 3, 2, 2), dtype=float)
dp_triangulated_dq = np.zeros((Npoints, 3, 2, 2), dtype=float)
dp_triangulated_dq_flattened = nps.clump(dp_triangulated_dq, n=-2)
for ipt in range(Npoints):
dp_triangulated_dq_empirical[
def quat_from_R(R, out=None):
r"""Convert a rotation defined as a rotation matrix to a unit quaternion
SYNOPSIS
print(R.shape)
===>
(3,3)
quat = mrcal.quat_from_R(R)
print(quat.shape)
===>
(4,)
c = quat[0]
s = nps.mag(quat[1:])
rotation_magnitude = 2. * np.arctan2(s,c)
rotation_axis = quat[1:] / s
This is mostly for compatibility with some old stuff. mrcal doesn't use
quaternions anywhere. Test this thoroughly before using.
This function supports broadcasting fully.
ARGUMENTS
- R: array of shape (3,3,). The rotation matrix that defines the rotation.
- out: optional argument specifying the destination. By default, new numpy
array(s) are created and returned. To write the results into existing (and
possibly non-contiguous) arrays, specify them with the 'out' kwarg. If 'out'
is given, we return the 'out' that was passed in. This is the standard
behavior provided by numpysane_pywrap.
RETURNED VALUE
We return an array of unit quaternions. Each broadcasted slice has shape (4,).
The values in the array are (u,i,j,k)
LICENSE AND COPYRIGHT
The implementation comes directly from the scipy project, the from_dcm()
function in
https://github.com/scipy/scipy/blob/master/scipy/spatial/transform/rotation.py
Commit: 1169d27ad47a29abafa8a3d2cb5b67ff0df80a8f
License:
Copyright (c) 2001-2002 Enthought, Inc. 2003-2019, SciPy Developers.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
1. Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above
copyright notice, this list of conditions and the following
disclaimer in the documentation and/or other materials provided
with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
# extra broadcasted shape
extra_dims = R.shape[:-2]
# R.shape = (..., 3,3) with some non-empty ...
R = nps.atleast_dims(R, -3)
# R.shape = (N,3,3)
R = nps.clump(R, n=R.ndim - 2)
num_rotations = R.shape[0]
decision_matrix = np.empty((num_rotations, 4))
decision_matrix[:, :3] = R.diagonal(axis1=1, axis2=2)
decision_matrix[:, -1] = decision_matrix[:, :3].sum(axis=1)
choices = decision_matrix.argmax(axis=1)
s = (num_rotations, 4)
if out is not None:
quat = out.reshape(s)
def base(x):
r'''Base function, that returns the self array if it's not a view'''
return x if x.base is None else x.base
if base(quat) is not base(out):
raise Exception(
"quat_from_R() in-place output isn't yet complete. Please fix")
else:
quat = np.empty(s)
ind = np.nonzero(choices != 3)[0]
i = choices[ind]
j = (i + 1) % 3
k = (j + 1) % 3
quat[ind, i + 1] = 1 - decision_matrix[ind, -1] + 2 * R[ind, i, i]
quat[ind, j + 1] = R[ind, j, i] + R[ind, i, j]
quat[ind, k + 1] = R[ind, k, i] + R[ind, i, k]
quat[ind, 0] = R[ind, k, j] - R[ind, j, k]
ind = np.nonzero(choices == 3)[0]
quat[ind, 1] = R[ind, 2, 1] - R[ind, 1, 2]
quat[ind, 2] = R[ind, 0, 2] - R[ind, 2, 0]
quat[ind, 3] = R[ind, 1, 0] - R[ind, 0, 1]
quat[ind, 0] = 1 + decision_matrix[ind, -1]
quat /= np.linalg.norm(quat, axis=1)[:, None]
return quat.reshape(extra_dims + (4, ))
object_width_n, object_height_n, object_spacing,
calobject_warp_ref,
np.array((0., 0., 0., -2, 0, 4.0)),
np.array((np.pi/180.*30., np.pi/180.*30., np.pi/180.*20., 2.5, 2.5, 2.0)),
Nframes)
############# I have perfect observations in q_ref. I corrupt them by noise
# weight has shape (Nframes, Ncameras, Nh, Nw),
weight01 = (np.random.rand(*q_ref.shape[:-1]) + 1.) / 2. # in [0,1]
weight0 = 0.2
weight1 = 1.0
weight = weight0 + (weight1 - weight0) * weight01
# I want observations of shape (Nframes*Ncameras, Nh, Nw, 3) where each row is
# (x,y,weight)
observations_ref = nps.clump(nps.glue(q_ref, nps.dummy(weight, -1), axis=-1),
n=2)
# These are perfect
intrinsics_ref = nps.cat(*[m.intrinsics()[1] for m in models_ref])
extrinsics_ref = nps.cat(*[m.extrinsics_rt_fromref() for m in models_ref[1:]])
if extrinsics_ref.size == 0:
extrinsics_ref = np.zeros((0, 6), dtype=float)
frames_ref = mrcal.rt_from_Rt(Rt_ref_board_ref)
# Dense observations. All the cameras see all the boards
indices_frame_camera = np.zeros((Nframes * Ncameras, 2), dtype=np.int32)
indices_frame = indices_frame_camera[:, 0].reshape(Nframes, Ncameras)
indices_frame.setfield(nps.outer(np.arange(Nframes, dtype=np.int32),
np.ones((Ncameras, ), dtype=np.int32)),
dtype=np.int32)
indices_camera = indices_frame_camera[:, 1].reshape(Nframes, Ncameras)
do_optimize_extrinsics=True,
do_optimize_frames=True,
do_optimize_calobject_warp=True,
do_apply_regularization=False,
outlier_indices=np.array((1, 2), dtype=np.int32)))
itest = 0
for kwargs in all_test_kwargs:
observations_copy = observations.copy()
if 'outlier_indices' in kwargs:
# mark the requested outliers, and delete the old way of specifying
# these
for i in kwargs['outlier_indices']:
nps.clump(observations_copy, n=3)[i, 2] = -1.
del kwargs['outlier_indices']
optimization_inputs = \
dict( intrinsics = intrinsics_data,
extrinsics_rt_fromref = nps.atleast_dims(extrinsics_rt_fromref, -2),
frames_rt_toref = frames_rt_toref,
points = points,
observations_board = observations_copy,
indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics,
observations_point = observations_point,
indices_point_camintrinsics_camextrinsics = indices_point_camintrinsics_camextrinsics,
lensmodel = lensmodel,
calobject_warp = np.array((1e-3, 2e-3)),
imagersizes = imagersizes,
calibration_object_spacing = 0.1,
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
axis=-3)
args.Nframes += 1
frames_true = mrcal.rt_from_Rt(Rt_cam0_board_true)
############# I have perfect observations in q_true. I corrupt them by noise
# weight has shape (Nframes, Ncameras, Nh, Nw),
weight01 = (np.random.rand(*q_true.shape[:-1]) + 1.) / 2. # in [0,1]
weight0 = 0.2
weight1 = 1.0
weight = weight0 + (weight1 - weight0) * weight01
# I want observations of shape (Nframes*Ncameras, Nh, Nw, 3) where each row is
# (x,y,weight)
observations_true = nps.clump(nps.glue(q_true, nps.dummy(weight, -1), axis=-1),
n=2)
# Dense observations. All the cameras see all the boards
indices_frame_camera = np.zeros((args.Nframes * args.Ncameras, 2),
dtype=np.int32)
indices_frame = indices_frame_camera[:, 0].reshape(args.Nframes, args.Ncameras)
indices_frame.setfield(nps.outer(np.arange(args.Nframes, dtype=np.int32),
np.ones((args.Ncameras, ), dtype=np.int32)),
dtype=np.int32)
indices_camera = indices_frame_camera[:, 1].reshape(args.Nframes,
args.Ncameras)
indices_camera.setfield(nps.outer(np.ones((args.Nframes, ), dtype=np.int32),
np.arange(args.Ncameras, dtype=np.int32)),
dtype=np.int32)
indices_frame_camintrinsics_camextrinsics = \
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 pinhole_model_for_reprojection(model_from,
fit=None,
scale_focal=None,
scale_image=None):
r'''Generate a pinhole model suitable for reprojecting an image
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)
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. THIS
function produces the pinhole camera model based on some guidance in the
arguments, and this model can then be used to "undistort" images.
ARGUMENTS
- model_from: the mrcal.cameramodel object used to build the pinhole model. We
use the intrinsics as the baseline, and we copy the extrinsics to the
resulting pinhole model.
- fit: optional specification for focal-length scaling. By default we use the
focal length values from the input model. This is either a numpy array of
shape (...,2) containing pixel coordinates that the resulting pinhole model
must represent, or one of ("corners","centers-horizontal","centers-vertical").
See the docstring for scale_focal__best_pinhole_fit() for details. Exclusive
with 'scale_focal'
- scale_focal: optional specification for focal-length scaling. By default we
use the focal length values from the input model. If given, we scale the input
focal lengths by the given value. Exclusive with 'fit'
- scale_image: optional specification for the scaling of the image size. By
default the output model represents an image of the same resolution as the
input model. If we want something else, the scaling can be given here.
RETURNED VALUE
A mrcal.cameramodel object with lensmodel = LENSMODEL_PINHOLE corresponding to
the input model.
'''
if scale_focal is None:
if fit is not None:
if isinstance(fit, np.ndarray):
if fit.shape[-1] != 2:
raise Exception(
"'fit' is an array, so it must have shape (...,2)")
fit = nps.atleast_dims(fit, -2)
fit = nps.clump(fit, n=len(fit.shape) - 1)
# fit now has shape (N,2)
elif re.match("^(corners|centers-horizontal|centers-vertical)$",
fit):
# this is valid. nothing to do
pass
else:
raise Exception(
"'fit' must be an array of shape (...,2) or one of ('corners','centers-horizontal','centers-vertical')",
file=sys.stderr)
sys.exit(1)
scale_focal = mrcal.scale_focal__best_pinhole_fit(model_from, fit)
else:
if fit is not None:
raise Exception(
"At most one of 'scale_focal' and 'fit' may be non-None")
# I have some scale_focal now. I apply it
lensmodel, intrinsics_data = model_from.intrinsics()
imagersize = model_from.imagersize()
if not mrcal.lensmodel_metadata_and_config(lensmodel)['has_core']:
raise Exception(
"This currently works only with models that have an fxfycxcy core")
cx, cy = intrinsics_data[2:4]
intrinsics_data[:2] *= scale_focal
if scale_image is not None:
# Now I apply the imagersize scale. The center of the imager should
# unproject to the same point:
#
# (q0 - cxy0)/fxy0 = v = (q1 - cxy1)/fxy1
# ((WH-1)/2 - cxy) / fxy = (((ki*WH)-1)/2 - kc*cxy) / (kf*fxy)
#
# The focal lengths scale directly: kf = ki
# ((WH-1)/2 - cxy) / fxy = (((ki*WH)-1)/2 - kc*cxy) / (ki*fxy)
# (WH-1)/2 - cxy = (((ki*WH)-1)/2 - kc*cxy) / ki
# (WH-1)/2 - cxy = (WH-1/ki)/2 - kc/ki*cxy
# -1/2 - cxy = (-1/ki)/2 - kc/ki*cxy
# 1/2 + cxy = 1/(2ki) + kc/ki*cxy
# -> kc = (1/2 + cxy - 1/(2ki)) * ki / cxy
# = (ki + 2*cxy*ki - 1) / (2 cxy)
#
# Sanity check: cxy >> 1: ki+2*cxy*ki = ki*(1+2cxy) ~ 2*cxy*ki
# 2*cxy*ki - 1 ~ 2*cxy*ki
# -> kc ~ 2*cxy*ki /( 2 cxy ) = ki. Yes.
# Looks like I scale cx and cy separately.
imagersize[0] = round(imagersize[0] * scale_image)
imagersize[1] = round(imagersize[1] * scale_image)
kfxy = scale_image
kcx = (kfxy + 2. * cx * kfxy - 1.) / (2. * cx)
kcy = (kfxy + 2. * cy * kfxy - 1.) / (2. * cy)
intrinsics_data[:2] *= kfxy
intrinsics_data[2] *= kcx
intrinsics_data[3] *= kcy
return \
mrcal.cameramodel( intrinsics = ('LENSMODEL_PINHOLE',intrinsics_data[:4]),
extrinsics_rt_fromref = model_from.extrinsics_rt_fromref(),
imagersize = imagersize )
def calibration_baseline(model,
Ncameras,
Nframes,
extra_observation_at,
object_width_n,
object_height_n,
object_spacing,
extrinsics_rt_fromref_true,
calobject_warp_true,
fixedframes,
testdir,
cull_left_of_center=False,
allow_nonidentity_cam0_transform=False,
range_to_boards=4.0):
r'''Compute a calibration baseline as a starting point for experiments
This is a perfect, noiseless solve. Regularization IS enabled, and the returned
model is at the optimization optimum. So the returned models will not sit
exactly at the ground-truth.
NOTE: if not fixedframes: the ref frame in the returned
optimization_inputs_baseline is NOT the ref frame used by the returned
extrinsics and frames arrays. The arrays in optimization_inputs_baseline had to
be transformed to reference off camera 0. If the extrinsics of camera 0 are the
identity, then the two ref coord systems are the same. To avoid accidental bugs,
we have a kwarg allow_nonidentity_cam0_transform, which defaults to False. if
not allow_nonidentity_cam0_transform and norm(extrinsics_rt_fromref_true[0]) >
0: raise
This logic is here purely for safety. A caller that handles non-identity cam0
transforms has to explicitly say that
ARGUMENTS
- model: string. 'opencv4' or 'opencv8' or 'splined'
- ...
'''
if re.match('opencv', model):
models_true = (
mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.opencv8.cameramodel"))
if model == 'opencv4':
# I have opencv8 models_true, but I truncate to opencv4 models_true
for m in models_true:
m.intrinsics(intrinsics=('LENSMODEL_OPENCV4',
m.intrinsics()[1][:8]))
elif model == 'splined':
models_true = (
mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.splined.cameramodel"))
else:
raise Exception("Unknown lens being tested")
models_true = models_true[:Ncameras]
lensmodel = models_true[0].intrinsics()[0]
Nintrinsics = mrcal.lensmodel_num_params(lensmodel)
for i in range(Ncameras):
models_true[i].extrinsics_rt_fromref(extrinsics_rt_fromref_true[i])
if not allow_nonidentity_cam0_transform and \
nps.norm2(extrinsics_rt_fromref_true[0]) > 0:
raise Exception(
"A non-identity cam0 transform was given, but the caller didn't explicitly say that they support this"
)
imagersizes = nps.cat(*[m.imagersize() for m in models_true])
# These are perfect
intrinsics_true = nps.cat(*[m.intrinsics()[1] for m in models_true])
extrinsics_true_mounted = nps.cat(
*[m.extrinsics_rt_fromref() for m in models_true])
x_center = -(Ncameras - 1) / 2.
# shapes (Nframes, Ncameras, Nh, Nw, 2),
# (Nframes, 4,3)
q_true,Rt_ref_board_true = \
mrcal.synthesize_board_observations(models_true,
object_width_n, object_height_n, object_spacing,
calobject_warp_true,
np.array((0., 0., 0., x_center, 0, range_to_boards)),
np.array((np.pi/180.*30., np.pi/180.*30., np.pi/180.*20., 2.5, 2.5, range_to_boards/2.0)),
Nframes)
if extra_observation_at is not None:
q_true_extra,Rt_ref_board_true_extra = \
mrcal.synthesize_board_observations(models_true,
object_width_n, object_height_n, object_spacing,
calobject_warp_true,
np.array((0., 0., 0., x_center, 0, extra_observation_at)),
np.array((np.pi/180.*30., np.pi/180.*30., np.pi/180.*20., 2.5, 2.5, extra_observation_at/10.0)),
Nframes = 1)
q_true = nps.glue(q_true, q_true_extra, axis=-5)
Rt_ref_board_true = nps.glue(Rt_ref_board_true,
Rt_ref_board_true_extra,
axis=-3)
Nframes += 1
frames_true = mrcal.rt_from_Rt(Rt_ref_board_true)
############# I have perfect observations in q_true.
# weight has shape (Nframes, Ncameras, Nh, Nw),
weight01 = (np.random.rand(*q_true.shape[:-1]) + 1.) / 2. # in [0,1]
weight0 = 0.2
weight1 = 1.0
weight = weight0 + (weight1 - weight0) * weight01
if cull_left_of_center:
imagersize = models_true[0].imagersize()
for m in models_true[1:]:
if np.any(m.imagersize() - imagersize):
raise Exception(
"I'm assuming all cameras have the same imager size, but this is false"
)
weight[q_true[..., 0] < imagersize[0] / 2.] /= 1000.
# I want observations of shape (Nframes*Ncameras, Nh, Nw, 3) where each row is
# (x,y,weight)
observations_true = nps.clump(nps.glue(q_true,
nps.dummy(weight, -1),
axis=-1),
n=2)
# Dense observations. All the cameras see all the boards
indices_frame_camera = np.zeros((Nframes * Ncameras, 2), dtype=np.int32)
indices_frame = indices_frame_camera[:, 0].reshape(Nframes, Ncameras)
indices_frame.setfield(nps.outer(np.arange(Nframes, dtype=np.int32),
np.ones((Ncameras, ), dtype=np.int32)),
dtype=np.int32)
indices_camera = indices_frame_camera[:, 1].reshape(Nframes, Ncameras)
indices_camera.setfield(nps.outer(np.ones((Nframes, ), dtype=np.int32),
np.arange(Ncameras, dtype=np.int32)),
dtype=np.int32)
indices_frame_camintrinsics_camextrinsics = \
nps.glue(indices_frame_camera,
indices_frame_camera[:,(1,)],
axis=-1)
if not fixedframes:
indices_frame_camintrinsics_camextrinsics[:, 2] -= 1
###########################################################################
# p = mrcal.show_geometry(models_true,
# frames = frames_true,
# object_width_n = object_width_n,
# object_height_n = object_height_n,
# object_spacing = object_spacing)
# sys.exit()
# I now reoptimize the perfect-observations problem. Without regularization,
# this is a no-op: I'm already at the optimum. With regularization, this will
# move us a certain amount (that the test will evaluate). Then I look at
# noise-induced motions off this optimization optimum
optimization_inputs_baseline = \
dict( intrinsics = copy.deepcopy(intrinsics_true),
points = None,
observations_board = observations_true,
indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics,
observations_point = None,
indices_point_camintrinsics_camextrinsics = None,
lensmodel = lensmodel,
calobject_warp = copy.deepcopy(calobject_warp_true),
imagersizes = imagersizes,
calibration_object_spacing = object_spacing,
verbose = False,
do_optimize_frames = not fixedframes,
do_optimize_intrinsics_core = False if model =='splined' else True,
do_optimize_intrinsics_distortions = True,
do_optimize_extrinsics = True,
do_optimize_calobject_warp = True,
do_apply_regularization = True,
do_apply_outlier_rejection = False)
if fixedframes:
# Frames are fixed: each camera has an independent pose
optimization_inputs_baseline['extrinsics_rt_fromref'] = \
copy.deepcopy(extrinsics_true_mounted)
optimization_inputs_baseline['frames_rt_toref'] = copy.deepcopy(
frames_true)
else:
# Frames are NOT fixed: cam0 is fixed as the reference coord system. I
# transform each optimization extrinsics vector to be relative to cam0
optimization_inputs_baseline['extrinsics_rt_fromref'] = \
mrcal.compose_rt(extrinsics_true_mounted[1:,:],
mrcal.invert_rt(extrinsics_true_mounted[0,:]))
optimization_inputs_baseline['frames_rt_toref'] = \
mrcal.compose_rt(extrinsics_true_mounted[0,:], frames_true)
mrcal.optimize(**optimization_inputs_baseline)
models_baseline = \
[ mrcal.cameramodel( optimization_inputs = optimization_inputs_baseline,
icam_intrinsics = i) \
for i in range(Ncameras) ]
return \
optimization_inputs_baseline, \
models_true, models_baseline, \
indices_frame_camintrinsics_camextrinsics, \
lensmodel, Nintrinsics, imagersizes, \
intrinsics_true, extrinsics_true_mounted, frames_true, \
observations_true, \
Nframes
def 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)
dv1_dr01,
dr01_dr1r,
out = dp_triangulated_dpstate[..., istate_e1:istate_e1+3])
dp_triangulated_dpstate[..., istate_e1:istate_e1+3] += \
nps.matmult(dp_triangulated_dt01, dt01_dr1r)
# dp_triangulated_dt1r =
# dp_triangulated_dt01 dt01_dt1r
nps.matmult( dp_triangulated_dt01,
dt01_dt1r,
out = dp_triangulated_dpstate[..., istate_e1+3:istate_e1+6])
# dp_triangulated_drtrf has shape (Npoints,Nframes,3,6). I reshape to (Npoints,3,Nframes*6)
dp_triangulated_dpstate[..., istate_f0:istate_f0+Nstate_frames] = \
nps.clump(nps.xchg(dp_triangulated_drtrf,-2,-3), n=-2)
########## Gradient check
dp_triangulated_di0_empirical = grad(lambda i0: triangulate_nograd([i0, models_baseline[1].intrinsics()[1]],
[m.extrinsics_rt_fromref() for m in models_baseline],
optimization_inputs_baseline['frames_rt_toref'], frames_true,
q_true,
lensmodel,
stabilize_coords=args.stabilize_coords),
models_baseline[0].intrinsics()[1])
dp_triangulated_di1_empirical = grad(lambda i1: triangulate_nograd([models_baseline[0].intrinsics()[1],i1],
[m.extrinsics_rt_fromref() for m in models_baseline],
optimization_inputs_baseline['frames_rt_toref'], frames_true,
q_true,
lensmodel,
stabilize_coords=args.stabilize_coords),
def calibration_baseline(model,
Ncameras,
Nframes,
extra_observation_at,
pixel_uncertainty_stdev,
object_width_n,
object_height_n,
object_spacing,
extrinsics_rt_fromref_true,
calobject_warp_true,
fixedframes,
testdir,
cull_left_of_center=False):
r'''Compute a calibration baseline as a starting point for experiments
This is a perfect, noiseless solve. Regularization IS enabled, and the returned
model is at the optimization optimum. So the returned models will not sit
exactly at the ground-truth
ARGUMENTS
- model: string. 'opencv4' or 'opencv8' or 'splined'
- ...
'''
if re.match('opencv', model):
models_true = (
mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.opencv8.cameramodel"))
if model == 'opencv4':
# I have opencv8 models_true, but I truncate to opencv4 models_true
for m in models_true:
m.intrinsics(intrinsics=('LENSMODEL_OPENCV4',
m.intrinsics()[1][:8]))
elif model == 'splined':
models_true = (
mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.splined.cameramodel"))
else:
raise Exception("Unknown lens being tested")
models_true = models_true[:Ncameras]
lensmodel = models_true[0].intrinsics()[0]
Nintrinsics = mrcal.lensmodel_num_params(lensmodel)
for i in range(Ncameras):
models_true[i].extrinsics_rt_fromref(extrinsics_rt_fromref_true[i])
imagersizes = nps.cat(*[m.imagersize() for m in models_true])
# These are perfect
intrinsics_true = nps.cat(*[m.intrinsics()[1] for m in models_true])
extrinsics_true_mounted = nps.cat(
*[m.extrinsics_rt_fromref() for m in models_true])
x_center = -(Ncameras - 1) / 2.
# shapes (Nframes, Ncameras, Nh, Nw, 2),
# (Nframes, 4,3)
q_true,Rt_cam0_board_true = \
mrcal.synthesize_board_observations(models_true,
object_width_n, object_height_n, object_spacing,
calobject_warp_true,
np.array((0., 0., 0., x_center, 0, 4.0)),
np.array((np.pi/180.*30., np.pi/180.*30., np.pi/180.*20., 2.5, 2.5, 2.0)),
Nframes)
if extra_observation_at:
c = mrcal.ref_calibration_object(object_width_n, object_height_n,
object_spacing, calobject_warp_true)
Rt_cam0_board_true_far = \
nps.glue( np.eye(3),
np.array((0,0,extra_observation_at)),
axis=-2)
q_true_far = \
mrcal.project(mrcal.transform_point_Rt(Rt_cam0_board_true_far, c),
*models_true[0].intrinsics())
q_true = nps.glue(q_true_far, q_true, axis=-5)
Rt_cam0_board_true = nps.glue(Rt_cam0_board_true_far,
Rt_cam0_board_true,
axis=-3)
Nframes += 1
frames_true = mrcal.rt_from_Rt(Rt_cam0_board_true)
############# I have perfect observations in q_true. I corrupt them by noise
# weight has shape (Nframes, Ncameras, Nh, Nw),
weight01 = (np.random.rand(*q_true.shape[:-1]) + 1.) / 2. # in [0,1]
weight0 = 0.2
weight1 = 1.0
weight = weight0 + (weight1 - weight0) * weight01
if cull_left_of_center:
imagersize = models_true[0].imagersize()
for m in models_true[1:]:
if np.any(m.imagersize() - imagersize):
raise Exception(
"I'm assuming all cameras have the same imager size, but this is false"
)
weight[q_true[..., 0] < imagersize[0] / 2.] /= 1000.
# I want observations of shape (Nframes*Ncameras, Nh, Nw, 3) where each row is
# (x,y,weight)
observations_true = nps.clump(nps.glue(q_true,
nps.dummy(weight, -1),
axis=-1),
n=2)
# Dense observations. All the cameras see all the boards
indices_frame_camera = np.zeros((Nframes * Ncameras, 2), dtype=np.int32)
indices_frame = indices_frame_camera[:, 0].reshape(Nframes, Ncameras)
indices_frame.setfield(nps.outer(np.arange(Nframes, dtype=np.int32),
np.ones((Ncameras, ), dtype=np.int32)),
dtype=np.int32)
indices_camera = indices_frame_camera[:, 1].reshape(Nframes, Ncameras)
indices_camera.setfield(nps.outer(np.ones((Nframes, ), dtype=np.int32),
np.arange(Ncameras, dtype=np.int32)),
dtype=np.int32)
indices_frame_camintrinsics_camextrinsics = \
nps.glue(indices_frame_camera,
indices_frame_camera[:,(1,)],
axis=-1)
if not fixedframes:
indices_frame_camintrinsics_camextrinsics[:, 2] -= 1
###########################################################################
# Now I apply pixel noise, and look at the effects on the resulting calibration.
# p = mrcal.show_geometry(models_true,
# frames = frames_true,
# object_width_n = object_width_n,
# object_height_n = object_height_n,
# object_spacing = object_spacing)
# sys.exit()
# I now reoptimize the perfect-observations problem. Without regularization,
# this is a no-op: I'm already at the optimum. With regularization, this will
# move us a certain amount (that the test will evaluate). Then I look at
# noise-induced motions off this optimization optimum
optimization_inputs_baseline = \
dict( intrinsics = copy.deepcopy(intrinsics_true),
extrinsics_rt_fromref = copy.deepcopy(extrinsics_true_mounted if fixedframes else extrinsics_true_mounted[1:,:]),
frames_rt_toref = copy.deepcopy(frames_true),
points = None,
observations_board = observations_true,
indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics,
observations_point = None,
indices_point_camintrinsics_camextrinsics = None,
lensmodel = lensmodel,
calobject_warp = copy.deepcopy(calobject_warp_true),
imagersizes = imagersizes,
calibration_object_spacing = object_spacing,
verbose = False,
observed_pixel_uncertainty = pixel_uncertainty_stdev,
do_optimize_frames = not fixedframes,
do_optimize_intrinsics_core = False if model =='splined' else True,
do_optimize_intrinsics_distortions = True,
do_optimize_extrinsics = True,
do_optimize_calobject_warp = True,
do_apply_regularization = True,
do_apply_outlier_rejection = False)
mrcal.optimize(**optimization_inputs_baseline)
models_baseline = \
[ mrcal.cameramodel( optimization_inputs = optimization_inputs_baseline,
icam_intrinsics = i) \
for i in range(Ncameras) ]
return \
optimization_inputs_baseline, \
models_true, models_baseline, \
indices_frame_camintrinsics_camextrinsics, \
lensmodel, Nintrinsics, imagersizes, \
intrinsics_true, extrinsics_true_mounted, frames_true, \
observations_true, \
Nframes
