def reproject_perturbed__diff(
q,
distance,
# shape (Ncameras, Nintrinsics)
baseline_intrinsics,
# shape (Ncameras, 6)
baseline_rt_cam_ref,
# shape (Nframes, 6)
baseline_rt_ref_frame,
# shape (2)
baseline_calobject_warp,
# shape (Ncameras, Nintrinsics)
query_intrinsics,
# shape (Ncameras, 6)
query_rt_cam_ref,
# shape (Nframes, 6)
query_rt_ref_frame,
# shape (2)
query_calobject_warp):
r'''Reproject by using the "diff" method to compute a rotation
'''
# shape (Ncameras, 3)
p_cam_baseline = mrcal.unproject(
q, lensmodel, baseline_intrinsics, normalize=True) * distance
p_cam_query = np.zeros((args.Ncameras, 3), dtype=float)
for icam in range(args.Ncameras):
# This method only cares about the intrinsics
model_baseline = \
mrcal.cameramodel( intrinsics = (lensmodel, baseline_intrinsics[icam]),
imagersize = imagersizes[0] )
model_query = \
mrcal.cameramodel( intrinsics = (lensmodel, query_intrinsics[icam]),
imagersize = imagersizes[0] )
implied_Rt10_query = \
mrcal.projection_diff( (model_baseline,
model_query),
distance = distance,
use_uncertainties = False,
focus_center = None,
focus_radius = 1000.)[3]
mrcal.transform_point_Rt(implied_Rt10_query,
p_cam_baseline[icam],
out=p_cam_query[icam])
# shape (Ncameras, 2)
return \
mrcal.project( p_cam_query,
lensmodel, query_intrinsics)
# I import the LOCAL mrcal since that's what I'm testing
sys.path[:0] = f"{testdir}/..",
import mrcal
import testutils
from test_calibration_helpers import grad
import scipy.optimize
# I want the RNG to be deterministic
np.random.seed(0)
############### World layout
# camera0 is the "reference"
model0 = mrcal.cameramodel(intrinsics=('LENSMODEL_PINHOLE',
np.array((1000., 1000., 500., 500.))),
imagersize=np.array((1000, 1000)))
model1 = mrcal.cameramodel(intrinsics=('LENSMODEL_PINHOLE',
np.array((1100., 1100., 500., 500.))),
imagersize=np.array((1000, 1000)))
# All the callback functions can broadcast on p,v
@nps.broadcast_define(((3, ), (3, ), (3, ), (3, )), ())
def callback_l2_geometric(p, v0, v1, t01):
if p[2] < 0: return 1e6
distance_p_v0 = nps.mag(p - nps.inner(p, v0) / nps.norm2(v0) * v0)
distance_p_v1 = nps.mag(p - t01 -
nps.inner(p - t01, v1) / nps.norm2(v1) * v1)
return np.abs(distance_p_v0) + np.abs(distance_p_v1)
indices_frame_camintrinsics_camextrinsics = np.zeros(
(len(indices_frame_camera), 3), dtype=indices_frame_camera.dtype)
indices_frame_camintrinsics_camextrinsics[:, :2] = indices_frame_camera
indices_frame_camintrinsics_camextrinsics[:,
2] = indices_frame_camintrinsics_camextrinsics[:,
1] - 1
i = (1, 2, 4, 5)
observations = observations[i, ...]
indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics[
i, ...]
paths = [paths[_] for _ in i]
# reference models
models = [
mrcal.cameramodel(m) for m in (
f"{testdir}/data/cam0.opencv8.cameramodel",
f"{testdir}/data/cam1.opencv8.cameramodel",
)
]
lensmodel = models[0].intrinsics()[0]
intrinsics_data = nps.cat(models[0].intrinsics()[1], models[1].intrinsics()[1])
extrinsics_rt_fromref = mrcal.compose_rt(models[1].extrinsics_rt_fromref(),
models[0].extrinsics_rt_toref())
imagersizes = nps.cat(models[0].imagersize(), models[1].imagersize())
# I now have the "right" camera parameters. I don't have the frames or points,
# but it's fine to just make them up. This is a regression test.
frames_rt_toref = linspace_shaped(3, 6)
frames_rt_toref[:, 5] += 5 # push them back
testdir = os.path.dirname(os.path.realpath(__file__))
# I import the LOCAL mrcal since that's what I'm testing
sys.path[:0] = f"{testdir}/..",
import mrcal
import testutils
from test_calibration_helpers import sample_dqref
# I want the RNG to be deterministic
np.random.seed(0)
############# Set up my world, and compute all the perfect positions, pixel
############# observations of everything
models_ref = (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"))
imagersizes = nps.cat(*[m.imagersize() for m in models_ref])
lensmodel = models_ref[0].intrinsics()[0]
# I have opencv8 models_ref, but let me truncate to opencv4 models_ref to keep this
# simple and fast
lensmodel = 'LENSMODEL_OPENCV4'
for m in models_ref:
m.intrinsics(intrinsics=(lensmodel, m.intrinsics()[1][:8]))
Nintrinsics = mrcal.lensmodel_num_params(lensmodel)
Ncameras = len(models_ref)
Nframes = 50
#!/usr/bin/python3
import sys
import mrcal
import cv2
import numpy as np
# Read the models and images from the commandline arguments
try:
models = [
mrcal.cameramodel(sys.argv[1]) if sys.argv[1] != '-' else None,
mrcal.cameramodel(sys.argv[2]) if sys.argv[2] != '-' else None,
]
images = [ cv2.imread(sys.argv[i]) \
for i in (3,4) ]
kind = sys.argv[5]
except:
print(f"Usage: {sys.argv[0]} model0 model1 image0 image1 kind",
file=sys.stderr)
sys.exit(1)
if models[0] is None or models[1] is None:
images_rectified = images
else:
# Annotate the image with its valid-intrinsics region. This will end up in the
# rectified images, and make it clear where successful matching shouldn't be
# expected
for i in range(2):
try:
mrcal.annotate_image__valid_intrinsics_region(images[i], models[i])
except:
r'''Tests the python-wrapped C API
'''
import sys
import numpy as np
import numpysane as nps
import os
testdir = os.path.dirname(os.path.realpath(__file__))
# I import the LOCAL mrcal since that's what I'm testing
sys.path[:0] = f"{testdir}/..",
import mrcal
import testutils
model_splined = mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel")
ux, uy = mrcal.knots_for_splined_models(model_splined.intrinsics()[0])
testutils.confirm_equal(ux,
np.array([
-1.33234678, -1.15470054, -0.9770543, -0.79940807,
-0.62176183, -0.44411559, -0.26646936, -0.08882312,
0.08882312, 0.26646936, 0.44411559, 0.62176183,
0.79940807, 0.9770543, 1.15470054, 1.33234678
]),
msg=f"knots_for_splined_models ux")
testutils.confirm_equal(uy,
np.array([
-0.88823118, -0.71058495, -0.53293871, -0.35529247,
-0.17764624, 0., 0.17764624, 0.35529247,
0.53293871, 0.71058495, 0.88823118
]),
pass
atexit.register(cleanup)
# I do this:
# load file
# compare with hardcoded
# save
# load again
# compare with hardcoded
#
# modify with setter
# call getter and compare
m = mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel")
testutils.confirm_equal(m.extrinsics_rt_fromref(), [
2e-2,
-3e-1,
-1e-2,
1.,
2,
-3.,
])
testutils.confirm_equal(m.intrinsics()[0], 'LENSMODEL_OPENCV8')
testutils.confirm_equal(m.intrinsics()[1], [
1761.181055,
1761.250444,
1965.706996,
1087.518797,
'''
import sys
import numpy as np
import numpysane as nps
import os
testdir = os.path.dirname(os.path.realpath(__file__))
# I import the LOCAL mrcal since that's what I'm testing
sys.path[:0] = f"{testdir}/..",
import mrcal
import testutils
model_opencv8 = mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel")
model_splined = mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel")
gridn_width = 50
########## Compare the model to itself. I should get 0 diff and identity transform
difflen, diff, q0, implied_Rt10 = \
mrcal.projection_diff( (model_splined,model_splined),
gridn_width = gridn_width,
distance = None,
use_uncertainties = False )
testutils.confirm_equal( difflen.shape[1], gridn_width,
msg = "Expected number of columns" )
testutils.confirm_equal( difflen.shape[0], int(round( model_splined.imagersize()[1] / model_splined.imagersize()[0] * gridn_width)),
msg = "Expected number of rows" )
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 cleanup():
global workdir
try:
shutil.rmtree(workdir)
workdir = None
except:
pass
atexit.register(cleanup)
filename_splined = f"{testdir}/data/cam0.splined.cameramodel"
with open(filename_splined, "r") as f:
text_splined = f.read()
model_splined = mrcal.cameramodel(filename_splined)
# These settings are semi-arbitrary. I could test that higher radii fit more
# stuff until we go too high, and it doesn't fit at all anymore. Need --sampled
# because my models don't have optimization_inputs. For a basic test this is
# fine
text_out_cahvor = \
subprocess.check_output( (f"{testdir}/../mrcal-convert-lensmodel",
"--radius", "800",
"--intrinsics-only",
"--sampled",
"--distance", "3",
"LENSMODEL_CAHVOR",
"-",),
encoding = 'ascii',
input = text_splined,
rt_0r = np.array((1., 2., 3., 4., 5., 6.))
rt_1r = np.array((0.8, -0.01, -0.2, 2., -4., 2.))
imagersize = np.array((2000, 1000), dtype=int)
cxy_center = (imagersize-1.)/2.
pitch_y = 100.
cxy_pitched = cxy_center + np.array((0., pitch_y))
# very long lenses. I want rotation to look very much like panning
fxycxy0 = nps.glue( np.array(( 50000., 50000.)), cxy_pitched, axis=-1)
fxycxy1 = nps.glue( np.array(( 50000., 50000.)), cxy_center, axis=-1)
model0 = mrcal.cameramodel( intrinsics = ('LENSMODEL_PINHOLE', fxycxy0),
imagersize = imagersize,
extrinsics_rt_fromref = rt_0r )
model1 = mrcal.cameramodel( intrinsics = ('LENSMODEL_PINHOLE', fxycxy1),
imagersize = imagersize,
extrinsics_rt_fromref = rt_1r )
filename0 = f"{workdir}/model0.cameramodel"
filename1 = f"{workdir}/model1.cameramodel"
model0.write(filename0)
model1.write(filename1)
# Basic test. Combine intrinsics and extrinsics without fitting any extra
# transform
out = subprocess.check_output( (f"{testdir}/../mrcal-graft-models",
testutils.confirm_equal(
q0_true[distance][icam],
q0_baseline,
eps=0.1,
worstcase=True,
msg=
f"Regularization bias small-enough for camera {icam} at distance={'infinity' if distance is None else distance}"
)
for icam in (0, 3):
# I move the extrinsics of a model, write it to disk, and make sure the same
# uncertainties come back
if icam >= args.Ncameras: break
model_moved = mrcal.cameramodel(models_baseline[icam])
model_moved.extrinsics_rt_fromref([1., 2., 3., 4., 5., 6.])
model_moved.write(f'{workdir}/out.cameramodel')
model_read = mrcal.cameramodel(f'{workdir}/out.cameramodel')
icam_intrinsics_read = model_read.icam_intrinsics()
icam_extrinsics_read = mrcal.corresponding_icam_extrinsics(
icam_intrinsics_read, **model_read.optimization_inputs())
testutils.confirm_equal(
icam if fixedframes else icam - 1,
icam_extrinsics_read,
msg=
f"corresponding icam_extrinsics reported correctly for camera {icam}")
p_cam_baseline = mrcal.unproject(q0_baseline,
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
#!/usr/bin/python3
import sys
import mrcal
import cv2
import numpy as np
# Read the model and image from the commandline arguments
try:
model = mrcal.cameramodel(sys.argv[1])
image = cv2.imread(sys.argv[2])
yaw_deg = float(sys.argv[3])
what = sys.argv[4]
except:
print(f"Usage: {sys.argv[0]} model image yaw_deg what", file=sys.stderr)
sys.exit(1)
# I want a pinhole model to cover the middle 1/3rd of my pixels
W, H = model.imagersize()
fit_points = \
np.array((( W/3., H/3.),
( W*2./3., H/3.),
( W/3., H*2./3.),
( W*2./3., H*2./3.)))
model_pinhole = \
mrcal.pinhole_model_for_reprojection(model,
fit = fit_points,
scale_image = 0.5)
# yaw transformation: pure rotation around the y axis
k = d[4:]
fxy = d[:2]
cxy = d[2:4]
x, y = v[:2] / v[2]
r2 = x * x + y * y
r4 = r2 * r2
r6 = r4 * r2
a1 = 2 * x * y
a2 = r2 + 2 * x * x
a3 = r2 + 2 * y * y
return np.array((1 + k[0] * r2 + k[1] * r4 + k[4] * r6,
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]
def _read(s, name):
r'''Reads a .cahvor file into a cameramodel
The input is the .cahvor file contents as a string'''
re_f = '[-+]?(?:\d+(?:\.\d*)?|\.\d+)(?:[eE][-+]?\d+)?'
re_u = '\d+'
re_d = '[-+]?\d+'
re_s = '.+'
# I parse all key=value lines into my dict as raw text. Further I
# post-process some of these raw lines.
x = {}
for l in s.splitlines():
if re.match('^\s*#|^\s*$', l):
continue
m = re.match('\s*(\w+)\s*=\s*(.+?)\s*\n?$',
l, flags=re.I)
if m:
key = m.group(1)
if key in x:
raise Exception("Reading '{}': key '{}' seen more than once".format(name,
m.group(1)))
value = m.group(2)
# for compatibility
if re.match('^DISTORTION', key):
key = key.replace('DISTORTION', 'LENSMODEL')
x[key] = value
# Done reading. Any values that look like numbers, I convert to numbers.
for i in x:
if re.match('{}$'.format(re_f), x[i]):
x[i] = float(x[i])
# I parse the fields I know I care about into numpy arrays
for i in ('Dimensions','C','A','H','V','O','R','E',
'LENSMODEL_OPENCV4',
'LENSMODEL_OPENCV5',
'LENSMODEL_OPENCV8',
'LENSMODEL_OPENCV12',
'VALID_INTRINSICS_REGION'):
if i in x:
# Any data that's composed only of digits and whitespaces (no "."),
# use integers
if re.match('[0-9\s]+$', x[i]): totype = int
else: totype = float
x[i] = np.array( [ totype(v) for v in re.split('\s+', x[i])], dtype=totype)
# Now I sanity-check the results and call it done
for k in ('Dimensions','C','A','H','V'):
if not k in x:
raise Exception("Cahvor file '{}' incomplete. Missing values for: {}".
format(name, k))
is_cahvor_or_cahvore = False
if 'LENSMODEL_OPENCV12' in x:
distortions = x["LENSMODEL_OPENCV12"]
lensmodel = 'LENSMODEL_OPENCV12'
elif 'LENSMODEL_OPENCV8' in x:
distortions = x["LENSMODEL_OPENCV8"]
lensmodel = 'LENSMODEL_OPENCV8'
elif 'LENSMODEL_OPENCV5' in x:
distortions = x["LENSMODEL_OPENCV5"]
lensmodel = 'LENSMODEL_OPENCV5'
elif 'LENSMODEL_OPENCV4' in x:
distortions = x["LENSMODEL_OPENCV4"]
lensmodel = 'LENSMODEL_OPENCV4'
elif 'R' not in x:
distortions = np.array(())
lensmodel = 'LENSMODEL_PINHOLE'
else:
is_cahvor_or_cahvore = True
if 'VALID_INTRINSICS_REGION' in x:
x['VALID_INTRINSICS_REGION'] = \
x['VALID_INTRINSICS_REGION'].reshape( len(x['VALID_INTRINSICS_REGION'])//2, 2)
# get extrinsics from cahvor
if 'Model' not in x:
x['Model'] = ''
m = re.match('CAHVORE3,([0-9\.e-]+)\s*=\s*general',x['Model'])
if m:
is_cahvore = True
cahvore_linearity = float(m.group(1))
else:
is_cahvore = False
Hp,Vp = _HVs_HVc_HVp(x)[-2:]
R_toref = nps.transpose( nps.cat( Hp,
Vp,
x['A'] ))
t_toref = x['C']
if is_cahvor_or_cahvore:
if 'O' not in x:
alpha = 0
beta = 0
else:
o = nps.matmult( x['O'], R_toref )
alpha = np.arctan2(o[0], o[2])
beta = np.arcsin( o[1] )
if is_cahvore:
# CAHVORE
if 'E' not in x:
raise Exception('Cahvor file {} LOOKS like a cahvore, but lacks the E'.format(name))
R0,R1,R2 = x['R'].ravel()
E0,E1,E2 = x['E'].ravel()
distortions = np.array((alpha,beta,R0,R1,R2,E0,E1,E2), dtype=float)
lensmodel = f'LENSMODEL_CAHVORE_linearity={cahvore_linearity}'
else:
# CAHVOR
if 'E' in x:
raise Exception('Cahvor file {} LOOKS like a cahvor, but has an E'.format(name))
if abs(beta) < 1e-8 and \
( 'R' not in x or np.linalg.norm(x['R']) < 1e-8):
# pinhole
alpha = 0
beta = 0
else:
R0,R1,R2 = x['R'].ravel()
if alpha == 0 and beta == 0:
distortions = np.array(())
lensmodel = 'LENSMODEL_PINHOLE'
else:
distortions = np.array((alpha,beta,R0,R1,R2), dtype=float)
lensmodel = 'LENSMODEL_CAHVOR'
m = mrcal.cameramodel(imagersize = x['Dimensions'].astype(np.int32),
intrinsics = (lensmodel, nps.glue( np.array(_fxy_cxy(x), dtype=float),
distortions,
axis = -1)),
valid_intrinsics_region = x.get('VALID_INTRINSICS_REGION'),
extrinsics_Rt_toref = np.ascontiguousarray(nps.glue(R_toref,t_toref, axis=-2)))
return m
testdir = os.path.dirname(os.path.realpath(__file__))
# I import the LOCAL mrcal since that's what I'm testing
sys.path[:0] = f"{testdir}/..",
import mrcal
import copy
import testutils
from test_calibration_helpers import sample_dqref
# I want the RNG to be deterministic
np.random.seed(0)
############# Set up my world, and compute all the perfect positions, pixel
############# observations of everything
models_ref = (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"))
imagersizes = nps.cat(*[m.imagersize() for m in models_ref])
lensmodel = models_ref[0].intrinsics()[0]
# I have opencv8 models_ref, but let me truncate to opencv4 models_ref to keep this
# simple and fast
lensmodel = 'LENSMODEL_OPENCV4'
for m in models_ref:
m.intrinsics(intrinsics=(lensmodel, m.intrinsics()[1][:8]))
Nintrinsics = mrcal.lensmodel_num_params(lensmodel)
Ncameras = len(models_ref)
Ncameras_extrinsics = Ncameras - 1
if terminal['png'] is None:
terminal[
'png'] = 'pngcairo size 1024,768 transparent noenhanced crop font ",12"'
extraset = dict()
for k in pointscale.keys():
extraset[k] = f'pointsize {pointscale[k]}'
# I want the RNG to be deterministic
np.random.seed(0)
############# Set up my world, and compute all the perfect positions, pixel
############# observations of everything
if re.match('opencv', args.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 args.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 args.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"))
import sys
import numpy as np
import numpysane as nps
import os
testdir = os.path.dirname(os.path.realpath(__file__))
# I import the LOCAL mrcal since that's what I'm testing
sys.path[:0] = f"{testdir}/..",
import mrcal
import testutils
############# Set up my world, and compute all the perfect positions, pixel
############# observations of everything
m = mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel")
imagersize = m.imagersize()
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)))
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
