transform.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
#######################################
#### LSPyIV --TRANSFORMATION MODULE
#######################################
Python LSPIV tools

Contains all definitions/functions that deals with 
transformations between coordinate systems (image/world)

"""
import numpy as np
import os
from tkinter import Tk
from tkinter.filedialog import askopenfilename

class GRP:
    """Groud Reference Points class"""
    def __init__(self):
        # World coordinates
        self.X     = -99.99 
        self.Y     = -99.99
        self.Z     = -99.99
        # Image coordinates
        self.i     = -99
        self.j     = -99
        
class imgRef:
    """Image References for transformations"""
    def __init__(self):
        self.xyMin  = (-99.99,-99.99)
        self.xyMax  = (-99.99,-99.99)
        self.res    = -99.99
        self.ninj   = (-99,-99)     # Size of ORIGINAL image
        
class camera:
    """Camera class. contains many informations that could also be used for other purposes"""
    def __init__(self):
        self.position       = []    # usually 3-D vector (numpy array)
        self.angles         = []    # usually 3D vector (numpy array)
        self.intrinsic      = {}    # dict containing intrinsic parameters 
        self.scale          = -99   # used for orthophotographic camera (equals to zoom factor)
        self.fps            = -99 
        self.resolution     = -99   # resolution of the camera (usually in m/pix or pix/m)
        self.resolutionInfo = ""    # enter here the unit of resolution to avoid confusions
        self.imagesize      = ()    # size of image (tuple)
        self.a              = []    # a matrix from DLT, used to convert world to image space 
        self.a_inv          = []    # a_inv matrix, used to convert from image to world coordinates
        self.xy0            = ()    # Position of lowest image corner in world unit
        
    def read_a(self,filename = ""):
        """
        Reads the file containing the a matrix for transformation between space and image coodinate s

        Parameters
        ----------
            filename : [str], optional
                Path to the file 'coeff.dat'. If "" then a GUI will be opened to select file
                The default is "".

        Returns
        -------
            - errno [int]
                Error number. Possible values : 
                    - 0 - no errors 
                    - 1 - wrong format contains file coeff.dat

        """
        # ---- Initialisation 
        isExist = os.path.exists(filename) # Check if file exists 
        
        if (filename == "") or (isExist == False):
            # Open GUI to find file - tkinker lib is used 
            Tk().withdraw()     
            filename = askopenfilename(title = "Select coeff.dat", filetypes = (("coeff file","coeff.dat"),("all files","*.*"))) # Get file path
        
        if (filename[-4:] != ".dat" ):
            print("ERROR : file format doesn't match\n")
            return 1
        
        # ---- Read coeff.dat 
        fCoeff = open(filename,"r")
        lstExtract = fCoeff.read()
        lstExtract = lstExtract.strip()
        lstExtract = lstExtract.split("\n")
        fCoeff.close()
        
        if(int(lstExtract[0]) == 11):
            #-------------------
            self.a = np.zeros(11,dtype = float)
            # from verif_ortho.f90
            self.a[0] = np.float(lstExtract[1])
            self.a[1] = np.float(lstExtract[2])
            self.a[2] = np.float(lstExtract[3])
            self.a[3] = np.float(lstExtract[4])
            self.a[4] = np.float(lstExtract[8])
            self.a[5] = np.float(lstExtract[9])
            self.a[6] = np.float(lstExtract[10])
            self.a[7] = np.float(lstExtract[11])
            self.a[8] = np.float(lstExtract[5])
            self.a[9] = np.float(lstExtract[6])
            self.a[10] = np.float(lstExtract[7]) 
            
        else: 
            print("ERROR: wrong format from file 'coeff.dat'\n")
            return 1 
        
        return 0
        
    def read_a_inv(self,filename = ""):
        """
        Reads the file containing the a_inv matrix for transformation between space and image coodinate s

        Parameters
        ----------
            filename : [str], optional
                Path to the file 'coeff.dat'. If "" then a GUI will be opened to select file
                The default is "".

        Returns
        -------
            - errno [int]
                Error number. Possible values : 
                    - 0 - no errors 
                    - 1 - wrong format contains file coeff.dat

        """
        # ---- Initialisation 
        isExist = os.path.exists(filename) # Check if file exists 
        
        if (filename == "") or (isExist == False):
            # Open GUI to find file - tkinker lib is used 
            Tk().withdraw()     
            filename = askopenfilename(title = "Select coeff_inv.dat", filetypes = (("coeff_inv file","coeff_inv.dat"),("all files","*.*"))) # Get file path
        
        if (filename[-4:] != ".dat" ):
            print("ERROR : file format doesn't match\n")
            return 1
        
        # ---- Read coeff.dat 
        fVerif = open(filename,"r")
        # Burn first line (header)
        ligne = fVerif.readline()
        # Initialize arrays
        ligne = fVerif.readline()
        extract = ligne[:-1].split(' ')
        fVerif.close()
        # Arrange extracted data
        extract = [nb for nb in extract if nb!='']
        self.a_inv = np.array(extract,dtype = np.float64)
        
        return 0
 
    
    def image2space(self,imgIndx,xy0 = True,ni = True):
        """
        Converts image indexes to world units with information of the camera and xy0. 
        Assume that the input image is already ortho-rectified (i.e. image can be assumed to be a plan XY)
    
        Parameters
        ----------
            - imgIndx : [npArrayInt]
                Indexes of the images to be transformed. --> shape : [(ind),(i,j)]
                WARNING : image coordinate are expected. 
                Values from Fudaa-LSPIV are usually given in a mathematical coordinate space with (0,0) located down left
                So the i index need to be "invert" --> correctiIndex = imageSize(i) - iIndexFudaa
                
            - camera : [lspiv.transform.camera()]
                Contains the information of the camera 
            - xyz0 : [npArrayFloat]
                Offset (in meters) apply on the values X, Y for transformation
    
        Returns
        -------
            - SpaceCoord [npArrayFloat]
                X,Y values in meters
            - errno [int]
                In case of an error this function returns an integer 
                    - 1 - Information missing on the camera : no resolution and no scaling
                    - 2 - Image isn't squarred, problem with scaling...
                    - 3 - ResolutionInfo of the camera isn't recognize
        """
        if self.resolutionInfo == "":
        # Case no resolution is entered, use simple scaling (typical blender case with othographic camera)
            if self.scale == -99:
                print("ERROR : Information missing on the camera : no resolution and no scaling")
                return 1
        # Image size is assumed to be squared 
            if self.imagesize[0]!=self.imagesize[1]:
                print("ERROR : Image isn't squarred, problem with scaling...")
                return 1
                res = self.imagesize[0] / self.scale 
        elif self.resolutionInfo == "m/pix":
        # Case resolution is given in meter/pixels (typical units from Fudaa-LSPIV project) 
            res = 1/self.resolution 
        elif self.resolutionInfo == "pix/m":
        # Case resolution is given in pixels/meter  
            res = self.resolution 
        else: 
            print("ERROR : resolution information of the camera isn't recognize. Expected : 'm/pix' or 'pix/m'")
            return 3
    
        # If no xy0 are passed to the function they're calculated from the camera data
        if isinstance(xy0,bool) and self.scale != -99:
            xy0 = (self.position - (0.5*self.scale))[:2]
           
        if len(imgIndx)==0:
            print("WARNING : imgIndx is empty...")
        # Swap column to get (j,i) as j is x and i is y 
        if ni == True:
            ni = np.max(imgIndx[:,0])
        JIimgIndx = np.array([imgIndx[:,1],ni-imgIndx[:,0]]).T
        out = (JIimgIndx/res) + xy0 
        
        return out
   

    def velSpace2dispImage(self,spaceVel,xy0 = np.nan):
        """
        Converts image indexes to world units with information of the camera and xy0. 
        Assume that the input image is already ortho-rectified (i.e. image can be assumed to be a plan XY)
    
        Parameters
        ----------
            - imgIndx : [npArrayInt]
                Indexes of the images to be transformed. --> shape : [(ind),(i,j)]
                WARNING : image coordinate are expected. 
                Values from Fudaa-LSPIV are usually given in a mathematical coordinate space with (0,0) located down left
                So the i index need to be "invert" --> correctiIndex = imageSize(i) - iIndexFudaa
                
            - camera : [lspiv.transform.camera()]
                Contains the information of the camera 
            - xyz0 : [npArrayFloat]
                Offset (in meters) apply on the values X, Y for transformation
    
        Returns
        -------
            - SpaceCoord [npArrayFloat]
                X,Y values in meters
            - errno [int]
                In case of an error this function returns an integer 
                    - 1 - Information missing on the camera : no resolution and no scaling
                    - 2 - Image isn't squarred, problem with scaling...
                    - 3 - ResolutionInfo of the camera isn't recognize
        """
        # if isinstance(camera,transform.camera):
        #     print("\nERROR : wrong format of camera passed to the function. Expected : lspiv.transform.camera ")
        #     return 1
        if self.resolutionInfo == "":
            # Case no resolution is entered, use simple scaling (typical blender case with othographic camera)
            if self.scale == -99:
                print("\nERROR : Information missing on the camera : no resolution and no scaling")
                return 1
            # Image size is assumed to be squared 
            if self.imagesize[0]!=self.imagesize[1]:
                print("\nERROR : Image isn't squarred, problem with scaling...")
                return 1
            res      = self.imagesize[0] / self.scale 
        elif self.resolutionInfo == "m/pix":
            # Case resolution is given in meter/pixels (typical units from Fudaa-LSPIV project) 
            res      = 1/self.resolution 
        elif self.resolutionInfo == "pix/m":
            # Case resolution is given in pixels/meter  
            res      = self.resolution 
        else: 
            print("\nERROR : resolution information of the camera isn't recognize. Expected : 'm/pix' or 'pix/m'")
            return 3
        
        # If no xy0 are passed to the function they're calculated from the camera data
        if np.isnan(xy0):
            xy0      = (self.position - (0.5*self.scale))[:2]
           
        if len(spaceVel)==0:
            print("\nWARNING : Values are empty...")
        
        # Compute displacement based on camera info 
        XYdisp       = spaceVel / self.fps
        JIspaceV     = (XYdisp)*res  
        # Swap column to get (i,j) as x is j and y is i     
        IJindex      = np.array([JIspaceV[:,1],JIspaceV[:,0]]).T
        # Invert i ax
        IJindex[:,0] = -IJindex[:,0]
        
        return IJindex
   

        
###################
#### FUNCTIONS 
###################

def read_a(filename = ""):
        """
        Reads the file containing the a matrix for transformation between space and image coodinate s

        Parameters
        ----------
            filename : [str], optional
                Path to the file 'coeff.dat'. If "" then a GUI will be opened to select file
                The default is "".

        Returns
        -------
            - errno [int]
                Error number. Possible values : 
                    - 0 - no errors 
                    - 1 - wrong format contains file coeff.dat

        """
        # ---- Initialisation 
        isExist = os.path.exists(filename) # Check if file exists 
        
        if (filename == "") or (isExist == False):
            # Open GUI to find file - tkinker lib is used 
            Tk().withdraw()     
            filename = askopenfilename(title = "Select coeff.dat", filetypes = (("coeff file","coeff.dat"),("all files","*.*"))) # Get file path
        
        if (filename[-4:] != ".dat" ):
            print("ERROR : file format doesn't match\n")
            return 1
        
        # ---- Read coeff.dat 
        fCoeff = open(filename,"r")
        lstExtract = fCoeff.read()
        lstExtract = lstExtract.strip()
        lstExtract = lstExtract.split("\n")
        fCoeff.close()
        
        if(int(lstExtract[0]) == 11):
            #-------------------
            a = np.zeros(11,dtype = float)
            # from verif_ortho.f90
            a[0] = np.float(lstExtract[1])
            a[1] = np.float(lstExtract[2])
            a[2] = np.float(lstExtract[3])
            a[3] = np.float(lstExtract[4])
            a[4] = np.float(lstExtract[8])
            a[5] = np.float(lstExtract[9])
            a[6] = np.float(lstExtract[10])
            a[7] = np.float(lstExtract[11])
            a[8] = np.float(lstExtract[5])
            a[9] = np.float(lstExtract[6])
            a[10] = np.float(lstExtract[7]) 
            
        else: 
            print("ERROR: wrong format from file 'coeff.dat'\n")
            return 1 
        
        return a

def read_a_inv(filename = ""):
        """
        Reads the file containing the a_inv matrix for transformation between space and image coodinate s

        Parameters
        ----------
            filename : [str], optional
                Path to the file 'coeff.dat'. If "" then a GUI will be opened to select file
                The default is "".

        Returns
        -------
            - errno [int]
                Error number. Possible values : 
                    - 0 - no errors 
                    - 1 - wrong format contains file coeff.dat

        """
        # ---- Initialisation 
        isExist = os.path.exists(filename) # Check if file exists 
        
        if (filename == "") or (isExist == False):
            # Open GUI to find file - tkinker lib is used 
            Tk().withdraw()     
            filename = askopenfilename(title = "Select coeff_inv.dat", filetypes = (("coeff_inv file","coeff_inv.dat"),("all files","*.*"))) # Get file path
        
        if (filename[-4:] != ".dat" ):
            print("ERROR : file format doesn't match\n")
            return 1
        
        # ---- Read coeff.dat 
        fVerif = open(filename,"r")
        # Burn first line (header)
        ligne = fVerif.readline()
        # Initialize arrays
        ligne = fVerif.readline()
        extract = ligne[:-1].split(' ')
        fVerif.close()
        # Arrange extracted data
        extract = [nb for nb in extract if nb!='']
        a_inv = np.array(extract,dtype = np.float64)
        
        return a_inv
    
# Space coordinates to image coordinate (from Fudaa-LSPIV source)
def space2CRT(a,x,y,h):
    xp=((a[0]*x)+(a[1]*y)+(a[2]*h+a[3])) / ((a[8]*x)+(a[9]*y)+(a[10]*h+1))
    yp=((a[4]*x)+(a[5]*y)+((a[6]*h)+a[7])) / ((a[8]*x)+(a[9]*y)+(a[10]*h+1))
 
    return (yp,xp)        
 
# Image to space coordinate 
def CRT2space(a_inv,i,j):
    x = (a_inv[0]*j+a_inv[1]*i+a_inv[2]) / (a_inv[3]*j+a_inv[4]*i+a_inv[5])
    y = (a_inv[6]*j+a_inv[7]*i+a_inv[8]) / (a_inv[3]*j+a_inv[4]*i+a_inv[5])

    return (x,y)  

def CRT2space2D(a_inv,i,j):
    x = (a_inv[0]*j+a_inv[1]*i+a_inv[2]) / (a_inv[6]*j+a_inv[7]*i+1)
    y = (a_inv[3]*j+a_inv[4]*i+a_inv[5]) / (a_inv[6]*j+a_inv[7]*i+1)

    return (x,y)  
        
def read_grid(filename = ""):
    """
        Reads grid.dat file and arrange it following the indexing order.

        Parameters
        ----------
            filename : [str], optional
                Path to the file 'coeff.dat'. If "" then a GUI will be opened to select file
                The default is "".
            
            indexing : [str], optional
                Defines the order used to store data. 
                    - "ij" : traditionnal ij indexing (used for images)
                    - "raw": Fudaa-LSPIV indexing (raw values in grid.dat)

        Returns
        -------
            - errno [int]
                Error number. Possible values : 
                    - 0 - no errors 
                    - 1 - wrong format contains file coeff.dat

        """
    # ---- Initialisation 
    isExist = os.path.exists(filename) # Check if file exists 
    
    if (filename == "") or (isExist == False):
        # Open GUI to find file - tkinker lib is used 
        Tk().withdraw()     
        filename = askopenfilename(title = "Select grid.dat", filetypes = (("grid file","grid.dat"),("all files","*.*"))) # Get file path
    
    if (filename[-4:] != ".dat" ):
        print("ERROR : file format doesn't match\n")
        return 1
    
    # ---- Read coeff.dat 
    fGrid = open(filename,"r")
    lstExtract = fGrid.read()
    lstExtract = lstExtract.strip()
    lstExtract = lstExtract.split("\n")
    fGrid.close()
    
    i,j = [],[]
    # Extract fi,fj (fudaa i and fudaa j)
    for el in lstExtract:
        fj,fi = el.split()
        i.append(int(fi))
        j.append(int(fj))
    

    return i,j
     
def bicubic_at(ip,jp,im):
    
        i = int(ip)
        j = int(jp)
     
        sum_fx = 0
        for k in range(4):
           for l in range(4):
              sx = abs(j+k-2-jp)
              sy = abs(i+l-2-ip)
              if ((sx >= 0) and (sx < 1)):
                 #Cx = un - deux*(sx*sx) + (sx*sx*sx)
                 Cx = 1. + (sx-2.)*sx*sx
              elif ((sx >= 1) and (sx < 2)):
                 #Cx = four - huit*sx + cinq*(sx*sx) - (sx*sx*sx)
                 Cx = 4. + ((5. - sx)*sx - 8.)*sx
              elif (sx > 2):
                 Cx = 0.
              
     
              if ((sy >= 0) and (sy < 1)):
                 #Cy = un - deux*(sy*sy) + (sy*sy*sy)
                 Cy = 1 + (sy-2)*sy*sy
              elif ((sy >= 1) and (sy < 2)):
                 #!Cy = four - huit*sy + cinq*(sy*sy) - (sy*sy*sy)
                 Cy = 4 + ((5. -sy)*sy - 8.)*sy
              elif (sy > 2):
                 Cy = 0.
              
     
              fx = im[i+l-1,j+k-1]*Cx*Cy
              sum_fx = sum_fx+fx


        if (int(sum_fx) > 255):
            sum_fx = 255
        if (int(sum_fx) < 0):
            sum_fx = 0
            
        return int(sum_fx)
   
def readGRPtable(filename):
    
    # Open GRP file 
    try:
        f       = open(filename,"r")
        buff    = f.read()
        f.close()
    except Exception as e:
        print("ERROR reading file coeff.dat : " + str(e))
        return 1
    
    # Get number of GRP        
    i0 = buff.find("\n")
    i1 = i0 + buff[i0+1:].find("\n")
    nbGRP = int(buff[i0+1:i1+1])
    # Read file 
    if0 = buff.find("j")
    tmp     = np.array(buff[if0+2:].split(),float)
    # Re-arrange data
    lstGRP  = tmp.reshape((nbGRP,5))
    
    return lstGRP

def writeGRPtable(GRP,filename,ni,nj):
    
    buff = "GRP V2.0 {} {}\n".format(int(ni),int(nj))
    buff += "{}\nX\tY\tZ\ti\tj\n".format(int(np.shape(GRP)[0]))
    
    for el in GRP:
        buff += "%.3f\t%.3f\t%.3f\t%d\t%d\n"%(el[0],el[1],el[2],int(el[3]),int(el[4]))
    # Open GRP file 
    try:
        f       = open(filename,"w")
        f.write(buff)
        f.close()
    except Exception as e:
        print("ERROR reading file coeff.dat : " + str(e))
        return 1
    
    return 0