def projectionPlots(rV, tV, data):
'''
compara los puntos de calibracio y sus proyecciones en las tres etapas
'''
imagePoints, objectPoints, cameraMatrix, model = data
imagePointsProj = cl.direct(objectPoints, rV, tV, cameraMatrix, distCoeffs,
model)
objectPointsProj = cl.inverse(imagePoints, rV, tV, cameraMatrix,
distCoeffs, model)
xc, yc, zc = cl.rotoTrasRodri(objectPoints, rV, tV).T
xp, yp = cl.ccd2homUndistorted(imagePoints, cameraMatrix, distCoeffs,
model)
xp2, yp2 = cl.rotoTrasHomog(objectPoints, rV, tV).T
ll = np.linalg.norm(objectPoints, axis=0)
r = np.mean(ll) / 7
plt.figure()
plt.title('image Points')
plt.imshow(img)
plt.scatter(imagePoints[:, 0],
imagePoints[:, 1],
marker='+',
label='calibration')
plt.scatter(imagePointsProj[:, 0],
imagePointsProj[:, 1],
marker='x',
label='projected')
plt.legend()
#plt.figure()
#plt.title('homogenous Points')
#plt.scatter(xp, yp, marker='+', label='undistorted from image')
#plt.scatter(xp2, yp2, marker='x', label='rptotraslated from map')
#plt.legend()
plt.figure()
plt.title('object Points')
plt.scatter(objectPoints[:, 0],
objectPoints[:, 1],
marker='+',
label='calibration')
plt.scatter(objectPointsProj[:, 0],
objectPointsProj[:, 1],
marker='x',
label='projected')
plt.legend()
#fig = plt.figure()
#plt.title('3D object Points, camera ref frame')
#ax = fig.gca(projection='3d')
#ax.axis('equal')
#ax.scatter(xc, yc, zc)
#ax.plot([0, r], [0, 0], [0, 0], "-r")
#ax.plot([0, 0], [0, r], [0, 0], "-b")
#ax.plot([0, 0], [0, 0], [0, r], "-k")
return imagePointsProj, objectPointsProj
def Esq(imagePoints, objectPoints, rVec, tVec, cameraMatrix, distCoeffs, model):
objectPointsProj = cl.inverse(imagePoints, rVec, tVec, cameraMatrix,
distCoeffs, model)
e = objectPoints - objectPointsProj
return np.sum(e**2)
def Esq(imagePoints, objectPoints, rVec, tVec, cameraMatrix, distCoeffs,
model):
objectPointsProj = cl.inverse(imagePoints, rVec, tVec, cameraMatrix,
distCoeffs, model)
e = objectPoints - objectPointsProj
return np.sum(e**2)
def project2diagonalisedError(xAll):
'''
no me queda otra que leer case desde el main como una variable global
'''
c = case
xm, ym, Cm = cl.inverse(c.xi, c.yi, xAll[:3], xAll[3:], c.cameraMatrix,
c.distCoeffs, c.model, c.Ci, c.Cf, c.Ck, c.covOpt, c.Cfk)
xNorm, yNorm = cl.points2linearised(xm - c.objpoints[:, 0],
ym - c.objpoints[:, 1], Cm).T
return np.concatenate([xNorm, yNorm])
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 projectionPlots(rV, tV, data):
'''
compara los puntos de calibracio y sus proyecciones en las tres etapas
'''
imagePoints, objectPoints, cameraMatrix, model = data
imagePointsProj = cl.direct(objectPoints, rV, tV, cameraMatrix, distCoeffs, model)
objectPointsProj = cl.inverse(imagePoints, rV, tV, cameraMatrix, distCoeffs, model)
xc, yc, zc = cl.rotoTrasRodri(objectPoints,rV,tV).T
xp, yp = cl.ccd2homUndistorted(imagePoints, cameraMatrix, distCoeffs, model)
xp2, yp2 = cl.rotoTrasHomog(objectPoints, rV, tV).T
ll = np.linalg.norm(objectPoints, axis=0)
r = np.mean(ll) / 7
plt.figure()
plt.title('image Points')
plt.imshow(img)
plt.scatter(imagePoints[:,0], imagePoints[:,1], marker='+', label='calibration')
plt.scatter(imagePointsProj[:,0], imagePointsProj[:,1], marker='x', label='projected')
plt.legend()
#plt.figure()
#plt.title('homogenous Points')
#plt.scatter(xp, yp, marker='+', label='undistorted from image')
#plt.scatter(xp2, yp2, marker='x', label='rptotraslated from map')
#plt.legend()
plt.figure()
plt.title('object Points')
plt.scatter(objectPoints[:,0], objectPoints[:,1], marker='+', label='calibration')
plt.scatter(objectPointsProj[:,0], objectPointsProj[:,1], marker='x', label='projected')
plt.legend()
#fig = plt.figure()
#plt.title('3D object Points, camera ref frame')
#ax = fig.gca(projection='3d')
#ax.axis('equal')
#ax.scatter(xc, yc, zc)
#ax.plot([0, r], [0, 0], [0, 0], "-r")
#ax.plot([0, 0], [0, r], [0, 0], "-b")
#ax.plot([0, 0], [0, 0], [0, r], "-k")
return imagePointsProj, objectPointsProj
def perform(self, node, inputs_storage, output_storage):
# global projCount
# projCount += 1
# self.count += 1
# print('IDX %d projection %d, global %d' %
# (self.idx, self.count, projCount))
Xint, Xext = inputs_storage
# saco los parametros de flat para que los use la func de projection
# print(Xint)
cameraMatrix, distCoeffs = bl.flat2int(Xint, Ns, model)
# print(cameraMatrix, distCoeffs)
xy = np.zeros((n, m, 2))
cm = np.zeros((n, m, 2, 2))
for j in range(n):
rVec, tVec = bl.flat2ext(Xext[j])
xy[j, :, 0], xy[j, :, 1], cm[j] = cl.inverse(imagePoints.reshape(
(n, m, 2))[j],
rVec,
tVec,
cameraMatrix,
distCoeffs,
model,
Cccd=Ci[j])
# print(xy)
xy -= objpoints2D
S = np.linalg.inv(cm)
u, s, v = np.linalg.svd(S)
sdiag = np.zeros_like(u)
sdiag[:, :, [0, 1], [0, 1]] = np.sqrt(s)
A = (u.reshape((n, m, 2, 2, 1, 1)) * sdiag.reshape(
(n, m, 1, 2, 2, 1)) * v.transpose((0, 1, 3, 2)).reshape(
(n, m, 1, 1, 2, 2))).sum(axis=(3, 4))
xy = np.sum(xy.reshape((n, m, 2, 1)) * A, axis=3)
output_storage[0][0] = xy
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 perform(self, node, inputs_storage, output_storage):
# global projCount
# projCount += 1
# self.count += 1
# print('IDX %d projection %d, global %d' %
# (self.idx, self.count, projCount))
Xint, Xext = inputs_storage
# saco los parametros de flat para que los use la func de projection
# print(Xint)
cameraMatrix, distCoeffs = bl.flat2int(Xint, Ns, model)
# print(cameraMatrix, distCoeffs)
xy = np.zeros((n, m, 2))
cm = np.zeros((n, m, 2, 2))
for j in range(n):
rVec, tVec = bl.flat2ext(Xext[j])
xy[j, :, 0], xy[j, :, 1], cm[j] = cl.inverse(
imagePoints.reshape((n, m, 2))[j], rVec, tVec,
cameraMatrix, distCoeffs, model, Cccd=Ci[j])
# print(xy)
xy -= objpoints2D
S = np.linalg.inv(cm)
u, s, v = np.linalg.svd(S)
sdiag = np.zeros_like(u)
sdiag[:,:,[0,1],[0,1]] = np.sqrt(s)
A = (u.reshape((n, m, 2, 2, 1, 1)) *
sdiag.reshape((n, m, 1, 2 ,2, 1)) *
v.transpose((0,1,3,2)).reshape((n, m, 1, 1, 2, 2))
).sum(axis=(3,4))
xy = np.sum(xy.reshape((n,m,2,1)) * A, axis=3)
output_storage[0][0] = xy
# %%
ret = opt.minimize(objective, case.xAllT)
xAllOpt = ret.x
covOpt = np.exp(ret.fun / 2) * ret.hess_inv
sigOpt = np.sqrt(np.diag(covOpt))
crrOpt = covOpt / (sigOpt.reshape((-1, 1)) * sigOpt.reshape((1, -1)))
# plt.matshow(crrOpt, vmin=-1, vmax=1, cmap='coolwarm')
xm, ym, Cm = cl.inverse(xi, yi, xAllOpt[:3], xAllOpt[3:], cameraMatrix,
distCoeffs, model, Ci, Cf, Ck, covOpt, Cfk)
plt.figure()
ax = plt.gca()
ax.scatter(xm, ym)
ax.scatter(objpoints[:, 0], objpoints[:, 1])
for i in range(len(xm)):
cl.plotEllipse(ax, Cm[i], xm[i], ym[i], 'k')
ax.axis('equal')
reload(cl)
xNorm, yNorm = cl.points2linearised(xm - objpoints[:, 0],
ym - objpoints[:, 1], Cm).T
fig = plt.figure()
ax = fig.gca()
ax.set_title('propagacion')
ax.scatter(xM, yM, marker='.', c='b', s=1)
cl.plotPointsUncert(ax, Cm, xm, ym, 'k')
cl.plotPointsUncert(ax, varM, xMm, yMm, 'b')
# %% TESTEO LOS TRES PASOS JUNTOS
# hago todos los mapeos de Monte Carlo
for posM, posI, pars in zip(posMap, posIgen, parsGen):
r, t, k, d = sacoParmams(pars)
posM[:,0], posM[:,1], _ = cl.inverse(posI, r, t, k, d, model)
# %% saco media y varianza de cada nube de puntos
posMapMean = np.mean(posMap, axis=0)
dif = (posMap - posMapMean).T
posMapVar = np.mean([dif[0] * dif, dif[1] * dif], axis=-1).T
# %% mapeo propagando incerteza los valores con los que comparar
xmJ, ymJ, CmJ = cl.inverse(imagePoints[j,0], rVecs[j], tVecs[j],
cameraMatrix, distCoeffs, model,
Cccd, Cf, Ck, Crt)
XJ = np.array([xmJ, ymJ]).T
# %% TEST INVERSE MAPPING (DISTORTION MODEL)
# pruebo con la imagen j-esima
if plotCornersInverse:
plt.figure()
plt.plot(chessboardModel[0, :, 0],
chessboardModel[0, :, 1],
'xr',
markersize=10)
for j in range(n): # range(len(imgpoints)):
rvec = rVecs[j]
tvec = tVecs[j]
xPos, yPos, cc = cl.inverse(imagePoints[j, 0], rvec, tvec, K, D, model)
im = plt.imread(images[j])
print(j, cc, images[j])
plt.plot(xPos, yPos, '+b', markersize=10)
# %% START BAYESIAN CALIBRATION
from dev import bayesLib as bl
from importlib import reload
import scipy.stats as sts
import sys
sys.path.append("/home/sebalander/Code/sebaPhD")
reload(cl)
reload(bl)
# standar deviation from subpixel epsilon used
# saco condiciones iniciales para los parametros uso opencv y heuristica
#reload(cl)
if model == modelos[3]:
# saco rVecs, tVecs de la galera
# tvecIni = camPrior
# rvecIni = np.array([np.pi, 0, 0]) # camara apuntando para abajo
Xext0 = camPrior.copy()
rvecIni, tvecIni = bl.flat2ext(Xext0)
else:
ret, rvecIni, tvecIni, inliers = cv2.solvePnPRansac(calibPts, xIm,
cameraMatrix, distCoeffs)
Xext0 = bl.ext2flat(rvecIni, tvecIni)
xpr, ypr, c = cl.inverse(xIm, rvecIni, tvecIni, cameraMatrix, distCoeffs, model=model)
plt.plot(xpr, ypr, '*')
# %%
std = 1.0
# output file
#extrinsicParamsOutFile = extrinsicFolder + camera + model + "intrinsicParamsAP"
#extrinsicParamsOutFile = extrinsicParamsOutFile + str(std) + ".npy"
Ci = np.repeat([ std**2 * np.eye(2)],n*m, axis=0).reshape(n,m,2,2)
params = dict()
params["n"] = n
params["m"] = m
params["imagePoints"] = xIm.reshape((1,1,-1,2))
IP[model] = []
OP[model] = []
# MAP TO HOMOGENOUS PLANE TO GET RADIUS
for j in range(n):
print('\t imagen', j)
rvec = rVecs[j]
tvec = tVecs[j]
imagePointsProjected = cl.direct(chessboardModel, rvec, tvec,
cameraMatrix, distCoeffs, model)
imagePointsProjected = imagePointsProjected.reshape((-1, 2))
IP[model].append(imagePointsProjected)
objectPointsProjected = cl.inverse(imagePoints[j, 0], rvec, tvec,
cameraMatrix, distCoeffs, model)
#objectPointsProjected = opbjectPointsProjected.reshape((-1,3))
OP[model].append(objectPointsProjected)
if plotCorners:
imagePntsX = imagePoints[j, 0, :, 0]
imagePntsY = imagePoints[j, 0, :, 1]
xPos = imagePointsProjected[:, 0]
yPos = imagePointsProjected[:, 1]
plt.figure(j)
im = plt.imread(images[j])
plt.imshow(im)
plt.plot(imagePntsX, imagePntsY, 'xr', markersize=10)
plt.plot(xPos, yPos, '+b', markersize=10)
#
##
#
## %% go to undistorted homogenous
#xp, yp, Cp = cl.homDist2homUndist(xpp, ypp, distCoeffs, model, Cpp=Cpp, Ck=Ck)
#
#fig = plt.figure()
#ax = fig.gca()
#for i in range(nPts):
# cl.plotEllipse(ax, Cp[i], xp[i], yp[i], 'b')
##
#
## %% project to map
#xm, ym, Cm = cl.xypToZplane(xp, yp, rV, tV, Cp=Cp, Crt=[Cr, Ct])
#
#fig = plt.figure()
#ax = fig.gca()
#ax.plot(xm, ym, '+', markersize=2)
#for i in range(nPts):
# cl.plotEllipse(ax, Cm[i], xm[i], ym[i], 'b')
# %% in one line
xm, ym, Cm = cl.inverse(imagePoints, rV, tV, cameraMatrix, distCoeffs, model,
Cccd, Cf, Ck, Crt)
fig = plt.figure()
ax = fig.gca()
ax.plot(xm, ym, '+', markersize=2)
for i in range(nPts):
cl.plotEllipse(ax, Cm[i], xm[i], ym[i], 'b')
# Cfk = np.eye(distCoeffs.shape[0], 4) # rows for distortion coeffs
# Crt = np.eye(6) # 6x6 covariance of pose
# Crt[[0,1,2],[0,1,2]] *= np.deg2rad(1)**2 # 5 deg stdev in every angle
# Crt[[3,4,5],[3,4,5]] *= 0.01**2 # 1/100 of the unit length as std for pos
# Crt = np.repeat([Crt] , n, axis=0)
Crt = np.repeat([False], n) # no RT error
# output file
intrinsicParamsOutFile = imagesFolder + camera + model + "intrinsicParamsML"
intrinsicParamsOutFile = intrinsicParamsOutFile + str(stdPix) + ".npy"
# pruebo con un par de imagenes
for j in range(0, n, 3):
xm, ym, Cm = cl.inverse(imagePoints[j, 0], rVecs[j], tVecs[j],
cameraMatrix,
distCoeffs, model, Cccd=Ci[j], Cf=False, Ck=False,
Crt=False, Cfk=False)
print(xm, ym, Cm)
# datos medidos, observados, experimentalmente en el mundo y en la imagen
yObs = objpoints2D.reshape(-1)
# no usar las constantes como tensores porque da error...
# # diccionario de parametros, constantes de calculo
# xObsConst = T.as_tensor_variable(imagePoints.reshape((n,m,2)), 'xObsConst',
# ndim=3)
# CiConst = T.as_tensor_variable(Ci, 'cIConst', ndim=4)
#fig.savefig("distortedPoints3.png")
#
# %% TEST INVERSE MAPPING (DISTORTION MODEL)
# pruebo con la imagen j-esima
if plotCornersInverse:
plt.figure()
plt.plot(chessboardModel[0,:,0], chessboardModel[0,:,1], 'xr', markersize=10)
for j in range(n): # range(len(imgpoints)):
rvec = rVecs[j]
tvec = tVecs[j]
xPos, yPos, cc = cl.inverse(imagePoints[j, 0], rvec, tvec, K, D, model)
im = plt.imread(images[j])
print(j, cc, images[j])
plt.plot(xPos, yPos, '+b', markersize=10)
# Crt[[3,4,5],[3,4,5]] *= 0.01**2 # 1/100 of the unit length as std for pos
# Crt = np.repeat([Crt] , n, axis=0)
Crt = np.repeat([False], n) # no RT error
# output file
intrinsicParamsOutFile = imagesFolder + camera + model + "intrinsicParamsML"
intrinsicParamsOutFile = intrinsicParamsOutFile + str(stdPix) + ".npy"
# pruebo con un par de imagenes
for j in range(0, n, 3):
xm, ym, Cm = cl.inverse(imagePoints[j, 0],
rVecs[j],
tVecs[j],
cameraMatrix,
distCoeffs,
model,
Cccd=Ci[j],
Cf=False,
Ck=False,
Crt=False,
Cfk=False)
print(xm, ym, Cm)
# datos medidos, observados, experimentalmente en el mundo y en la imagen
yObs = objpoints2D.reshape(-1)
# no usar las constantes como tensores porque da error...
# # diccionario de parametros, constantes de calculo
# xObsConst = T.as_tensor_variable(imagePoints.reshape((n,m,2)), 'xObsConst',
# ndim=3)
# CiConst = T.as_tensor_variable(Ci, 'cIConst', ndim=4)
# cl.plotEllipse(ax, Cpp[i], xpp[i], ypp[i], 'b')
#
##
#
## %% go to undistorted homogenous
#xp, yp, Cp = cl.homDist2homUndist(xpp, ypp, distCoeffs, model, Cpp=Cpp, Ck=Ck)
#
#fig = plt.figure()
#ax = fig.gca()
#for i in range(nPts):
# cl.plotEllipse(ax, Cp[i], xp[i], yp[i], 'b')
##
#
## %% project to map
#xm, ym, Cm = cl.xypToZplane(xp, yp, rV, tV, Cp=Cp, Crt=[Cr, Ct])
#
#fig = plt.figure()
#ax = fig.gca()
#ax.plot(xm, ym, '+', markersize=2)
#for i in range(nPts):
# cl.plotEllipse(ax, Cm[i], xm[i], ym[i], 'b')
# %% in one line
xm, ym, Cm = cl.inverse(imagePoints, rV, tV, cameraMatrix, distCoeffs, model, Cccd, Cf, Ck, Crt)
fig = plt.figure()
ax = fig.gca()
ax.plot(xm, ym, '+', markersize=2)
for i in range(nPts):
cl.plotEllipse(ax, Cm[i], xm[i], ym[i], 'b')
#reload(cl)
if model == modelos[3]:
# saco rVecs, tVecs de la galera
# tvecIni = camPrior
# rvecIni = np.array([np.pi, 0, 0]) # camara apuntando para abajo
Xext0 = camPrior.copy()
rvecIni, tvecIni = bl.flat2ext(Xext0)
else:
ret, rvecIni, tvecIni, inliers = cv2.solvePnPRansac(
calibPts, xIm, cameraMatrix, distCoeffs)
Xext0 = bl.ext2flat(rvecIni, tvecIni)
xpr, ypr, c = cl.inverse(xIm,
rvecIni,
tvecIni,
cameraMatrix,
distCoeffs,
model=model)
plt.plot(xpr, ypr, '*')
# %%
std = 1.0
# output file
#extrinsicParamsOutFile = extrinsicFolder + camera + model + "intrinsicParamsAP"
#extrinsicParamsOutFile = extrinsicParamsOutFile + str(std) + ".npy"
Ci = np.repeat([std**2 * np.eye(2)], n * m, axis=0).reshape(n, m, 2, 2)
params = dict()
params["n"] = n
IP[model] = []
OP[model] = []
# MAP TO HOMOGENOUS PLANE TO GET RADIUS
for j in range(n):
print('\t imagen', j)
rvec = rVecs[j]
tvec = tVecs[j]
imagePointsProjected = cl.direct(chessboardModel, rvec, tvec,
cameraMatrix, distCoeffs, model)
imagePointsProjected = imagePointsProjected.reshape((-1,2))
IP[model].append(imagePointsProjected)
objectPointsProjected = cl.inverse(imagePoints[j,0], rvec, tvec,
cameraMatrix, distCoeffs, model)
#objectPointsProjected = opbjectPointsProjected.reshape((-1,3))
OP[model].append(objectPointsProjected)
if plotCorners:
imagePntsX = imagePoints[j, 0, :, 0]
imagePntsY = imagePoints[j, 0, :, 1]
xPos = imagePointsProjected[:, 0]
yPos = imagePointsProjected[:, 1]
plt.figure(j)
im = plt.imread(images[j])
plt.imshow(im)
plt.plot(imagePntsX, imagePntsY, 'xr', markersize=10)
plt.plot(xPos, yPos, '+b', markersize=10)
