def CooksD_test(J, x, f, CooksD, k_dima, k_cook, i=0):
r'''Test the computation of Cook's D
I have an analytical expression for this computed in
compute_outliernesses(). This explicitly computes the quantity represented
by compute_outliernesses() to make sure that that analytical expression is
correct
'''
# I reoptimize without measurement i
Nmeasurements,Nstate = J.shape
J1 = nps.glue(J[:i,:], J[(i+1):,:], axis=-2)
f1 = nps.glue(f[:i ], f[(i+1): ], axis=-1)
p1 = nps.matmult( f1, nps.transpose(np.linalg.pinv(J1)))
x1 = nps.matmult(p1, nps.transpose(J)) - f
dx = x1-x
report_mismatch_relerr( nps.inner(dx,dx) * k_cook,
CooksD['self_others'][i],
"self_others CooksD computed analytically, explicitly")
report_mismatch_relerr( (nps.inner(dx,dx) - dx[i]*dx[i]) * k_cook,
CooksD['others'][i],
"others CooksD computed analytically, explicitly")
def CooksD_query_test(J,p,x, f, query,fquery_ref, CooksD_nox, k_dima, k_cook, i=0):
r'''Test the concept of CooksD for querying hypothetical data
fquery_test = f(q) isn't true here. If it WERE true, the x of the query
point would be 0 (we fit the model exactly), so the outlierness factor would
be 0 also
'''
# current solve
Nmeasurements,Nstate = J.shape
query = query [i]
fquery_ref = fquery_ref[i]
# I add a new point, and reoptimize
fquery = func_hypothesis(query,p)
xquery = fquery - fquery_ref
jquery = model_matrix(query, len(p))
J1 = nps.glue(J, jquery, axis=-2)
f1 = nps.glue(f, fquery_ref, axis=-1)
p1 = nps.matmult( f1, nps.transpose(np.linalg.pinv(J1)))
x1 = nps.matmult(p1, nps.transpose(J1)) - f1
dx = x1[:-1] - x
dx_both = x1 - nps.glue(x,xquery, axis=-1)
report_mismatch_relerr( nps.inner(dx_both,dx_both)*k_cook,
CooksD_nox['self_others'][i]*xquery*xquery,
"self_others query-CooksD computed analytically, explicitly")
report_mismatch_relerr( nps.inner(dx,dx)*k_cook,
CooksD_nox['others'][i]*xquery*xquery,
"others query-CooksD computed analytically, explicitly")
def outlierness_test(J, x, f, outlierness, k_dima, k_cook, i=0):
r'''Test the computation of outlierness
I have an analytical expression for this computed in
compute_outliernesses(). This explicitly computes the quantity represented
by compute_outliernesses() to make sure that that analytical expression is
correct
'''
# I reoptimize without measurement i
E0 = nps.inner(x,x)
J1 = nps.glue(J[:i,:], J[(i+1):,:], axis=-2)
f1 = nps.glue(f[:i ], f[(i+1): ], axis=-1)
p1 = nps.matmult( f1, nps.transpose(np.linalg.pinv(J1)))
x1 = nps.matmult(p1, nps.transpose(J1)) - f1
E1 = nps.inner(x1,x1)
report_mismatch_relerr( (E0-E1) * k_dima,
outlierness['self_others'][i],
"self_others outlierness computed analytically, explicitly")
report_mismatch_relerr( (E0-x[i]*x[i] - E1) * k_dima,
outlierness['others'][i],
"others outlierness computed analytically, explicitly")
def _splined_stereographic_domain(lensmodel):
r'''Return the stereographic domain for splined-stereographic lens models
SYNOPSIS
model = mrcal.cameramodel(model_filename)
lensmodel = model.intrinsics()[0]
domain_contour = mrcal._splined_stereographic_domain(lensmodel)
Splined stereographic models are defined by a splined surface. This surface is
indexed by normalized stereographic-projected points. This surface is defined in
some finite area, and this function reports a piecewise linear contour reporting
this region.
This function only makes sense for splined stereographic models.
RETURNED VALUE
An array of shape (N,2) containing a contour representing the projection domain.
'''
if not re.match('LENSMODEL_SPLINED_STEREOGRAPHIC', lensmodel):
raise Exception(f"This only makes sense with splined models. Input uses {lensmodel}")
ux,uy = mrcal.knots_for_splined_models(lensmodel)
# shape (Ny,Nx,2)
u = np.ascontiguousarray(nps.mv(nps.cat(*np.meshgrid(ux,uy)), 0, -1))
meta = mrcal.lensmodel_metadata(lensmodel)
if meta['order'] == 2:
# spline order is 3. The valid region is 1/2 segments inwards from the
# outer contour
return \
nps.glue( (u[0,1:-2] + u[1,1:-2]) / 2.,
(u[0,-2] + u[1,-2] + u[0,-1] + u[1,-1]) / 4.,
(u[1:-2, -2] + u[1:-2, -1]) / 2.,
(u[-2,-2] + u[-1,-2] + u[-2,-1] + u[-1,-1]) / 4.,
(u[-2, -2:1:-1] + u[-1, -2:1:-1]) / 2.,
(u[-2, 1] + u[-1, 1] + u[-2, 0] + u[-1, 0]) / 4.,
(u[-2:0:-1, 0] +u[-2:0:-1, 1]) / 2.,
(u[0, 0] +u[0, 1] + u[1, 0] +u[1, 1]) / 4.,
(u[0,1] + u[1,1]) / 2.,
axis = -2 )
elif meta['order'] == 3:
# spline order is 3. The valid region is the outer contour, leaving one
# knot out
return \
nps.glue( u[1,1:-2], u[1:-2, -2], u[-2, -2:1:-1], u[-2:0:-1, 1],
axis=-2 )
else:
raise Exception("I only support cubic (order==3) and quadratic (order==2) models")
def _densify_polyline(p, spacing):
r'''Returns the input polyline, but resampled more densely
The input and output polylines are a numpy array of shape (N,2). The output
is resampled such that each input point is hit, but each linear segment is
also sampled with at least the given spacing
'''
if p is None or p.size == 0:
return p
p1 = np.array(p[0, :], dtype=p.dtype)
for i in range(1, len(p)):
a = p[i - 1, :]
b = p[i, :]
d = b - a
l = nps.mag(d)
# A hacky method of rounding up
N = int(l / spacing - 1e-6 + 1.)
for j in range(N):
p1 = nps.glue(p1, float(j + 1) / N * d + a, axis=-2)
return p1
def _align_procrustes_points_Rt01(p0, p1, weights):
p0 = nps.transpose(p0)
p1 = nps.transpose(p1)
# I process Mt instead of M to not need to transpose anything later, and to
# end up with contiguous-memory results
Mt = nps.matmult((p0 - np.mean(p0, axis=-1)[..., np.newaxis]) * weights,
nps.transpose(p1 - np.mean(p1, axis=-1)[..., np.newaxis]))
V, S, Ut = np.linalg.svd(Mt)
R = nps.matmult(V, Ut)
# det(R) is now +1 or -1. If it's -1, then this contains a mirror, and thus
# is not a physical rotation. I compensate by negating the least-important
# pair of singular vectors
if np.linalg.det(R) < 0:
V[:, 2] *= -1
R = nps.matmult(V, Ut)
# Now that I have my optimal R, I compute the optimal t. From before:
#
# t = mean(a) - R mean(b)
t = np.mean(p0, axis=-1)[..., np.newaxis] - nps.matmult(
R,
np.mean(p1, axis=-1)[..., np.newaxis])
return nps.glue(R, t.ravel(), axis=-2)
def invert_Rt(Rt):
r'''Simple reference implementation
b = Ra + t -> a = R'b - R't
'''
R = Rt[:3, :]
tinv = -nps.matmult(Rt[3, :], R)
return nps.glue(nps.transpose(R), tinv.ravel(), axis=-2)
def close_contour(c):
r'''Close a polyline, if it isn't already closed
SYNOPSIS
print( a.shape )
===>
(5, 2)
print( a[0] )
===>
[844 204]
print( a[-1] )
===>
[886 198]
b = mrcal.close_contour(a)
print( b.shape )
===>
(6, 2)
print( b[0] )
===>
[844 204]
print( b[-2:] )
===>
[[886 198]
[844 204]]
This function works with polylines represented as arrays of shape (N,2). The
polygon represented by such a polyline is "closed" if its first and last points
sit at the same location. This function ingests a polyline, and returns the
corresponding, closed polygon. If the first and last points of the input match,
the input is returned. Otherwise, the first point is appended to the end, and
this extended polyline is returned.
None is accepted as an empty polygon: we return None.
ARGUMENTS
- c: an array of shape (N,2) representing the polyline to be closed. None and
arrays of shape (0,2) are accepted as special cases ("unknown" and "empty"
regions, respectively)
RETURNED VALUE
An array of shape (N,2) representing the closed polygon. The input is returned
if the input was None or has shape (0,2)
'''
if c is None or c.size == 0: return c
if np.linalg.norm(c[0, :] - c[-1, :]) < 1e-6:
return c
return nps.glue(c, c[0, :], axis=-2)
def invert_rt(rt):
r'''Simple reference implementation
b = Ra + t -> a = R'b - R't
'''
r = rt[:3]
t = rt[3:]
R = R_from_r(r)
tinv = -nps.matmult(t, R)
return nps.glue(-r, tinv.ravel(), axis=-1)
def compose_Rt(Rt0, Rt1):
r'''Simple reference implementation
b = R0 (R1 x + t1) + t0 =
= R0 R1 x + R0 t1 + t0
'''
R0 = Rt0[:3, :]
t0 = Rt0[3, :]
R1 = Rt1[:3, :]
t1 = Rt1[3, :]
R2 = nps.matmult(R0, R1)
t2 = nps.matmult(t1, nps.transpose(R0)) + t0
return nps.glue(R2, t2.ravel(), axis=-2)
def outlierness_query_test(J,p,x, f, query,fquery_ref, outlierness_nox, k_dima, k_cook, i=0):
r'''Test the concept of outlierness for querying hypothetical data
fquery_test = f(q) isn't true here. If it WERE true, the x of the query
point would be 0 (we fit the model exactly), so the outlierness factor would
be 0 also
'''
# current solve
E0 = nps.inner(x,x)
query = query [i]
fquery_ref = fquery_ref[i]
# I add a new point, and reoptimize
fquery = func_hypothesis(query,p)
xquery = fquery - fquery_ref
jquery = model_matrix(query, len(p))
J1 = nps.glue(J, jquery, axis=-2)
f1 = nps.glue(f, fquery_ref, axis=-1)
p1 = nps.matmult( f1, nps.transpose(np.linalg.pinv(J1)))
x1 = nps.matmult(p1, nps.transpose(J1)) - f1
E1 = nps.inner(x1,x1)
report_mismatch_relerr( (x1[-1]*x1[-1]) * k_dima,
outlierness_nox['self'][i]*xquery*xquery,
"self query-outlierness computed analytically, explicitly")
report_mismatch_relerr( (E1-x1[-1]*x1[-1] - E0) * k_dima,
outlierness_nox['others'][i]*xquery*xquery,
"others query-outlierness computed analytically, explicitly")
report_mismatch_relerr( (E1 - E0) * k_dima,
outlierness_nox['self_others'][i]*xquery*xquery,
"self_others query-outlierness computed analytically, explicitly")
def pq_from_Rt(Rt):
r'''Converts an Rt transformation to an pq transformation
pq is a 7-long array: a 3-long translation followed by a 4-long unit
quaternion.
Rt is a (4,3) array: a (3,3) rotation matrix with a 3-long translation in
the last row
'''
R = Rt[:3,:]
t = Rt[ 3,:]
q = mrcal.quat_from_R(R)
return nps.glue(t,q, axis=-1)
def Rt_from_pq(pq):
r'''Converts a pq transformation to an Rt transformation
pq is a 7-long array: a 3-long translation followed by a 4-long unit
quaternion.
Rt is a (4,3) array: a (3,3) rotation matrix with a 3-long translation in
the last row
Broadcasting is supported
'''
p = pq[..., :3]
q = pq[..., 3:]
R = mrcal.R_from_quat(q)
return nps.glue(R, nps.dummy(p, -2), axis=-2)
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
def Rt_from_rt(rt):
r'''Simple reference implementation'''
return nps.glue(R_from_r(rt[:3]), rt[3:], axis=-2)
H10 = np.linalg.inv(H01)
# The feature I'm going to test with. This is the corner of one of the towers
q0 = np.array((294, 159), dtype=np.float32)
# The transformed image. The matcher should end-up reversing this
# transformation, since it will be given the homography.
#
# shape (H,W,2)
image1 = \
mrcal.transform_image(
image,
mrcal.apply_homography(
H01,
nps.glue(*[ nps.dummy(arr, -1) for arr in \
np.meshgrid( np.arange(500),
np.arange(600))],
axis=-1).astype(np.float32) ))
# I have the source images and the "right" homography and the "right" matching
# pixels coords. Run the matcher, and compare
templatesize = (30, 20)
search_radius = 50
H10_shifted = H10.copy()
H10_shifted[0, 2] += 10.2
H10_shifted[1, 2] -= 20.4
q1_matched, diagnostics = \
mrcal.match_feature( image, image1,
templatesize,
def unproject(q, lensmodel, intrinsics_data,
normalize = False,
get_gradients = False,
out = None):
r'''Unprojects pixel coordinates to observation vectors
SYNOPSIS
# q is a (...,2) array of pixel observations
v = mrcal.unproject( q,
lensmodel, intrinsics_data )
### OR ###
m = mrcal.cameramodel(...)
v = mrcal.unproject( q, *m.intrinsics() )
Maps a set of 2D imager points q to a set of 3D vectors in camera coordinates
that produced these pixel observations. Each 3D vector is unique only
up-to-length, and the returned vectors aren't normalized by default. The default
length of the returned vector is arbitrary, and selected for the convenience of
the implementation. Pass normalize=True to always return unit vectors.
This is the "reverse" direction, so an iterative nonlinear optimization is
performed internally to compute this result. This is much slower than
mrcal_project. For OpenCV distortions specifically, OpenCV has
cvUndistortPoints() (and cv2.undistortPoints()), but these are inaccurate and we
do not use them: https://github.com/opencv/opencv/issues/8811
Gradients are available by passing get_gradients=True. Since unproject() is
implemented as an iterative solve around project(), the unproject() gradients
are computed by manipulating the gradients reported by project() at the
solution. The reported gradients are relative to whatever unproject() is
reporting; the unprojection is unique only up-to-length, and the magnitude isn't
fixed. So the gradients may include a component in the direction of the returned
observation vector: this follows the arbitrary scaling used by unproject(). It
is possible to pass normalize=True; we then return NORMALIZED observation
vectors and the gradients of those NORMALIZED vectors. In that case, those
gradients are guaranteed to be orthogonal to the observation vector. The vector
normalization involves a bit more computation, so it isn't the default.
NOTE: THE MAGNITUDE OF THE RETURNED VECTOR CHANGES IF get_gradients CHANGES. The
reported gradients are correct relative to the output returned with
get_gradients=True. Passing normalize=True can be used to smooth this out:
unproject(..., normalize=True)
returns the same vectors as
unproject(..., normalize=True, get_gradients=True)[0]
Broadcasting is fully supported across q and intrinsics_data.
Models that have no gradients available cannot use mrcal_unproject() in C, but
CAN still use this mrcal.unproject() Python routine: a slower routine is
employed that uses numerical differences instead of analytical gradients.
ARGUMENTS
- q: array of dims (...,2); the pixel coordinates we're unprojecting
- lensmodel: a string such as
LENSMODEL_PINHOLE
LENSMODEL_OPENCV4
LENSMODEL_CAHVOR
LENSMODEL_SPLINED_STEREOGRAPHIC_order=3_Nx=16_Ny=12_fov_x_deg=100
- intrinsics_data: array of dims (Nintrinsics):
(focal_x, focal_y, center_pixel_x, center_pixel_y, distortion0, distortion1,
...)
The focal lengths are given in pixels.
- normalize: optional boolean defaults to False. If True: normalize the output
vectors
- get_gradients: optional boolean that defaults to False. Whether we should
compute and report the gradients. This affects what we return (see below). If
not normalize, the magnitude of the reported vectors changes if get_gradients
is turned on/off (see above)
- out: optional argument specifying the destination. By default, new numpy
array(s) are created and returned. To write the results into existing arrays,
specify them with the 'out' kwarg. If not get_gradients: 'out' is the one
numpy array we will write into. Else: 'out' is a tuple of all the output numpy
arrays. If 'out' is given, we return the same arrays passed in. This is the
standard behavior provided by numpysane_pywrap.
RETURNED VALUE
if not get_gradients:
we return an (...,3) array of unprojected observation vectors. Not normalized
by default; see description above
if get_gradients: we return a tuple:
- (...,3) array of unprojected observation vectors
- (...,3,2) array of gradients of unprojected observation vectors in respect
to pixel coordinates
- (...,3,Nintrinsics) array of gradients of unprojected observation vectors in
respect to the intrinsics
'''
def apply_normalization_to_output_with_gradients(v,dv_dq,dv_di):
# vn = v/mag(v)
# dvn = dv (1/mag(v)) + v d(1/mag(v))
# = dv( 1/mag(v) - v vt / mag^3(v) )
# = dv( 1/mag(v) - vn vnt / mag(v) )
# = dv/mag(v) ( 1 - vn vnt )
# v has shape (...,3)
# dv_dq has shape (...,3,2)
# dv_di has shape (...,3,N)
# shape (...,1)
magv_recip = 1. / nps.dummy(nps.mag(v), -1)
v *= magv_recip
# shape (...,1,1)
magv_recip = nps.dummy(magv_recip,-1)
dv_dq *= magv_recip
dv_dq -= nps.xchg( nps.matmult( nps.dummy(nps.xchg(dv_dq, -1,-2), -2),
nps.dummy(nps.outer(v,v),-3) )[...,0,:],
-1, -2)
dv_di *= magv_recip
dv_di -= nps.xchg( nps.matmult( nps.dummy(nps.xchg(dv_di, -1,-2), -2),
nps.dummy(nps.outer(v,v),-3) )[...,0,:],
-1, -2)
# First, handle some trivial cases. I don't want to run the
# optimization-based unproject() if I don't have to
if lensmodel == 'LENSMODEL_PINHOLE' or \
lensmodel == 'LENSMODEL_LONLAT' or \
lensmodel == 'LENSMODEL_LATLON' or \
lensmodel == 'LENSMODEL_STEREOGRAPHIC':
if lensmodel == 'LENSMODEL_PINHOLE':
func = mrcal.unproject_pinhole
always_normalized = False
elif lensmodel == 'LENSMODEL_LONLAT':
func = mrcal.unproject_lonlat
always_normalized = True
elif lensmodel == 'LENSMODEL_LATLON':
func = mrcal.unproject_latlon
always_normalized = True
elif lensmodel == 'LENSMODEL_STEREOGRAPHIC':
func = mrcal.unproject_stereographic
always_normalized = False
if not get_gradients:
v = func(q, intrinsics_data, out = out)
if normalize and not always_normalized:
v /= nps.dummy(nps.mag(v), axis=-1)
return v
# shapes (...,2)
fxy = intrinsics_data[..., :2]
cxy = intrinsics_data[..., 2:]
# shapes (...,3) and (...,3,2)
v, dv_dq = \
func(q, intrinsics_data,
get_gradients = True,
out = None if out is None else (out[0],out[1]))
# q = f l(v) + c
# l(v) = (q-c)/f
#
# dl/dv dv/df = (c-q) / f^2
# dl/dv dv/dq = 1/f
# -> dl/dv = 1 / ( f dv/dq )
# -> dv/df = (c-q) / (f^2 dl/dv) = (c-q) dv/dq / f
#
# dl/dv dv/dc = -1/f
# -> dv/dc = -1 / (f dl/dv) = -1 / (f /( f dv/dq )) = -dv/dq
dv_di_shape = dv_dq.shape[:-1] + (4,)
if out is None:
dv_di = np.zeros( dv_di_shape, dtype=float)
else:
if not (out[2].shape[-len(dv_di_shape):] == dv_di_shape and \
not any(np.array(out[2].shape[:-len(dv_di_shape)]) - 1)):
raise Exception(f"Shape of out[2] doesn't match broadcasted shape for dv_di. Wanted {dv_di_shape}, but got {out[2].shape}")
dv_di = out[2]
dv_di *= 0
# dv/df
dv_di[..., :2] += nps.dummy((cxy - q)/fxy, -2) * dv_dq
# dv/dc
dv_di[..., 2:] -= dv_dq
if normalize and not always_normalized:
apply_normalization_to_output_with_gradients(v,dv_dq,dv_di)
return v,dv_dq,dv_di
try:
meta = mrcal.lensmodel_metadata_and_config(lensmodel)
except:
raise Exception(f"Invalid lens model '{lensmodel}': couldn't get the metadata")
if meta['has_gradients']:
# Main path. We have gradients.
#
# Internal function must have a different argument order so
# that all the broadcasting stuff is in the leading arguments
if not get_gradients:
v = mrcal._mrcal_npsp._unproject(q, intrinsics_data, lensmodel=lensmodel,
out=out)
if normalize:
# Explicitly handle nan and inf to set their normalized values
# to 0. Otherwise I get a scary-looking warning from numpy
i_vgood = \
np.isfinite(v[...,0]) * \
np.isfinite(v[...,1]) * \
np.isfinite(v[...,2])
v[~i_vgood] = np.array((0.,0.,1.))
v /= nps.dummy(nps.mag(v), -1)
v[~i_vgood] = np.array((0.,0.,0.))
return v
# We need to report gradients
vs = mrcal._mrcal_npsp._unproject(q, intrinsics_data, lensmodel=lensmodel)
# I have no gradients available for unproject(), and I need to invert a
# non-square matrix to use the gradients from project(). I deal with this
# with a stereographic mapping
#
# With a simple unprojection I have q -> v
# Instead I now do q -> vs -> u -> v
# I reproject vs, to produce a scaled v = k*vs. I'm assuming all
# projections are central, so vs represents q just as well as v does. u
# is a 2-vector, so dq_du is (2x2), and I can invert it
u = mrcal.project_stereographic(vs)
dv_du = np.zeros( vs.shape + (2,), dtype=float)
v, dv_du = \
mrcal.unproject_stereographic(u,
get_gradients = True,
out = (vs if out is None else out[0],
dv_du))
_,dq_dv,dq_di = mrcal.project(v,
lensmodel, intrinsics_data,
get_gradients = True)
# shape (..., 2,2). Square. Invertible!
dq_du = nps.matmult( dq_dv, dv_du )
# dv/dq = dv/du du/dq =
# = dv/du inv(dq/du)
# = transpose(inv(transpose(dq/du)) transpose(dv/du))
dv_dq = nps.transpose(np.linalg.solve( nps.transpose(dq_du),
nps.transpose(dv_du) ))
if out is not None:
out[1] *= 0.
out[1] += dv_dq
dv_dq = out[1]
# dv/di is a bit different. I have (q,i) -> v. I want to find out
# how moving i affects v while keeping q constant. Taylor expansion
# of projection: q = q0 + dq/dv dv + dq/di di. q is constant so
# dq/dv dv + dq/di di = 0 -> dv/di = - dv/dq dq/di
dv_di = nps.matmult(dv_dq, dq_di,
out = None if out is None else out[2])
dv_di *= -1.
if normalize:
apply_normalization_to_output_with_gradients(v,dv_dq,dv_di)
return v, dv_dq, dv_di
# No projection gradients implemented in C. We should get here approximately
# never. At this time, the only projection function that has no gradients
# implemented is LENSMODEL_CAHVORE, which nobody is really expected to be
# using. If these see use, real gradients should be implemented
#
# We compute the gradients numerically. This is a reimplementation of the C
# code. It's barely maintained, and here for legacy compatibility only
if get_gradients:
raise Exception(f"unproject(..., get_gradients=True) is unsupported for models with no gradients, such as '{lensmodel}'")
if q is None: return q
if q.size == 0:
s = q.shape
return np.zeros(s[:-1] + (3,))
if out is not None:
raise Exception(f"unproject(..., out) is unsupported if out is not None and we're using a model with no gradients, such as '{lensmodel}'")
fxy = intrinsics_data[..., :2]
cxy = intrinsics_data[..., 2:4]
# undistort the q, by running an optimizer
import scipy.optimize
# I optimize each point separately because the internal optimization
# algorithm doesn't know that each point is independent, so if I optimized
# it all together, it would solve a dense linear system whose size is linear
# in Npoints. The computation time thus would be much slower than
# linear(Npoints)
@nps.broadcast_define( ((2,),), )
def undistort_this(q0):
def cost_no_gradients(vxy, *args, **kwargs):
'''Optimization functions'''
return \
mrcal.project(np.array((vxy[0],vxy[1],1.)), lensmodel, intrinsics_data) - \
q0
# seed assuming distortions aren't there
vxy_seed = (q0 - cxy) / fxy
# no gradients available
result = scipy.optimize.least_squares(cost_no_gradients, vxy_seed,
'3-point')
vxy = result.x
# This needs to be precise; if it isn't, I barf. Shouldn't happen
# very often
if np.sqrt(result.cost/2.) > 1e-3:
if not unproject.__dict__.get('already_complained'):
sys.stderr.write("WARNING: unproject() wasn't able to precisely compute some points. Returning nan for those. Will complain just once\n")
unproject.already_complained = True
return np.array((np.nan,np.nan))
return vxy
vxy = undistort_this(q)
# I append a 1. shape = (..., 3)
v = nps.glue(vxy, np.ones( vxy.shape[:-1] + (1,) ), axis=-1)
if normalize:
v /= nps.dummy(nps.mag(v), -1)
return v
def grad_broadcasted(q_ref, i_ref):
return grad(lambda qi: \
mrcal.unproject_stereographic( \
mrcal.project_stereographic(
mrcal.unproject(qi[:2], intrinsics[0], qi[2:]))),
nps.glue(q_ref,i_ref, axis=-1))
def grad_broadcasted(q_ref, i_ref):
return grad(lambda qi: mrcal.unproject(qi[:2], intrinsics[0], qi[2:]),
nps.glue(q_ref,i_ref, axis=-1))
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_point_camintrinsics_camextrinsics = \
np.array(((0,1,-1),
(1,0,-1),
(1,1, 0),
(2,0,-1),
(2,1, 0)),
dtype = np.int32)
points = 10. + 2. * linspace_shaped(3, 3)
observations_point_xy = 1000. + 500. * linspace_shaped(5, 2)
observations_point_weights = np.array((0.9, 0.8, 0.9, 1.3, 1.8))
observations_point = \
nps.glue(observations_point_xy,
nps.transpose(observations_point_weights),
axis = -1)
all_test_kwargs = (dict(do_optimize_intrinsics_core=False,
do_optimize_intrinsics_distortions=True,
do_optimize_extrinsics=False,
do_optimize_frames=False,
do_optimize_calobject_warp=False,
do_apply_regularization=True),
dict(do_optimize_intrinsics_core=True,
do_optimize_intrinsics_distortions=False,
do_optimize_extrinsics=False,
do_optimize_frames=False,
do_optimize_calobject_warp=False,
do_apply_regularization=True),
dict(do_optimize_intrinsics_core=False,
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
base[1, 1, 3:6, 1] = np.array((3., 5., -2.4)) t0_ref = base[1, 1, 3:6, 1] base[1, 1, 6:9, 1] = np.array((-.3, -.2, 1.1)) r1_ref = base[1, 1, 6:9, 1] base[1, 1, 9:12, 1] = np.array((-8., .5, -.4)) t1_ref = base[1, 1, 9:12, 1] base[1, 2, 0:3, 1] = np.array((-10., -108., 3.)) x = base[1, 2, 0:3, 1] base[1, :3, :3, 2] = R_from_r(r0_ref) R0_ref = base[1, :3, :3, 2] base[1, 3:7, :3, 2] = nps.glue(R0_ref, t0_ref, axis=-2) Rt0_ref = base[1, 3:7, :3, 2] base[1, 2, 3:9, 1] = nps.glue(r0_ref, t0_ref, axis=-1) rt0_ref = base[1, 2, 3:9, 1] base[1, 7:10, :3, 2] = R_from_r(r1_ref) R1_ref = base[1, 7:10, :3, 2] base[2, :4, :3, 2] = nps.glue(R1_ref, t1_ref, axis=-2) Rt1_ref = base[2, :4, :3, 2] base[1, 3, :6, 1] = nps.glue(r1_ref, t1_ref, axis=-1) rt1_ref = base[1, 3, :6, 1] # the implementation has a separate path for tiny R, so I test it separately
nps.transpose(p_triangulated0[ipt]))[0] / nps.norm2(p_triangulated0[ipt])
diff = p_triangulated0[1] - p_triangulated0[0]
distance = nps.mag(diff)
distance_true = nps.mag(p_triangulated_true0[:, 0] -
p_triangulated_true0[:, 1])
distance_sampled = nps.mag(p_triangulated_sampled0[:, 1, :] -
p_triangulated_sampled0[:, 0, :])
mean_distance_sampled = distance_sampled.mean()
Var_distance_sampled = distance_sampled.var()
# diff = p1-p0
# dist = np.mag(diff)
# ddist_dp01 = [-diff diff] / dist
# Var(dist) = ddist_dp01 var(p01) ddist_dp01T
# = [-diff diff] var(p01) [-diff diff]T / norm2(diff)
Var_distance = nps.matmult(nps.glue(-diff, diff, axis=-1),
Var_p_joint.reshape(Npoints * 3, Npoints * 3),
nps.transpose(nps.glue(
-diff, diff, axis=-1), ))[0] / nps.norm2(diff)
# I have the observed and predicted distributions, so I make sure things match.
# For some not-yet-understood reason, the distance distribution isn't
# normally-distributed: there's a noticeable fat tail. Thus I'm not comparing
# those two distributions (Var_distance,Var_distance_sampled) in this test.
p_sampled = nps.clump(p_triangulated_sampled0, n=-2)
mean_p_sampled = np.mean(p_sampled, axis=-2)
Var_p_sampled = nps.matmult(nps.transpose(p_sampled - mean_p_sampled),
p_sampled - mean_p_sampled) / args.Nsamples
testutils.confirm_equal(
mean_p_sampled,
def f5(a,b,c,d):
return nps.glue( c, d, axis=-1 )
def rt_from_Rt(Rt):
r'''Simple reference implementation'''
return nps.glue(r_from_R(Rt[:3, :]), Rt[3, :], axis=-1)
def hypothesis_board_corner_positions(icam_intrinsics=None,
idx_inliers=None,
**optimization_inputs):
r'''Reports the 3D chessboard points observed by a camera at calibration time
SYNOPSIS
model = mrcal.cameramodel("xxx.cameramodel")
optimization_inputs = model.optimization_inputs()
# shape (Nobservations, Nheight, Nwidth, 3)
pcam = mrcal.hypothesis_board_corner_positions(**optimization_inputs)[0]
i_intrinsics = \
optimization_inputs['indices_frame_camintrinsics_camextrinsics'][:,1]
# shape (Nobservations,1,1,Nintrinsics)
intrinsics = nps.mv(optimization_inputs['intrinsics'][i_intrinsics],-2,-4)
optimization_inputs['observations_board'][...,:2] = \
mrcal.project( pcam,
optimization_inputs['lensmodel'],
intrinsics )
# optimization_inputs now contains perfect, noiseless board observations
x = mrcal.optimizer_callback(**optimization_inputs)[1]
print(nps.norm2(x[:mrcal.num_measurements_boards(**optimization_inputs)]))
==>
0
The optimization routine generates hypothetical observations from a set of
parameters being evaluated, trying to match these hypothetical observations to
real observations. To facilitate analysis, this routine returns these
hypothetical coordinates of the chessboard corners being observed. This routine
reports the 3D points in the coordinate system of the observing camera.
The hypothetical points are constructed from
- The calibration object geometry
- The calibration object-reference transformation in
optimization_inputs['frames_rt_toref']
- The camera extrinsics (reference-camera transformation) in
optimization_inputs['extrinsics_rt_fromref']
- The table selecting the camera and calibration object frame for each
observation in
optimization_inputs['indices_frame_camintrinsics_camextrinsics']
ARGUMENTS
- icam_intrinsics: optional integer specifying which intrinsic camera in the
optimization_inputs we're looking at. If omitted (or None), we report
camera-coordinate points for all the cameras
- idx_inliers: optional numpy array of booleans of shape
(Nobservations,object_height,object_width) to select the outliers manually. If
omitted (or None), the outliers are selected automatically: idx_inliers =
observations_board[...,2] > 0. This argument is available to pick common
inliers from two separate solves.
- **optimization_inputs: a dict() of arguments passable to mrcal.optimize() and
mrcal.optimizer_callback(). We use the geometric data. This dict is obtainable
from a cameramodel object by calling cameramodel.optimization_inputs()
RETURNED VALUE
- An array of shape (Nobservations, Nheight, Nwidth, 3) containing the
coordinates (in the coordinate system of each camera) of the chessboard
corners, for ALL the cameras. These correspond to the observations in
optimization_inputs['observations_board'], which also have shape
(Nobservations, Nheight, Nwidth, 3)
- An array of shape (Nobservations_thiscamera, Nheight, Nwidth, 3) containing
the coordinates (in the camera coordinate system) of the chessboard corners,
for the particular camera requested in icam_intrinsics. If icam_intrinsics is
None: this is the same array as the previous returned value
- an (N,3) array containing camera-frame 3D points observed at calibration time,
and accepted by the solver as inliers. This is a subset of the 2nd returned
array.
- an (N,3) array containing camera-frame 3D points observed at calibration time,
but rejected by the solver as outliers. This is a subset of the 2nd returned
array.
'''
observations_board = optimization_inputs.get('observations_board')
if observations_board is None:
return Exception("No board observations available")
indices_frame_camintrinsics_camextrinsics = \
optimization_inputs['indices_frame_camintrinsics_camextrinsics']
object_width_n = observations_board.shape[-2]
object_height_n = observations_board.shape[-3]
object_spacing = optimization_inputs['calibration_object_spacing']
calobject_warp = optimization_inputs.get('calobject_warp')
# shape (Nh,Nw,3)
full_object = mrcal.ref_calibration_object(object_width_n, object_height_n,
object_spacing, calobject_warp)
frames_Rt_toref = \
mrcal.Rt_from_rt( optimization_inputs['frames_rt_toref'] )\
[ indices_frame_camintrinsics_camextrinsics[:,0] ]
extrinsics_Rt_fromref = \
nps.glue( mrcal.identity_Rt(),
mrcal.Rt_from_rt(optimization_inputs['extrinsics_rt_fromref']),
axis = -3 ) \
[ indices_frame_camintrinsics_camextrinsics[:,2]+1 ]
Rt_cam_frame = mrcal.compose_Rt(extrinsics_Rt_fromref, frames_Rt_toref)
p_cam_calobjects = \
mrcal.transform_point_Rt(nps.mv(Rt_cam_frame,-3,-5), full_object)
# shape (Nobservations,Nheight,Nwidth)
if idx_inliers is None:
idx_inliers = observations_board[..., 2] > 0
idx_outliers = ~idx_inliers
if icam_intrinsics is None:
return \
p_cam_calobjects, \
p_cam_calobjects, \
p_cam_calobjects[idx_inliers, ...], \
p_cam_calobjects[idx_outliers, ...]
# The user asked for a specific camera. Separate out its data
# shape (Nobservations,)
idx_observations = indices_frame_camintrinsics_camextrinsics[:,
1] == icam_intrinsics
idx_inliers[~idx_observations] = False
idx_outliers[~idx_observations] = False
return \
p_cam_calobjects, \
p_cam_calobjects[idx_observations, ...], \
p_cam_calobjects[idx_inliers, ...], \
p_cam_calobjects[idx_outliers, ...]
def grad_normalized_broadcasted(q_ref, i_ref):
return grad(lambda qi: \
mrcal.unproject(qi[:2], intrinsics[0], qi[2:], normalize=True),
nps.glue(q_ref,i_ref, axis=-1))
relative=False,
worstcase=True,
msg=f'{what} q0',
eps=25.)
testutils.confirm_equal(q1,
q1_ref,
relative=False,
worstcase=True,
msg=f'{what} q1',
eps=25.)
# square camera layout
t01 = np.array((1., 0.1, -0.2))
R01 = mrcal.R_from_r(np.array((0.001, -0.002, -0.003)))
Rt01 = nps.glue(R01, t01, axis=-2)
p = np.array((
(300., 20., 2000.), # far away AND center-ish
(-310., 18., 2000.),
(30., 290., 1500.), # far away AND center-ish
(-31., 190., 1500.),
(3000., 200., 2000.), # far away AND off to either side
(-3100., 180., 2000.),
(300., 2900., 1500.), # far away AND off up/down
(-310., 1980., 1500.),
(3000., -200., 20.), # very close AND off to either side
(-3100., 180., 20.),
(300., 2900., 15.), # very close AND off up/down
(-310., 1980., 15.)))
test_geometry(Rt01, p, "square-camera-geometry", check_gradients=True)
def f5(a, b, c, d):
return nps.glue(c, d, axis=-1)
def grad_broadcasted(p_ref, i_ref):
return grad(lambda pi: mrcal.project(pi[:3], intrinsics[0], pi[3:]),
nps.glue(p_ref,i_ref, axis=-1))
