def f64(x):
    def __init__(self, k=None, uv=None, s=None, camera=None, model=None):
    def __init__(self, nIm=None, nPt=None, rVecs=None, tVecs=None, objPt=None,
                 imgPt=None, imgNse=None):
    def __init__(self, ang=None, h=None, rVecs=None, tVecs=None, objPt=None,
                 imgPt=None, index10=None, imgNse=None):
    def __init__(self, Intr=None, Ches=None, Extr=None):
    def __init__(self, nIm=None, nPt=None, objPt=None, imgPt=None,
                 imgFls=None):
    def __init__(self, objPt=None, imgPt=None, priorLLA=None, imgFl=None):
    def __init__(self, carGPS=None, carIm=None):
    def __init__(self, Ches=None, Balk=None, Dete=None):
    def __init__(self, Synt=None, Real=None):
def euler(al, be, ga):
def unit2CovTransf(C):
def rotateRodrigues(x, r):
def rotoTrasRodri(x, r, t):
def rotoTrasRodriInverse(x, r, t):
def rotoTrasHomog(x, r, t):
def formatParameters(rVec, tVec, cameraMatrix, distCoeffs, model):
def retrieveParameters(params, model):
def hom2ccd(xd, yd, cameraMatrix):
def direct(objectPoints, rVec, tVec, cameraMatrix, distCoeffs, model,
           ocv=False):
def residualDirect(params, objectPoints, imagePoints, model):
def calibrateDirect(objectPoints, imagePoints, rVec, tVec, cameraMatrix,
                    distCoeffs, model):
def ccd2disJacobian(xi, yi, cameraMatrix):
def ccd2dis(xi, yi, cameraMatrix, Ci=False, Cf=False):
def dis2hom_ratioJacobians(xd, yd, distCoeffs, model):
def dis2hom(xd, yd, distCoeffs, model, Cd=False, Ck=False, Cfk=False,
            Jd_f=False):
def jacobianosHom2Map(xh, yh, rV, tV):
def xyhToZplane(xh, yh, rV, tV, Ch=False, Crt=False):
def inverse(xi, yi, rV, tV, cameraMatrix, distCoeffs, model,
            Ci=False, Cf=False, Ck=False, Crt=False, Cfk=False):
def errorCuadraticoImagen(imagePoints, chessboardModel, rvec, tvec,
                          cameraMatrix, distCoeffs, model, Ci=False, Cf=False,
                          Ck=False, Crt=False, Cfk=False, mahDist=False):
def points2linearised(x, y, C):
def cornerComparison(img, corners1, corners2=None,
                     label1='Corners', label2='Proyectados'):
def fiducialComparison(rVec, tVec, fiducial1, fiducial2=None,
                       label1='Calibration points', label2='Projected points'):
    def __init__(self, xs, ys, zs, *args, **kwargs):
    def draw(self, renderer):
def plotFrameRefBase(ax, rVec, tVec, s, *args, **kwargs):
def fiducialComparison3D(rVec, tVec, fiducial1, fiducial2=None,
                         label1='Fiducial points',
                         label2='Projected points'):
def joinPoints(pts1, pts2):
def plotHomographyToMatch(objectPoints, corners, f, imgSize, images=None):
def plotForwardHomography(objectPoints, corners, f, imgSize, Hs, images=None):
def plotBackwardHomography(objectPoints, corners, f, imgSize, Hs):
def plotRationalDist(distCoeffs, imgSize, cameraMatrix):
def plotEllipse(ax, C, mux, muy, col):
def plotPointsUncert(ax, C, mux, muy, col):
def calibrateIntrinsic(objpoints, imgpoints, imgSize, model, K=None, D=None,
                       flags=None, criteria=None):
class syntintr:
class syntches:
class syntextr:
class synt:
class realches:
class realbalk:
class realdete:
class real:
class datafull:
class Arrow3D(FancyArrowPatch):
        T = (u.reshape((-1, d, d, 1)) *
             s.reshape((n, 1, d, 1)) *
             v.reshape((n, 1, d, d)))
        R = Rodrigues(rV)[0]
        S = inv(Cm)  # inverse of covariance matrix
        S = inv(C)
        A = (u.reshape((n, 2, 2, 1, 1)) *
             sdiag.reshape((n, 1, 2, 2, 1)) *
             v.transpose((0, 2, 1)).reshape((n, 1, 1, 2, 2))
             ).sum(axis=(3, 4))
    X1 = corners1[:, 0]
    Y1 = corners1[:, 1]
        X2 = corners2[:, 0]
        Y2 = corners2[:, 1]
    X1, Y1, _ = fiducial1.T  # [:, 0]
        X2, Y2, _ = fiducial2.T  # [:, 0]
    R = Rodrigues(rVec)[0]
    XYZ = R * s + tWorld
    T = unit2CovTransf(C)
        K = eye(3)
        K[0, 2] = imgSize[1]/2
        K[1, 2] = imgSize[0]/2
        K[0, 0] = K[1, 1] = 600.0
        D = switcherIntrCalibD[model] def int2flat(cameraMatrix, distCoeffs, model):
def flat2CamMatrix(kFlat, model):
def flat2int(X, Ns, model):
def ext2flat(rVec, tVec):
def flat2ext(X):
def errorCuadraticoImagen(Xext, Xint, Ns, params, j, mahDist=False):
def etotalExt(Xext, Xint, Ns, params, j):
def errorCuadraticoInt(Xint, Ns, XextList, params, mahDist=False):
def etotalInt(Xint, Ns, XextList, params):
    def __init__(self, Ns, XextList, params, sampleador):
    def nuevo(self, old, oldE):
    def __init__(self, Xint, Ns, params, sampleador, priorExt, W):
    def ePrior(self, Xext):
    def etotalExt(self, Xext):
    def nuevo(self, old, oldE):
def procJint(Xint, Ns, XextList, params, ret):
def procHint(Xint, Ns, XextList, params, ret):
def procJext(Xext, Xint, Ns, params, j, ret):
def procHext(Xext, Xint, Ns, params, j, ret):
def jacobianos(Xint, Ns, XextList, params, hessianos=True):
def varVar2(c, N):
def varMahal(c1, n, c2, rank=False):
def trasPerMAt(k,l):
def varVarN(c, Nsamples):
    X = np.concatenate((kFlat, dFlat))
    X = np.concatenate((rFlat, tFlat))
        S = np.linalg.inv(Cm)  # inverse of covariance matrix
    A = vecA.reshape((k,l))
class metHas:
class metHasExt: class datafull:
    '''
    Nested namedtuples that hold the data for the paper

    Data
        Synt
            Intr         # listo: SyntIntr
                camera  'vcaWide' string camera model
                model   string indicating camera intrinsic model
                        ['poly', 'rational', 'fisheye', 'stereographic']
                s       is the image size
                k       sintehtic stereographic parameter
                uv      = s / 2 is the stereographic optical center
            Ches         # listo: SyntChes
                nIm     number of images
                nPt     number of point in image
                objPt   chessboard model grid
                rVecs   synth rotation vectors
                tVecs   synth tVecs
                imgPt   synth corners projected from objPt with synth params
                imgNse  noise of 1 sigma for the image
            Extr         # listo: SyntExtr
                ang     angles of synth pose tables
                h       heights  of synth pose tables
                rVecs   rotation vectors associated to angles
                tVecs   tVecs associated to angles and h
                objPt   distributed 3D points on the floor
                imgPt   projected to image
                imgNse  noise for image detection, sigma 1
                index10 indexes to select 10 points well distributed
        Real
            Ches         # listo: RealChes
                nIm     number of chess images
                nPt     number of chess points per image
                objPt   chessboard model, 3D
                imgPt   detected corners in chessboard images
                imgFls  list of paths to the chessboard images
            Balk
                objPt   calibration world points, lat lon
                imgPt   image points for calibration
                priLLA  prior lat-lon-altura
                imgFl   camera snapshot file
            Dete
                carGps  car gps coordinates
                carIm   car image detection traces
    '''
    def __init__(self, Synt=None, Real=None):
        self.Synt = Synt
        self.Real = Real class real:
    def __init__(self, Ches=None, Balk=None, Dete=None):
        self.Ches = Ches
        self.Balk = Balk
        self.Dete = Dete class realdete:
    def __init__(self, carGPS=None, carIm=None):
        self.carGPS = carGPS
        self.carIm = carIm class realbalk:
    def __init__(self, objPt=None, imgPt=None, priorLLA=None, imgFl=None):
        self.objPt = objPt
        self.imgPt = imgPt
        self.priorLLA = priorLLA
        self.imgFl = imgFl class realches:
    def __init__(self, nIm=None, nPt=None, objPt=None, imgPt=None,
                 imgFls=None):
        self.nIm = nIm
        self.nPt = nPt
        self.objPt = objPt
        self.imgPt = imgPt
        self.imgFls = imgFls class synt:
    def __init__(self, Intr=None, Ches=None, Extr=None):
        self.Intr = Intr
        self.Ches = Ches
        self.Extr = Extr class syntextr:
    def __init__(self, ang=None, h=None, rVecs=None, tVecs=None, objPt=None,
                 imgPt=None, index10=None, imgNse=None):
        self.ang = ang
        self.h = h
        self.rVecs = rVecs
        self.tVecs = tVecs
        self.objPt = objPt
        self.imgPt = imgPt
        self.index10 = index10
        self.imgNse = imgNse class syntches:
    def __init__(self, nIm=None, nPt=None, rVecs=None, tVecs=None, objPt=None,
                 imgPt=None, imgNse=None):
        self.nIm = nIm
        self.nPt = nPt
        self.rVecs = rVecs
        self.tVecs = tVecs
        self.objPt = objPt
        self.imgPt = imgPt
        self.imgNse = imgNse class syntintr:
    def __init__(self, k=None, uv=None, s=None, camera=None, model=None):
        self.k = k
        self.uv = uv
        self.s = s
        self.camera = camera
        self.model = model
import os
import numpy as np
import scipy.stats as sts
import matplotlib.pyplot as plt
import corner
import time
import theano
import theano. tensor as T
import pymc3 as pm
import scipy as sc
import seaborn as sns
import scipy.optimize as opt
import sys
import pickle
from copy import deepcopy as dc
from importlib import reload
from calibration import calibrator as cl
from dev import bayesLib as bl
from calibration.calibrator import datafull, real, realdete, realbalk, realches
from calibration.calibrator import synt, syntextr, syntches, syntintr
from scipy.optimize import curve_fit
from numpy.random import rand as rn


def radiusStepsNdim(n):
    '''
    retorna moda, la media y la desv est del radio  de pasos al samplear
    de gaussianas hiperdimensionales de sigma 1
    '''
    # https://www.wolframalpha.com/input/?i=integrate+x%5E(n-1)+exp(-x%5E2%2F2)+from+0+to+infinity
    # integral_0^∞ x^(n - 1) exp(-x^2/2) dx = 2^(n/2 - 1) Γ(n/2) for Re(n)>0
    Inorm = 2**(n / 2 - 1) * sc.special.gamma(n / 2)

    # https://www.wolframalpha.com/input/?i=integrate+x%5En+exp(-x%5E2%2F2)+from+0+to+infinity
    # integral_0^∞ x^n exp(-x^2/2) dx = 2^((n - 1)/2) Γ((n + 1)/2) for Re(n)>-1
    ExpectedR = 2**((n - 1) / 2) * sc.special.gamma((n + 1) / 2)

    # https://www.wolframalpha.com/input/?i=integrate+x%5E(n%2B1)+exp(-x%5E2%2F2)+from+0+to+infinity
    # integral_0^∞ x^(n + 1) exp(-x^2/2) dx = 2^(n/2) Γ(n/2 + 1) for Re(n)>-2
    ExpectedR2 = 2**(n / 2) * sc.special.gamma(n / 2 + 1)

    ModeR = np.sqrt(n - 1)

    # normalizo las integrales:
    ExpectedR /= Inorm
    ExpectedR2 /= Inorm

    DesvEstR = np.sqrt(ExpectedR2 - ExpectedR**2)

    return np.array([ModeR, ExpectedR, DesvEstR])

# %% LOAD DATA
# input
plotCorners = False

#import collections as clt
fullDataFile = "./resources/fullDataIntrExtr.npy"
dataFile = open(fullDataFile, "rb")
fullData = pickle.load(dataFile)
dataFile.close()


# cam puede ser ['vca', 'vcaWide', 'ptz'] son los datos que se tienen
camera = fullData.Synt.Intr.camera
#modelos = ['poly', 'rational', 'fisheye', 'stereographic']
model = fullData.Synt.Intr.model
Ns = [2, 3]

nPt = fullData.Synt.Ches.nPt  # cantidad de imagenes
nIm = fullData.Synt.Ches.nIm  # puntos por imagen



# ## load data
stdPix = 1.0
imagePoints = fullData.Synt.Ches.imgPt + stdPix * fullData.Synt.Ches.imgNse

chessboardModel = fullData.Synt.Ches.objPt
imgSize = fullData.Synt.Intr.s
# images = glob.glob(imagesFolder+'*.png')

# Parametros de entrada/salida de la calibracion
objpoints2D = np.reshape([chessboardModel[:, :2]] * nIm, (nIm, nPt, 2))

fkVT = np.concatenate([fullData.Synt.Intr.uv, [fullData.Synt.Intr.k]]) #

# load model specific data
cameraMatrixT, distCoeffsT = bl.flat2int(fkVT, Ns, model)
rVecsT = fullData.Synt.Ches.rVecs
tVecsT = fullData.Synt.Ches.tVecs

print('raw data loaded')

# %%
# pongo en forma flat los valores iniciales
XintT, Ns = bl.int2flat(cameraMatrixT, distCoeffsT, model)
XextListT = np.array([bl.ext2flat(rVecsT[i], tVecsT[i]) for i in range(nIm)])
xAllT = np.concatenate([XintT, XextListT.reshape(-1)])

nIntr = XintT.shape[0]
nExtr = nIm * 6
nFree = nExtr + nIntr
nData = nIm * nPt

# 0.1pix as image std
# https://stackoverflow.com/questions/12102318/opencv-findcornersubpix-precision
# increase to 1pix porque la posterior da demasiado rara
Ci = np.repeat([stdPix**2 * np.eye(2)], nData, axis=0).reshape(nIm, nPt, 2, 2)

Crt = np.repeat([False], nIm)  # 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, nIm, 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)

print('data formated')

# %% testeo el error calculado como antes
params = dict()

params["imagePoints"] = imagePoints
params["model"] = model
params["chessboardModel"] = chessboardModel
params["Cccd"] = Ci
params["Cf"] = False
params["Ck"] = False
params["Crt"] = [False] * nIm
params["Cfk"] = False

reload(bl)

reload(cl)