def costo(pars, rayo_pix_costo):
# pars : [n1,n2,z0,xfeR,yfeR,zfeR,alfaR,betaR,gamaR] PARAMETROS A OPTIMIZAR
# rayo_pix_costo : pixeles en la imagen FE de los puntos de calibracion
# defino la camara cuyos parametros se van optimizando
FE_Rcosto.update_parametros(
[pars[3], pars[4], pars[5], pars[6], pars[7], pars[8]])
p1.update_normal([pars[0], pars[1], 0.9])
p1.actualizarZ0(pars[2])
#retroproyecto los rayos al mundo
V_m_costo = FE_Rcosto.CameraToWorld_Proyect(rayo_pix_costo)
# interseco los rayos con el plano a optimizar
r1_costo = sim.interseccion(V_m_costo, p1)
#error
aux = r1_costo - r2
DistanciaTotal_costo = np.sum(np.linalg.norm(aux[0:2], axis=0))
# DistanciasX_costo=np.linalg.norm(aux[0],axis=0)
# DistanciasY_costo=np.linalg.norm(aux[1],axis=0)
# DistanciaTotal_costo= DistanciasX_costo+DistanciasY_costo
return DistanciaTotal_costo
parsOpt[3], parsOpt[4], parsOpt[5], parsOpt[6], parsOpt[7],
parsOpt[8]
])
vectParamFinal = np.hstack((parsOpt[0:2], parsOpt[2], parsOpt[3:9]))
#genero el plano optimizado
normalPlanoOpt = np.array([parsOpt[0], parsOpt[1], 0.9])
p1opt = sim.Plano(normalPlanoOpt, [X, Y], parsOpt[2])
#retroproyecto los rayos al mundo
V_mF = FE_Opt.CameraToWorld_Proyect(rayo_pixi)
# interseco los rayos con el plano a optimizar
r1opt = sim.interseccion(V_mF, p1opt)
rayo1_pix_OPT = FE.WorldToCamera_Proyect(r1opt)
################ VALIDACION ###########################################
rayo_pix_V = FE.WorldToCamera_Proyect(r2V)
rayo_pix_Vf = rayo_pix_V + ruido_Pixel_Validacion
#retroproyecto los rayos al mundo
V_mF_V = FE_Opt.CameraToWorld_Proyect(rayo_pix_Vf)
# interseco los rayos con el plano a optimizar
r1opt_V = sim.interseccion(V_mF_V, p1opt)
parsOpt[8]
])
vectParamFinal = np.hstack(
(parsOpt[0:2], parsOpt[2], parsOpt[3:9]))
#genero el plano optimizado
normalPlanoOpt = np.array([parsOpt[0], parsOpt[1], 0.9])
p1opt = sim.Plano(normalPlanoOpt, [X, Y], parsOpt[2])
#retroproyecto los rayos al mundo
V_mF = FE_Opt.CameraToWorld_Proyect(rayo_pixi)
# interseco los rayos con el plano a optimizar
r1opt = sim.interseccion(V_mF, p1opt)
rayo1_pix_OPT = FE.WorldToCamera_Proyect(r1opt)
'''Mido los diferentes tipos de error:
Error 3D: Error 3D [en metros] entrre los puntos retroroyectados p=(X,Y,Z) del plano optimizado (con param FE optimizados) y de la superficie real
Error mapa :Error 2D [en metros] entrre los puntos retroroyectados al mapa satelital del plano optimizado (con param FE optimizados) y de la superficie real
Error imagen FE: Error [en pixeles] de la proyeccion en el plano imagen FE entre el plano optimizado (con param FE optimizados) y la superficie real
'''
pp = rayo_pix
pp1 = rayo1_pix_OPT
#Error en mapa satelital (calibracion)