def analyticVsMC(imagePoints, Ci, cameraMatrix, Cf, distCoeffs, Ck, rtV, Crt):
rV, tV = rtV
# propagate to homogemous
xpp, ypp, Cpp = cl.ccd2hom(imagePoints, cameraMatrix, Cccd=Ci, Cf=Cf)
# go to undistorted homogenous
xp, yp, Cp = cl.homDist2homUndist(xpp, ypp, distCoeffs, model, Cpp=Cpp, Ck=Ck)
# project to map
xm, ym, Cm = cl.xypToZplane(xp, yp, rV, tV, Cp=Cp, Crt=Crt)
# generar puntos y parametros
# por suerte todas las pdfs son indep
xI = np.random.randn(N, npts, 2).dot(np.sqrt(Ci[0])) + imagePoints
rots = np.random.randn(N, 3).dot(np.sqrt(Cr)) + rV
tras = np.random.randn(N, 3).dot(np.sqrt(Ct)) + tV
kD = np.random.randn(N, Ck.shape[0]).dot(np.sqrt(Ck)) + distCoeffs # disorsion
kL = np.zeros((N, 3, 3), dtype=float) # lineal
kL[:, 2, 2] = 1
kL[:, :2, 2] = np.random.randn(N, 2).dot(np.sqrt(Cf[2:, 2:])) + cameraMatrix[:2, 2]
kL[:, [0, 1], [0, 1]] = np.random.randn(N, 2).dot(np.sqrt(Cf[:2, :2]))
kL[:, [0, 1], [0, 1]] += cameraMatrix[[0, 1], [0, 1]]
xPP, yPP, xP, yP, xM, yM = np.empty((6, N, npts), dtype=float)
for i in range(N):
# % propagate to homogemous
xPP[i], yPP[i] = cl.ccd2hom(xI[i], kL[i])
# % go to undistorted homogenous
xP[i], yP[i] = cl.homDist2homUndist(xPP[i], yPP[i], kD[i], model)
# % project to map
xM[i], yM[i] = cl.xypToZplane(xP[i], yP[i], rots[i], tras[i])
muI, CiNum = calculaCovarianza(xI[:, :, 0], xI[:, :, 1])
muPP, CppNum = calculaCovarianza(xPP, yPP)
muP, CpNum = calculaCovarianza(xP, yP)
muM, CmNum = calculaCovarianza(xM, yM)
ptsTeo = [imagePoints, [xpp, ypp], [xp, yp], [xm, ym]]
ptsNum = [muI, muPP, muP, muM]
covTeo = [Ci, Cpp, Cp, Cm]
covNum = [CiNum, CppNum, CpNum, CmNum]
return ptsTeo, ptsNum, covTeo, covNum
def step1vsParams(params, imagePoints):
cameraMatrix = np.zeros((3,3))
cameraMatrix[[0 , 1, 0, 1], [0, 1, 2, 2]] = params
cameraMatrix[2,2] = 1
xpp, ypp, Cpp = cl.ccd2hom(imagePoints, cameraMatrix)
return np.array([xpp, ypp])
def step1vsX(X, cameraMatrix):
imagePoints = X.T
xpp, ypp, Cpp = cl.ccd2hom(imagePoints, cameraMatrix)
return np.array([xpp, ypp])
# COMPARO JACOBIANOS
plt.figure()
plt.title('Jacobianos relative error')
plt.plot(jkanal, jknumDif / np.abs(jkanal), '+', label='params')
plt.plot(jianal.reshape((-1,1)), (jinum / jianal).T, 'xk', label='posicion')
plt.yscale('log')
plt.legend(loc=0)
#### COMPARACION DE COVARIANZAS ####
# %% montecarlo vs analitico comparo covarianzas
mahalanobis = np.zeros([n, m])
for j in range(n):
# parte ANALITICA propagacion de incerteza
xpp, ypp, Cpp = cl.ccd2hom(imagePoints[j,0], cameraMatrix, Cccd=Cccd, Cf=Cf)
posIgen = imagePoints[j,0].reshape((1,-1,2)) + noisePos * stdIm
xyp = np.zeros((nSampl,m,2))
# parte MONTE CARLO
for i in range(nSampl):
# posMap[i,:,0], posMap[i,:,1], _ = cl.ccd2hom(posIgen[i], cameraMatrix)
xyp[i,:,0], xyp[i,:,1], _ = cl.ccd2hom(posIgen[i], kG[i])
# # COMPARO VARIANZAS
# # medias y varianzas de las nubes de puntos
# posIMean = np.mean(posIgen, axis=0)
# dif = (posIgen - posIMean).T
# posIVar = np.sum([dif[0] * dif, dif[1] * dif], axis=-1) / (nSampl - 1)
# posIVar = posIVar.transpose((2,0,1))
nSampl = int(1e3) # cantidad de samples MC ep = 1e-3 # relative standard deviation in parameters stdIm = 1e-1 # error in image # %% test CCD2HOM # no hay grados de libertad porque es lineal impts = np.array([[0.0, 0.0]]) Cccd = np.eye(2) * stdIm**2 Cf = np.diag(cameraMatrix[[0,1,0,1],[0,1,2,2]] * ep)**2 # jacobianos xpp, ypp, Cpp = cl.ccd2hom(impts, cameraMatrix, Cccd=Cccd, Cf=Cf)
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)
#fig.savefig("distortedPoints3.png")
# calculate distorted radius
xpp, ypp = cl.ccd2hom(imagePoints[j, 0], cameraMatrix)
RPP[model].append(ln.norm([xpp, ypp], axis=0))
# calculate undistorted homogenous radius from 3D rototraslation
xyp = cl.rotoTrasHomog(chessboardModel, rVecs[j], tVecs[j])
RP[model].append(ln.norm(xyp, axis=1))
# to array
RP[model] = np.array(RP[model]).flatten()
RPP[model] = np.array(RPP[model]).flatten()
OP[model] = np.array(OP[model])
IP[model] = np.array(IP[model])
0
# %% plot comparison of models
jinum = np.abs(Jd_inumeric[:, [0, 1], [0, 1]] - jianal)
# COMPARO JACOBIANOS
plt.figure()
plt.title('Jacobianos relative error')
plt.plot(jkanal, jknumDif / np.abs(jkanal), '+', label='params')
plt.plot(jianal.reshape((-1, 1)), (jinum / jianal).T, 'xk', label='posicion')
plt.yscale('log')
plt.legend(loc=0)
#### COMPARACION DE COVARIANZAS ####
## Solo incerteza en coordenadas imagen ##
# parte ANALITICA propagacion de incerteza
xpp, ypp, Cpp = cl.ccd2hom(imagePoints[j, 0], cameraMatrix, Cccd=Cccd)
# parte MONTE CARLO
for i in range(nSampl):
posMap[i, :, 0], posMap[i, :, 1], _ = cl.ccd2hom(posIgen[i], cameraMatrix)
# COMPARO VARIANZAS
# medias y varianzas de las nubes de puntos
posIMean = np.mean(posIgen, axis=0)
dif = (posIgen - posIMean).T
posIVar = np.sum([dif[0] * dif, dif[1] * dif], axis=-1) / (nSampl - 2)
posIVar = posIVar.transpose((2, 0, 1))
posMapMean = np.mean(posMap, axis=0)
dif = (posMap - posMapMean).T
posMapVar = np.sum([dif[0] * dif, dif[1] * dif], axis=-1) / (nSampl - 1)
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)
#fig.savefig("distortedPoints3.png")
# calculate distorted radius
xpp, ypp = cl.ccd2hom(imagePoints[j,0], cameraMatrix)
RPP[model].append(ln.norm([xpp,ypp], axis=0))
# calculate undistorted homogenous radius from 3D rototraslation
xyp = cl.rotoTrasHomog(chessboardModel, rVecs[j], tVecs[j])
RP[model].append(ln.norm(xyp, axis=1))
# to array
RP[model] = np.array(RP[model]).flatten()
RPP[model] = np.array(RPP[model]).flatten()
OP[model] = np.array(OP[model])
IP[model] = np.array(IP[model])
0
# %% plot comparison of models