def dis2hom(xd,
yd,
distCoeffs,
model,
Cd=False,
Ck=False,
Cfk=False,
Jd_f=False):
'''
takes ccd cordinates and projects to homogenpus coords and undistorts
'''
xd, yd, distCoeffs = [f64(xd), f64(yd), f64(distCoeffs)]
# Hay notacion confusa a veces a xd se la refiere como d o pp
# a xh se la refiere como p; a ccd matrix como k y a distors como f o k
Cdbool = anny(Cd)
Ckbool = anny(Ck)
if Cdbool or Ckbool: # no hay incertezas Ck ni Cd
q, _, Jh_d, Jh_k = dis2hom_ratioJacobians(xd, yd, distCoeffs, model)
xh = xd / q # undistort in homogenous coords
yh = yd / q
Ch = zeros((len(xh), 2, 2))
if Cdbool: # incerteza Cd
Jh_dResh = Jh_d.reshape((-1, 2, 1, 2, 1))
Ch += (Jh_dResh * Cd.reshape(
(-1, 1, 2, 2, 1)) * Jh_dResh.transpose((0, 4, 3, 2, 1))).sum(
(2, 3))
if Ckbool: # incerteza Ck
nk = Jh_k.shape[-1] # nro de param distorsion
Jp_fResh = Jh_k.reshape((-1, 2, 1, nk, 1))
Ch += (Jp_fResh * Ck.reshape(
(1, 1, nk, nk, 1)) * Jp_fResh.transpose((0, 4, 3, 2, 1))).sum(
(2, 3))
if anny(Cfk) and anny(Jd_f): # si hay covarianza cruzada
# jacobiano de xh respecto a ccd matrix
Jh_f = (Jd_f.reshape((-1, 1, 2, 4)) * Jh_d.reshape(
(-1, 2, 2, 1))).sum(2)
# ahora agrego termino cruzado
Jh_fResh = Jh_f.reshape((-1, 2, 4, 1, 1))
Jh_kResh = transpose(Jh_k.reshape((-1, 1, 1, 2, nk)),
(0, 1, 2, 4, 3))
Ch += 2 * (Jh_fResh * Cfk.reshape(
(1, 1, nk, 4, 1)) * Jh_kResh).sum((2, 3))
else:
# calculate ratio of distortion
rd = norm([xd, yd], axis=0)
q, _ = undistort[model](rd, distCoeffs, quot=True, der=False)
xh = xd / q # undistort in homogenous coords
yh = yd / q
Ch = False
return xh, yh, Ch
def ccd2hom(imagePoints, cameraMatrix, Cccd=False, Cf=False):
'''
must provide covariances for every point if cov is not None
Cf is the covariance matrix of intrinsic linear parameters fx, fy, u, v
(in that order).
'''
# undo CCD projection, asume diagonal ccd rescale
xpp = (imagePoints[:, 0] - cameraMatrix[0, 2]) / cameraMatrix[0, 0]
ypp = (imagePoints[:, 1] - cameraMatrix[1, 2]) / cameraMatrix[1, 1]
Cccdbool = anny(Cccd)
Cfbool = anny(Cf)
if Cccdbool or Cfbool:
Cpp = zeros((xpp.shape[0], 2, 2)) # create covariance matrix
Jd_i, Jd_k = ccd2homJacobian(imagePoints, cameraMatrix)
if Cccdbool:
Jd_iResh = Jd_i.reshape((-1, 2, 2, 1, 1))
Cpp += (Jd_iResh * Cccd.reshape(
(-1, 1, 2, 2, 1)) * Jd_iResh.transpose((0, 4, 3, 2, 1))).sum(
(2, 3))
if Cfbool:
# propagate uncertainty via Jacobians
Jd_kResh = Jd_k.reshape((-1, 2, 4, 1, 1))
Cpp += (Jd_kResh * Cf.reshape(
(-1, 1, 4, 4, 1)) * Jd_kResh.transpose((0, 4, 3, 2, 1))).sum(
(2, 3))
else:
Cpp = False # return without covariance matrix
return xpp, ypp, Cpp
def dis2hom(xd, yd, distCoeffs, model, Cd=False, Ck=False, Cfk=False,
Jd_f=False):
'''
takes ccd cordinates and projects to homogenpus coords and undistorts
'''
xd, yd, distCoeffs = [f64(xd), f64(yd), f64(distCoeffs)]
# Hay notacion confusa a veces a xd se la refiere como d o pp
# a xh se la refiere como p; a ccd matrix como k y a distors como f o k
Cdbool = anny(Cd)
Ckbool = anny(Ck)
if Cdbool or Ckbool: # no hay incertezas Ck ni Cd
q, _, Jh_d, Jh_k = dis2hom_ratioJacobians(xd, yd, distCoeffs, model)
xh = xd / q # undistort in homogenous coords
yh = yd / q
Ch = zeros((len(xh), 2, 2))
if Cdbool: # incerteza Cd
Jh_dResh = Jh_d.reshape((-1, 2, 1, 2, 1))
Ch += (Jh_dResh *
Cd.reshape((-1, 1, 2, 2, 1)) *
Jh_dResh.transpose((0, 4, 3, 2, 1))
).sum((2, 3))
if Ckbool: # incerteza Ck
nk = Jh_k.shape[-1] # nro de param distorsion
Jp_fResh = Jh_k.reshape((-1, 2, 1, nk, 1))
Ch += (Jp_fResh *
Ck.reshape((1, 1, nk, nk, 1)) *
Jp_fResh.transpose((0, 4, 3, 2, 1))
).sum((2, 3))
if anny(Cfk) and anny(Jd_f): # si hay covarianza cruzada
# jacobiano de xh respecto a ccd matrix
Jh_f = (Jd_f.reshape((-1, 1, 2, 4)) *
Jh_d.reshape((-1, 2, 2, 1))
).sum(2)
# ahora agrego termino cruzado
Jh_fResh = Jh_f.reshape((-1, 2, 4, 1, 1))
Jh_kResh = transpose(Jh_k.reshape((-1, 1, 1, 2, nk)),
(0, 1, 2, 4, 3))
Ch += 2 * (Jh_fResh *
Cfk.reshape((1, 1, nk, 4, 1)) *
Jh_kResh).sum((2, 3))
else:
# calculate ratio of distortion
rd = norm([xd, yd], axis=0)
q, _ = undistort[model](rd, distCoeffs, quot=True, der=False)
xh = xd / q # undistort in homogenous coords
yh = yd / q
Ch = False
return xh, yh, Ch
def xyhToZplane(xh, yh, rV, tV, Ch=False, Crt=False):
'''
projects a point from homogenous undistorted to 3D asuming z=0
'''
xh, yh, rV, tV = [f64(xh), f64(yh), f64(rV), f64(tV)]
if prod(rV.shape) == 3:
R = Rodrigues(rV)[0]
# auxiliar calculations
a = R[0, 0] - R[2, 0] * xh
b = R[0, 1] - R[2, 1] * xh
c = tV[0] - tV[2] * xh
d = R[1, 0] - R[2, 0] * yh
e = R[1, 1] - R[2, 1] * yh
f = tV[1] - tV[2] * yh
q = a*e - d*b
xm = (f*b - c*e) / q
ym = (c*d - f*a) / q
Chbool = anny(Ch)
Crtbool = anny(Crt)
if Chbool or Crtbool: # no hay incertezas
Cm = zeros((xm.shape[0], 2, 2), dtype=float64)
# calculo jacobianos
JXm_Xp, JXm_rtV = jacobianosHom2Map(xh, yh, rV, tV)
if Crtbool: # contribucion incerteza Crt
JXm_rtVResh = JXm_rtV.reshape((2, 6, 1, 1, -1))
Cm += (JXm_rtVResh *
Crt.reshape((1, 6, 6, 1, 1)) *
JXm_rtVResh.transpose((3, 2, 1, 0, 4))
).sum((1, 2)).transpose((2, 0, 1))
if Chbool: # incerteza Ch
JXm_XpResh = JXm_Xp.reshape((2, 2, 1, 1, -1))
Cm += (JXm_XpResh *
Ch.T.reshape((1, 2, 2, 1, -1)) *
JXm_XpResh.transpose((3, 2, 1, 0, 4))
).sum((1, 2)).transpose((2, 0, 1))
else:
Cm = False # return None covariance
return xm, ym, Cm
def xyhToZplane(xh, yh, rV, tV, Ch=False, Crt=False):
'''
projects a point from homogenous undistorted to 3D asuming z=0
'''
xh, yh, rV, tV = [f64(xh), f64(yh), f64(rV), f64(tV)]
if prod(rV.shape) == 3:
R = Rodrigues(rV)[0]
# auxiliar calculations
a = R[0, 0] - R[2, 0] * xh
b = R[0, 1] - R[2, 1] * xh
c = tV[0] - tV[2] * xh
d = R[1, 0] - R[2, 0] * yh
e = R[1, 1] - R[2, 1] * yh
f = tV[1] - tV[2] * yh
q = a * e - d * b
xm = (f * b - c * e) / q
ym = (c * d - f * a) / q
Chbool = anny(Ch)
Crtbool = anny(Crt)
if Chbool or Crtbool: # no hay incertezas
Cm = zeros((xm.shape[0], 2, 2), dtype=float64)
# calculo jacobianos
JXm_Xp, JXm_rtV = jacobianosHom2Map(xh, yh, rV, tV)
if Crtbool: # contribucion incerteza Crt
JXm_rtVResh = JXm_rtV.reshape((2, 6, 1, 1, -1))
Cm += (JXm_rtVResh * Crt.reshape(
(1, 6, 6, 1, 1)) * JXm_rtVResh.transpose((3, 2, 1, 0, 4))).sum(
(1, 2)).transpose((2, 0, 1))
if Chbool: # incerteza Ch
JXm_XpResh = JXm_Xp.reshape((2, 2, 1, 1, -1))
Cm += (JXm_XpResh * Ch.T.reshape(
(1, 2, 2, 1, -1)) * JXm_XpResh.transpose((3, 2, 1, 0, 4))).sum(
(1, 2)).transpose((2, 0, 1))
else:
Cm = False # return None covariance
return xm, ym, Cm
def xypToZplane(xp, yp, rV, tV, Cp=False, Crt=False):
'''
projects a point from homogenous undistorted to 3D asuming z=0
'''
if prod(rV.shape) == 3:
R = Rodrigues(rV)[0]
# auxiliar calculations
a = R[0, 0] - R[2, 0] * xp
b = R[0, 1] - R[2, 1] * xp
c = tV[0] - tV[2] * xp
d = R[1, 0] - R[2, 0] * yp
e = R[1, 1] - R[2, 1] * yp
f = tV[1] - tV[2] * yp
q = a * e - d * b
xm = (f * b - c * e) / q
ym = (c * d - f * a) / q
Cpbool = anny(Cp)
Crtbool = anny(Crt)
if Cpbool or Crtbool: # no hay incertezas
Cm = zeros((xm.shape[0], 2, 2))
# calculo jacobianos
JXm_Xp, JXm_rtV = jacobianosHom2Map(xp, yp, rV, tV)
if Crtbool: # contribucion incerteza Crt
JXm_rtVResh = JXm_rtV.reshape((2, 6, 1, 1, -1))
Cm += (JXm_rtVResh * Crt.reshape(
(1, 6, 6, 1, 1)) * JXm_rtVResh.transpose((3, 2, 1, 0, 4))).sum(
(1, 2)).transpose((2, 0, 1))
if Cpbool: # incerteza Cp
Cp = Cp.transpose((1, 2, 0))
JXm_XpResh = JXm_Xp.reshape((2, 2, 1, 1, -1))
Cm += (JXm_XpResh * Cp.reshape(
(1, 2, 2, 1, -1)) * JXm_XpResh.transpose((3, 2, 1, 0, 4))).sum(
(1, 2)).transpose((2, 0, 1))
else:
Cm = False # return None covariance
return xm, ym, Cm
def ccd2dis(xi, yi, cameraMatrix, Ci=False, Cf=False):
'''
maps from CCd, image to homogenous distorted coordiantes.
must provide covariances for every point if cov is not None
Cf: is the covariance matrix of intrinsic linear parameters fx, fy, u, v
(in that order).
'''
xi, yi, cameraMatrix = [f64(xi), f64(yi), f64(cameraMatrix)]
# undo CCD projection, asume diagonal ccd rescale
xd = (xi - cameraMatrix[0, 2]) / cameraMatrix[0, 0]
yd = (yi - cameraMatrix[1, 2]) / cameraMatrix[1, 1]
Cibool = anny(Ci)
Cfbool = anny(Cf)
if Cibool or Cfbool:
Cd = zeros((xd.shape[0], 2, 2), dtype=float64) # create cov matrix
Jd_i, Jd_f = ccd2disJacobian(xi, yi, cameraMatrix)
if Cibool:
Jd_iResh = Jd_i.reshape((-1, 2, 2, 1, 1))
Cd += (Jd_iResh *
Ci.reshape((-1, 1, 2, 2, 1)) *
Jd_iResh.transpose((0, 4, 3, 2, 1))
).sum((2, 3))
if Cfbool:
# propagate uncertainty via Jacobians
Jd_fResh = Jd_f.reshape((-1, 2, 4, 1, 1))
Cd += (Jd_fResh *
Cf.reshape((-1, 1, 4, 4, 1)) *
Jd_fResh.transpose((0, 4, 3, 2, 1))
).sum((2, 3))
else:
Cd = False # return without covariance matrix
Jd_f = False
return xd, yd, Cd, Jd_f
def ccd2dis(xi, yi, cameraMatrix, Ci=False, Cf=False):
'''
maps from CCd, image to homogenous distorted coordiantes.
must provide covariances for every point if cov is not None
Cf: is the covariance matrix of intrinsic linear parameters fx, fy, u, v
(in that order).
'''
xi, yi, cameraMatrix = [f64(xi), f64(yi), f64(cameraMatrix)]
# undo CCD projection, asume diagonal ccd rescale
xd = (xi - cameraMatrix[0, 2]) / cameraMatrix[0, 0]
yd = (yi - cameraMatrix[1, 2]) / cameraMatrix[1, 1]
Cibool = anny(Ci)
Cfbool = anny(Cf)
if Cibool or Cfbool:
Cd = zeros((xd.shape[0], 2, 2), dtype=float64) # create cov matrix
Jd_i, Jd_f = ccd2disJacobian(xi, yi, cameraMatrix)
if Cibool:
Jd_iResh = Jd_i.reshape((-1, 2, 2, 1, 1))
Cd += (Jd_iResh * Ci.reshape(
(-1, 1, 2, 2, 1)) * Jd_iResh.transpose((0, 4, 3, 2, 1))).sum(
(2, 3))
if Cfbool:
# propagate uncertainty via Jacobians
Jd_fResh = Jd_f.reshape((-1, 2, 4, 1, 1))
Cd += (Jd_fResh * Cf.reshape(
(-1, 1, 4, 4, 1)) * Jd_fResh.transpose((0, 4, 3, 2, 1))).sum(
(2, 3))
else:
Cd = False # return without covariance matrix
Jd_f = False
return xd, yd, Cd, Jd_f
def errorCuadraticoImagen(imagePoints,
chessboardModel,
rvec,
tvec,
cameraMatrix,
distCoeffs,
model,
Ci=False,
Cf=False,
Ck=False,
Crt=False,
Cfk=False,
mahDist=False):
'''
el error asociado a una sola imagen
if mahDist=True it returns the squared mahalanobis distance for the
proyection points
the result Er is such that the probability P:
Er = - log(P)
where n is the number of points, d is the number of dimensions
the line where Er += log(det(Cm)) + pi2factor adds part of the
normalising constant
'''
# hago la proyeccion
xi, yi = imagePoints.T
xm, ym, Cm = inverse(xi, yi, rvec, tvec, cameraMatrix, distCoeffs, model,
Ci, Cf, Ck, Crt, Cfk)
# error
er = ([xm, ym] - chessboardModel[:, :2].T).T
Cmbool = anny(Cm)
if Cmbool:
# devuelvo error cuadratico pesado por las covarianzas
S = inv(Cm) # inverse of covariance matrix
# distancia mahalanobis
Er = sum(er.reshape((-1, 2, 1)) * S * er.reshape((-1, 1, 2)),
axis=(1, 2))
if not mahDist:
# add covariance normalisation error
Er += log(det(Cm)) + pi2factor
Er /= 2
else:
# error cuadratico sin pesos ni nada
Er = sum(er**2, axis=1)
return Er
def homDist2homUndist(xpp, ypp, distCoeffs, model, Cpp=False, Ck=False):
'''
takes ccd cordinates and projects to homogenpus coords and undistorts
'''
Cppbool = anny(Cpp)
Ckbool = anny(Ck)
if Cppbool or Ckbool: # no hay incertezas Ck ni Cpp
q, _, Jp_pp, Jp_k = homDist2homUndist_ratioJacobians(
xpp, ypp, distCoeffs, model)
xp = xpp / q # undistort in homogenous coords
yp = ypp / q
Cp = zeros((len(xp), 2, 2))
# nk = nparams[model] # distCoeffs.shape[0] # unmber of dist coeffs
if Cppbool: # incerteza Cpp
Jp_ppResh = Jp_pp.reshape((-1, 2, 1, 2, 1))
Cp = (Jp_ppResh * Cpp.reshape(
(-1, 1, 2, 2, 1)) * Jp_ppResh.transpose((0, 4, 3, 2, 1))).sum(
(2, 3))
if Ckbool: # incerteza Ck
nk = Jp_k.shape[-1]
Jp_kResh = Jp_k.reshape((-1, 2, 1, nk, 1))
Cp += (Jp_kResh * Ck.reshape(
(1, 1, nk, nk, 1)) * Jp_kResh.transpose((0, 4, 3, 2, 1))).sum(
(2, 3))
else:
# calculate ratio of undistortion
rpp = norm([xpp, ypp], axis=0)
q, _ = undistort[model](rpp, distCoeffs, quot=True, der=False)
xp = xpp / q # undistort in homogenous coords
yp = ypp / q
Cp = False
return xp, yp, Cp
def errorCuadraticoImagen(Xext, Xint, Ns, params, j, mahDist=False):
'''
el error asociado a una sola imagen, es para un par rvec, tvec
necesita tambien los paramatros intrinsecos
if mahDist=True it returns the squared mahalanobis distance for the
proyection points
'''
# saco los parametros de flat para que los use la func de projection
cameraMatrix, distCoeffs = flat2int(Xint, Ns, params['model'])
rvec, tvec = flat2ext(Xext)
# saco los parametros auxiliares
imagePoints = params["imagePoints"]
model = params["model"]
chessboardModel = params["chessboardModel"]
Cccd = params["Cccd"]
Cf = params["Cf"]
Ck = params["Ck"]
Crt = params["Crt"]
Cfk = params["Cfk"]
try: # check if there is covariance for this image
Cov = Cccd[j]
except:
Cov = None
# hago la proyeccion
xi, yi = imagePoints[j].T
xm, ym, Cm = cl.inverse(xi, yi, rvec, tvec, cameraMatrix, distCoeffs,
model, Cov, Cf, Ck, Crt[j], Cfk)
# error
er = ([xm, ym] - chessboardModel[:, :2].T).T
Cmbool = anny(Cm)
if Cmbool:
# devuelvo error cuadratico pesado por las covarianzas
S = np.linalg.inv(Cm) # inverse of covariance matrix
# distancia mahalanobis
Er = np.sum(er.reshape((-1, 2, 1)) * S * er.reshape((-1, 1, 2)),
axis=(1, 2))
if not mahDist:
# add covariance normalisation error
Er += np.linalg.det(Cm)
else:
# error cuadratico sin pesos ni nada
Er = np.sum(er**2, axis=1)
return Er
def errorCuadraticoImagen(Xext, Xint, Ns, params, j):
'''
el error asociado a una sola imagen, es para un par rvec, tvec
necesita tambien los paramatros intrinsecos
'''
# saco los parametros de flat para que los use la func de projection
cameraMatrix, distCoeffs = flat2int(Xint, Ns)
rvec, tvec = flat2ext(Xext)
# saco los parametros auxiliares
n, m, imagePoints, model, chessboardModel, Ci = params
try: # check if there is covariance for this image
Cov = Ci[j]
except:
Cov = None
# hago la proyeccion
xm, ym, Cm = cl.inverse(imagePoints[j, 0],
rvec,
tvec,
cameraMatrix,
distCoeffs,
model,
Cccd=Cov)
# print(Cm)
# error
er = ([xm, ym] - chessboardModel[0, :, :2].T).T
Cmbool = anny(Cm)
if Cmbool:
# devuelvo error cuadratico pesado por las covarianzas
S = np.linalg.inv(Cm) # inverse of covariance matrix
# q1 = [np.sum(S[:, :, 0] * er.T, 1), # fastest way I found to do product
# np.sum(S[:, :, 1] * er.T, 1)]
# distancia mahalanobis
Er = (er.reshape((-1, 2, 1)) * S * er.reshape((-1, 1, 2))).sum()
Er += np.log(np.linalg.det(Cm)).sum() # sumo termino de normalizacion
else:
# error cuadratico pelado, escalar
Er = np.sum(er**2)
return Er
def errorCuadraticoImagen(imagePoints, chessboardModel, rvec, tvec,
cameraMatrix, distCoeffs, model, Ci=False, Cf=False,
Ck=False, Crt=False, Cfk=False, mahDist=False):
'''
el error asociado a una sola imagen
if mahDist=True it returns the squared mahalanobis distance for the
proyection points
the result Er is such that the probability P:
Er = - log(P)
where n is the number of points, d is the number of dimensions
the line where Er += log(det(Cm)) + pi2factor adds part of the
normalising constant
'''
# hago la proyeccion
xi, yi = imagePoints.T
xm, ym, Cm = inverse(xi, yi, rvec, tvec, cameraMatrix, distCoeffs, model,
Ci, Cf, Ck, Crt, Cfk)
# error
er = ([xm, ym] - chessboardModel[:, :2].T).T
Cmbool = anny(Cm)
if Cmbool:
# devuelvo error cuadratico pesado por las covarianzas
S = inv(Cm) # inverse of covariance matrix
# distancia mahalanobis
Er = sum(er.reshape((-1, 2, 1))
* S
* er.reshape((-1, 1, 2)), axis=(1, 2))
if not mahDist:
# add covariance normalisation error
Er += log(det(Cm)) + pi2factor
Er /= 2
else:
# error cuadratico sin pesos ni nada
Er = sum(er**2, axis=1)
return Er