def model_matrix(q, order):
r'''Returns the model matrix S for particular domain points
Here the "order" is the number of parameters in the fit. Thus order==2 means
"linear" and order==3 means "quadratic""
'''
q = nps.atleast_dims(q,-1)
return nps.transpose(nps.cat(*[q ** i for i in range(order)]))
def triangulate_grad( intrinsics_data,
rt_cam_ref,
rt_ref_frame, rt_ref_frame_true,
q,
lensmodel,
stabilize_coords = True):
return _triangulate( intrinsics_data,
rt_cam_ref,
rt_ref_frame,
rt_ref_frame_true,
nps.atleast_dims(q,-3),
lensmodel,
stabilize_coords = stabilize_coords,
get_gradients = True)
[ 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
############### Do everything in the ref coord system, with a few fixed-position
############### points to set the coords
extrinsics_rt_fromref, points, observations = make_noisy_inputs()
# De-noise the fixed points. We know where they are exactly. And correctly
Npoints_fixed = 3
points[-Npoints_fixed:, ...] = ref_p[-Npoints_fixed:, ...]
stats = mrcal.optimize(nps.atleast_dims(intrinsics_data, -2),
extrinsics_rt_fromref,
None,
points,
None,
None,
observations,
indices_point_camintrinsics_camextrinsics,
lensmodel,
imagersizes=nps.atleast_dims(imagersize, -2),
Npoints_fixed=Npoints_fixed,
point_min_range=1.0,
point_max_range=1000.0,
do_optimize_intrinsics_core=False,
do_optimize_intrinsics_distortions=False,
do_optimize_extrinsics=True,
def triangulate_nograd(intrinsics_data0,
intrinsics_data1,
rt_cam0_ref,
rt_cam0_ref_baseline,
rt_cam1_ref,
rt_ref_frame,
rt_ref_frame_baseline,
q,
lensmodel,
stabilize_coords=True):
q = nps.atleast_dims(q, -3)
rt01 = mrcal.compose_rt(rt_cam0_ref, mrcal.invert_rt(rt_cam1_ref))
# all the v have shape (...,3)
vlocal0 = \
mrcal.unproject(q[...,0,:],
lensmodel, intrinsics_data0)
vlocal1 = \
mrcal.unproject(q[...,1,:],
lensmodel, intrinsics_data1)
v0 = vlocal0
v1 = \
mrcal.rotate_point_r(rt01[:3], vlocal1)
# The triangulated point in the perturbed camera-0 coordinate system.
# Calibration-time perturbations move this coordinate system, so to get
# a better estimate of the triangulation uncertainty, we try to
# transform this to the original camera-0 coordinate system; the
# stabilization path below does that.
#
# shape (..., 3)
p_triangulated0 = \
mrcal.triangulate_leecivera_mid2(v0, v1, rt01[3:])
if not stabilize_coords:
return p_triangulated0
# Stabilization path. This uses the "true" solution, so I cannot do
# this in the field. But I CAN do this in the randomized trials in
# the test. And I can use the gradients to propagate the uncertainty
# of this computation in the field
#
# Data flow:
# point_cam_perturbed -> point_ref_perturbed -> point_frames
# point_frames -> point_ref_baseline -> point_cam_baseline
p_cam0_perturbed = p_triangulated0
p_ref_perturbed = mrcal.transform_point_rt(rt_cam0_ref,
p_cam0_perturbed,
inverted=True)
# shape (..., Nframes, 3)
p_frames = \
mrcal.transform_point_rt(rt_ref_frame,
nps.dummy(p_ref_perturbed,-2),
inverted = True)
# shape (..., Nframes, 3)
p_ref_baseline_all = mrcal.transform_point_rt(rt_ref_frame_baseline,
p_frames)
# shape (..., 3)
p_ref_baseline = np.mean(p_ref_baseline_all, axis=-2)
# shape (..., 3)
return mrcal.transform_point_rt(rt_cam0_ref_baseline, p_ref_baseline)
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, ))
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,
point_min_range = 1.0,
point_max_range = 1000.0,
verbose = False,
**kwargs )
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 )