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 func(X):
xp, yp, rV, tV = X[0], X[1], X[2:5], X[5:]
y1, y2, _ = cl.xypToZplane(xp, yp, rV, tV)
return array([y1,y2])
def step3vsX(X, rt):
xp, yp = X
xm, ym, Cm= cl.xypToZplane(xp, yp, rt[:3], rt[3:])
return np.array([xm, ym])
#ax.plot(xPPgen, yPPgen, '.b')
#cl.plotPointsUncert(ax, Cpp, xpp, ypp, 'k')
#cl.plotPointsUncert(ax, varPP, xPPm, yPPm, 'b')
fig = plt.figure()
ax = fig.gca()
ax.set_title('propagacion')
ax.scatter(xP, yP, marker='.', c='b', s=1)
cl.plotPointsUncert(ax, Cp, xp, yp, 'k')
cl.plotPointsUncert(ax, varP, xPm, yPm, 'b')
# %% TEST EACH STEP. STEP 3: PROJECT TO MAP, UNDO ROTOTRASLATION
# parte ANALITICA propagacion de incerteza
xm, ym, Cm = cl.xypToZplane(xp, yp, rVecs[j], tVecs[j], Cp=Cp, Crt=Crt)
# parte ANALITICA jacobianos
JXm_Xp, JXm_rtV = cl.jacobianosHom2Map(xp, yp, rVecs[j], tVecs[j])
# parte MONTE CARLO
# dejo los valores preparados
# matriz para rotoescalear el ruido
convEllip3 = np.array([cl.unit2CovTransf(c) for c in Cp])
# aplico rotoescaleo
xypertub3 = (convEllip3.reshape((1,-1,2,2)) *
noisePos.reshape((nSampl,-1,1,2))).sum(-1)
xPgen = xp.reshape((1, -1)) + xypertub3[:,:,0]
yPgen = yp.reshape((1, -1)) + xypertub3[:,:,1]
xM = np.zeros_like(xPgen)
# mismo centro para todos los puntos
xp, yp = np.array([[1, 1]] * N).T
xm = np.empty_like(xp)
ym = np.empty_like(xp)
rV, tV = np.random.rand(2, 3)
# genero muchas covarianzas diferentes
#Cp = np.random.rand(N,2,2)*10 - 5 # np.array([[1, 0.3],[0.3, 1]])/10
c = np.random.rand(2, 2) * 10 - 5
c = c.dot(c.T)
scale = np.linspace(0.1, 10, num=N)
Cp = np.empty((N, 2, 2), dtype=float)
Cm = np.empty_like(Cp)
# mapeo las coordenadas y las covarianzas
for i in range(N):
Cp[i] = c * scale[i]
xm[i], ym[i], Cm[i] = cl.xypToZplane(xp[i], yp[i], rV, tV, Cp=Cp[i])
# suma de autovalores (area)
lp = Cp[:, [0, 1], [0, 1]].sum(axis=1)
lm = Cm[:, [0, 1], [0, 1]].sum(axis=1)
plt.plot(lp, lm, '+')
'''
esto me demuestra que el tamaño de la elipse es lineal si se deja la rototraslacion fija, o sea que deberia ser lo mismo que hacer la
pero el centro de la elipse seguramente se mueve... eso no esta bien
'''
xp, yp = np.array([[1,1]]*N).T
xm = np.empty_like(xp)
ym = np.empty_like(xp)
rV, tV = np.random.rand(2,3)
# genero muchas covarianzas diferentes
#Cp = np.random.rand(N,2,2)*10 - 5 # np.array([[1, 0.3],[0.3, 1]])/10
c = np.random.rand(2,2)*10 - 5
c = c.dot(c.T)
scale = np.linspace(0.1,10,num=N)
Cp = np.empty((N,2,2), dtype=float)
Cm = np.empty_like(Cp)
# mapeo las coordenadas y las covarianzas
for i in range(N):
Cp[i] = c * scale[i]
xm[i], ym[i], Cm[i] = cl.xypToZplane(xp[i], yp[i], rV, tV, Cp=Cp[i])
# suma de autovalores (area)
lp = Cp[:,[0,1],[0,1]].sum(axis=1)
lm = Cm[:,[0,1],[0,1]].sum(axis=1)
plt.plot(lp, lm, '+')
'''
esto me demuestra que el tamaño de la elipse es lineal si se deja la rototraslacion fija, o sea que deberia ser lo mismo que hacer la
pero el centro de la elipse seguramente se mueve... eso no esta bien
'''
def func(X):
xp, yp, rV, tV = X[0], X[1], X[2:5], X[5:]
y1, y2, _ = cl.xypToZplane(xp, yp, rV, tV)
return array([y1, y2])