# Path: calibration/StereographicCalibration.py
# def radialDistort(rh, k, quot=False, der=False):
# def radialUndistort(rd, k, quot=False, der=False):
# 
# Path: calibration/UnifiedCalibration.py
# def formatParameters(rVec, tVec, linearCoeffs, distCoeffs):
# def retrieveParameters(params):
# def radialDistort(rp, k, quot=False):
# def radialUndistort(rp, k, quot=False):
# 
# Path: calibration/RationalCalibration.py
# def formatParameters(rVec, tVec, linearCoeffs, distCoeffs):
# def setParametersVary(params, flag):
# def retrieveParameters(params):
# def radialDistort(rp, k, quot=False, der=False):
# def radialUndistort(rpp, k, quot=False, der=False):
# def inverseRationalOwn(corners, rotMatrix, tVec, cameraMatrix, distCoeffs):
# def directRationalOwn(objectPoints, rotMatrix, tVec, cameraMatrix, distCoeffs,
#                    plot=False, img=None, corners=None):
# def calibrateIntrinsic(objpoints, imgpoints, imgSize, K, D,
#                        flags, criteria):
#     X = -(c*e - f*b)/q # check why wrong sign, why must put '-' in front?
#     Y = -(f*a - c*d)/q
#     XYZ = np.array([X, Y, np.zeros(shape[1])]).T
#     XYZ = np.reshape(XYZ, shape)
#         D = zeros((1, 8))
# 
# Path: calibration/FisheyeCalibration.py
# def radialDistort(rp, k, quot=False, der=False):
# def radialUndistort(rpp, k, quot=False, der=False):
# def calibrateIntrinsic(objpoints, imgpoints, imgSize, K, D,
#                        flags, criteria):
#         D = zeros((4))
# 
# Path: calibration/PolyCalibration.py
# def formatParameters(rVec, tVec, linearCoeffs, distCoeffs):
# def setParametersVary(params, flag):
# def retrieveParameters(params):
# def radialDistort(rp, k, quot=False, der=False):
# def radialUndistort(rpp, k, quot=False, der=False):
# def inverseRationalOwn(corners, rotMatrix, tVec, cameraMatrix, distCoeffs):
# def directRationalOwn(objectPoints, rotMatrix, tVec, cameraMatrix, distCoeffs,
#                    plot=False, img=None, corners=None):
# def calibrateIntrinsic(objpoints, imgpoints, imgSize, K, D,
#                        flags, criteria):
#     X = -(c*e - f*b)/q # check why wrong sign, why must put '-' in front?
#     Y = -(f*a - c*d)/q
#     XYZ = array([X, Y, zeros(shape[1])]).T
#     XYZ = reshape(XYZ, shape)
#         D = zeros((1, 5))


from matplotlib.pyplot import plot, imshow, legend, show, figure, gcf, imread
from matplotlib.pyplot import xlabel, ylabel
from cv2 import Rodrigues  # , homogr2pose
from numpy import max, zeros, array, sqrt, roots, diag, sum, log
from numpy import sin, cos, cross, ones, concatenate, flipud, dot, isreal
from numpy import linspace, polyval, eye, linalg, mean, prod, vstack
from numpy import ones_like, zeros_like, pi, float64, transpose
from numpy import any as anny
from numpy.linalg import svd, inv, det
from scipy.linalg import norm
from scipy.special import chdtri
from matplotlib.patches import FancyArrowPatch
from mpl_toolkits.mplot3d import proj3d
from importlib import reload
from calibration import StereographicCalibration as stereographic
from calibration import UnifiedCalibration as unified
from calibration import RationalCalibration as rational
from calibration import FisheyeCalibration as fisheye
from calibration import PolyCalibration as poly
        Cd = zeros((xd.shape[0], 2, 2), dtype=float64)  # create cov matrix
        Jd_i, Jd_f = ccd2disJacobian(xi, yi, cameraMatrix)

        if Cibool:
            Jd_iResh = Jd_i.reshape((-1, 2, 2, 1, 1))
            Cd += (Jd_iResh *
                   Ci.reshape((-1, 1, 2, 2, 1)) *
                   Jd_iResh.transpose((0, 4, 3, 2, 1))
                   ).sum((2, 3))

        if Cfbool:
            # propagate uncertainty via Jacobians
            Jd_fResh = Jd_f.reshape((-1, 2, 4, 1, 1))
            Cd += (Jd_fResh *
                   Cf.reshape((-1, 1, 4, 4, 1)) *
                   Jd_fResh.transpose((0, 4, 3, 2, 1))
                   ).sum((2, 3))
    else:
        Cd = False  # return without covariance matrix
        Jd_f = False

    return xd, yd, Cd, Jd_f


# switcher for radial un-distortion
undistort = {
    'stereographic': stereographic.radialUndistort,
    'unified': unified.radialUndistort,
    'rational': rational.radialUndistort,
    'poly': poly.radialUndistort,

# Path: calibration/StereographicCalibration.py
# def radialDistort(rh, k, quot=False, der=False):
# def radialUndistort(rd, k, quot=False, der=False):
# 
# Path: calibration/UnifiedCalibration.py
# def formatParameters(rVec, tVec, linearCoeffs, distCoeffs):
# def retrieveParameters(params):
# def radialDistort(rp, k, quot=False):
# def radialUndistort(rp, k, quot=False):
# 
# Path: calibration/RationalCalibration.py
# def formatParameters(rVec, tVec, linearCoeffs, distCoeffs):
# def setParametersVary(params, flag):
# def retrieveParameters(params):
# def radialDistort(rp, k, quot=False, der=False):
# def radialUndistort(rpp, k, quot=False, der=False):
# def inverseRationalOwn(corners, rotMatrix, tVec, cameraMatrix, distCoeffs):
# def directRationalOwn(objectPoints, rotMatrix, tVec, cameraMatrix, distCoeffs,
#                    plot=False, img=None, corners=None):
# def calibrateIntrinsic(objpoints, imgpoints, imgSize, K, D,
#                        flags, criteria):
#     X = -(c*e - f*b)/q # check why wrong sign, why must put '-' in front?
#     Y = -(f*a - c*d)/q
#     XYZ = np.array([X, Y, np.zeros(shape[1])]).T
#     XYZ = np.reshape(XYZ, shape)
#         D = zeros((1, 8))
# 
# Path: calibration/FisheyeCalibration.py
# def radialDistort(rp, k, quot=False, der=False):
# def radialUndistort(rpp, k, quot=False, der=False):
# def calibrateIntrinsic(objpoints, imgpoints, imgSize, K, D,
#                        flags, criteria):
#         D = zeros((4))
# 
# Path: calibration/PolyCalibration.py
# def formatParameters(rVec, tVec, linearCoeffs, distCoeffs):
# def setParametersVary(params, flag):
# def retrieveParameters(params):
# def radialDistort(rp, k, quot=False, der=False):
# def radialUndistort(rpp, k, quot=False, der=False):
# def inverseRationalOwn(corners, rotMatrix, tVec, cameraMatrix, distCoeffs):
# def directRationalOwn(objectPoints, rotMatrix, tVec, cameraMatrix, distCoeffs,
#                    plot=False, img=None, corners=None):
# def calibrateIntrinsic(objpoints, imgpoints, imgSize, K, D,
#                        flags, criteria):
#     X = -(c*e - f*b)/q # check why wrong sign, why must put '-' in front?
#     Y = -(f*a - c*d)/q
#     XYZ = array([X, Y, zeros(shape[1])]).T
#     XYZ = reshape(XYZ, shape)
#         D = zeros((1, 5))


# Path: calibration/calibrator.py
from matplotlib.pyplot import plot, imshow, legend, show, figure, gcf, imread
from matplotlib.pyplot import xlabel, ylabel
from cv2 import Rodrigues  # , homogr2pose
from numpy import max, zeros, array, sqrt, roots, diag, sum, log
from numpy import sin, cos, cross, ones, concatenate, flipud, dot, isreal
from numpy import linspace, polyval, eye, linalg, mean, prod, vstack
from numpy import ones_like, zeros_like, pi, float64, transpose
from numpy import any as anny
from numpy.linalg import svd, inv, det
from scipy.linalg import norm
from scipy.special import chdtri
from matplotlib.patches import FancyArrowPatch
from mpl_toolkits.mplot3d import proj3d
from importlib import reload
from calibration import StereographicCalibration as stereographic
from calibration import UnifiedCalibration as unified
from calibration import RationalCalibration as rational
from calibration import FisheyeCalibration as fisheye
from calibration import PolyCalibration as poly

        Cd = zeros((xd.shape[0], 2, 2), dtype=float64)  # create cov matrix
        Jd_i, Jd_f = ccd2disJacobian(xi, yi, cameraMatrix)

        if Cibool:
            Jd_iResh = Jd_i.reshape((-1, 2, 2, 1, 1))
            Cd += (Jd_iResh *
                   Ci.reshape((-1, 1, 2, 2, 1)) *
                   Jd_iResh.transpose((0, 4, 3, 2, 1))
                   ).sum((2, 3))

        if Cfbool:
            # propagate uncertainty via Jacobians
            Jd_fResh = Jd_f.reshape((-1, 2, 4, 1, 1))
            Cd += (Jd_fResh *
                   Cf.reshape((-1, 1, 4, 4, 1)) *
                   Jd_fResh.transpose((0, 4, 3, 2, 1))
                   ).sum((2, 3))
    else:
        Cd = False  # return without covariance matrix
        Jd_f = False

    return xd, yd, Cd, Jd_f


# switcher for radial un-distortion
undistort = {
    'stereographic': stereographic.radialUndistort,
    'unified': unified.radialUndistort,
    'rational': rational.radialUndistort,
    'poly': poly.radialUndistort,

    'fisheye': fisheye.radialUndistort