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])