initial commit

utils/functions.py

+import plotly.express as px
+import pandas as pd
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.interpolate import CubicSpline
+
+'''
+This file contains the functions required for running the shape sensing notebooks. 
+To use these functions, ensure that your virtual environment has the requirement.txt file installed:
+$ pip3 install -r requirements.txt
+
+List of the functions:
+
+    read_strain(file) - Read CSV microstrain data file and return a python array 
+                      -> return eps
+    
+    reject_outliers(data, dx, m=1)  -
+
+    smoothen_strain(cores: np.ndarray, kernel_size: int = 5) -
+    
+    get_ss_variables (spl_eps, ds, r_in) - Compute the variable theta, K, phi, R and tau using all core strain measurements at a 
+                                           given time step
+                                         -> return theta, K, phi, R, tau
+    
+    Frenet_Serret(ds, K, tau, T_0, N_0, r_0) - Return the 3D curve derived from the K and tau variable arrays at a given time 
+                                               step
+                                             ->  return r
+                                               
+    Matrix_Transform(theta, R, phi) - Return the 3D curve derived from the phi, R and theta variable arrays at a given time step
+                                    > return P
+                                    
+    
+
+List of variables
+file - input csv file containing straining measurements 
+length - 
+eps -
+ds -
+spl_eps -
+ds -
+r_in -
+K -
+tau -
+phi -
+theta -
+R -
+r0 -
+r - 
+T_0 -
+T -
+N_0 -
+N -
+B_0 -
+B -
+
+'''
+
+
+def read_strain(file: str) -> np.ndarray:
+    # Read CSV file using pandas and specify tab as the separator
+    df = pd.read_csv(file, sep="\t")
+
+    # Interpolate NaN values in the data
+    for row in df:
+        df[row] = pd.to_numeric(df[row], errors='coerce')
+    df.index = pd.DatetimeIndex(df.index)
+    df_int = df.interpolate(method='time', axis=0)
+
+    # Convert pandas dataframe to numpy array with dtype float64
+    eps = df_int.to_numpy(dtype=np.float64)
+    eps = np.nan_to_num(eps, nan=1e15, posinf=1e15, neginf=1e-15)
+    
+    #check that NaNs have been successfully removed
+    if np.isnan(eps).any() == True:
+        print("NaNs still present in the data")
+    
+    # Return the numpy array with interpolated values
+    return eps
+
+
+def reject_outliers(data, dx, m=1):
+    # Repeat until no more outliers are found
+    restart = True
+    while restart:  
+        restart = True
+        break_flag = False
+
+       
+        for i in range(data.shape[0]):  
+            
+            # Calculate gradient of each row
+            grad = np.gradient(data[i, :], dx)
+
+            for j in range(data.shape[1] - 1): 
+               
+                # Check if gradient is too high
+                if abs(grad[j]) > 1e6*m:  
+                    
+                    # Replace the value with the previous one in the same row
+                    data[i, j + 1] = data[i, j]
+
+                    break_flag = True
+                    break
+                    
+                # If it reaches the last column of the row
+                if j == data.shape[1] - 2: 
+                    if i == data.shape[0] - 1:  # and it's the last row
+                        restart = False  # stop the loop
+                        continue
+                        exit()  
+
+                    else:  # if it's not the last row
+                        restart = True  # continue the loop
+                        break_flag = False
+
+                if break_flag:  # if an outlier is found
+                    break
+            if break_flag:  # if an outlier is found
+                break
+                
+    # Return the cleaned data
+    return data  
+
+
+def smoothen_strain(cores: np.ndarray, kernel_size: int = 5) -> tuple:
+    # Get dimensions of input array
+    nt = cores.shape[1] # number of time steps
+    ns = cores.shape[2] # number of sensor readings
+
+    # Create output arrays
+    sc1 = np.zeros((nt, ns))
+    sc2 = np.zeros((nt, ns))
+    sc3 = np.zeros((nt, ns))
+
+    # Create a 1D smoothing kernel
+    kernel = np.ones(kernel_size) / kernel_size
+
+    # Apply convolution to each time step for each sensor reading
+    for t in range(nt):
+        sc1[t,:] = np.convolve(cores[0,t,:], kernel, mode='same')
+        sc2[t,:] = np.convolve(cores[1,t,:], kernel, mode='same')
+        sc3[t,:] = np.convolve(cores[2,t,:], kernel, mode='same')
+
+    # Return the smoothed arrays as a tuple
+    return sc1, sc2, sc3
+
+
+def get_ss_var(spl_eps, ds, r_in):
+    n_cores = spl_eps.shape[0]   # number of cores in splines
+    n_measurements = spl_eps.shape[1]   # number of measurements
+
+    # Initialize arrays
+    theta = np.zeros([n_measurements])
+    K = np.zeros([n_measurements])
+
+    # Angular distance of cores from horizontal axis (in radians)
+    theta_cores = np.zeros((n_cores))
+
+    # Compute the angular position of each core
+    for i in range(n_cores):
+        theta_cores[i] = ((2 * np.pi * i) / n_cores)
+    
+    # Compute the angle increment for each measurement and add it to the angular position of each core 
+    # This effect is due to core twisting around JIP-CALM sensor
+    angle_increment = (np.deg2rad(360) / n_measurements)
+
+    for i in range(n_measurements):
+        theta_cores += angle_increment
+        
+        # Compute the core location matrix M
+        M = np.zeros([3, 3])
+        for j in range(n_cores):
+            M[j, :] = np.array([-r_in * np.cos(theta_cores[j]), r_in * np.sin(theta_cores[j]), 1])
+
+        # Compute the pseudo-inverse of M
+        cross = np.linalg.pinv(M)
+
+        # Compute the dot product of the location matrix and the strain
+        alpha = np.dot(cross, spl_eps[:, i])
+
+        # Compute the angle of bending theta
+        theta[i] = np.arctan(alpha[1] / alpha[0])
+      
+        # Compute the curvature K
+        K[i] = np.sqrt(alpha[0] ** 2 + alpha[1] ** 2)
+     
+    # Compute the direction of bending tau
+   # theta = np.unwrap(theta,  period=np.pi) 
+    tau = np.gradient(np.abs(theta), ds)
+   # tau = np.gradient(np.abs(theta), ds)
+  #  tau = np.gradient(np.unwrap(theta, period=np.pi), ds)
+
+    # Compute the radius of curvature R
+    R = 1 / K
+
+    # Compute the twist angle phi
+    phi = ds / R
+
+    # Return variable arrays
+    return theta, K, phi, R, tau
+
+
+def Frenet_Serret(ds: float, K: np.ndarray, tau: np.ndarray, T_0: np.ndarray, N_0: np.ndarray, r_0: np.ndarray) -> np.ndarray:
+    n = len(K) # number of measurements
+
+    # initial conditions
+    B_0 = np.cross(T_0, N_0)
+
+    # Initialize arrays for storing vectors at n+1 points along the curve
+    T = np.zeros([n + 1, 3])
+    N = np.zeros([n + 1, 3])
+    B = np.zeros([n + 1, 3])
+    r = np.zeros([n + 1, 3])
+
+    # Set initial values
+    T[0, :] = T_0
+    N[0, :] = N_0
+    B[0, :] = B_0
+    r[0, :] = r_0
+
+    # Solve Frenet-Serret equations
+    for i in range(n):
+        # Get curvature and torsion at i-th point
+        k = K[i]
+        t = tau[i]
+
+        # Update normal vector using Frenet-Serret equations
+        N[i + 1, :] = (ds * t * B[i, :] - ds * k * T[i, :] + N[i, :]) / (1 + ds * ds * t * t + ds * ds * k * k)
+
+        # Update tangent and binormal vectors using Frenet-Serret equations
+        T[i + 1, :] = ds * k * N[i + 1, :] + T[i, :]
+        B[i + 1, :] = -ds * t * N[i + 1, :] + B[i, :]
+
+        # Update position vector using Frenet-Serret equations
+        r[i + 1, :] = ds * T[i + 1, :] + r[i, :]
+        
+    # Return 3D positions of points along the curve
+    return r
+
+
+def Matrix_Transform(theta, R, phi, ds):
+    n = len(R) # number of measurements
+
+    ### reconstruct shape using homogeneous transformation matrices
+    P = np.zeros([n, 4])
+    A = np.zeros([n, 4, 4])
+    Acum = np.zeros([n, 4, 4])
+    P_0 = [0, 0, 0, 1]
+
+    for i in range(n):
+        t = theta[i]*ds
+        p = phi[i]
+        r = R[i]
+
+        ct = np.cos(t)
+        st = np.sin(t)
+        cp = np.cos(p)
+        sp = np.sin(p)
+
+        A_1 = [ct * cp, -st, ct * sp, r * (1 - cp) * ct]
+        A_2 = [st * cp, ct, st * sp, r * (1 - cp) * st]
+        A_3 = [-sp, 0, cp, r * sp]
+        A_4 = [0, 0, 0, 1]
+        A[i, :, :] = [A_1, A_2, A_3, A_4]
+
+        if i == 0:
+            Acum[i, :, :] = A[i, :, :]
+        else:
+            Acum[i, :, :] = np.matmul(Acum[i - 1, :, :], A[i, :, :])
+
+        P[i, :] = np.matmul(Acum[i, :, :], P_0)
+    return P
+
+
+def path_info(P, name):
+    P_length = 0
+    for i in range(P.shape[0] - 1):
+        P_length += np.linalg.norm(P[i + 1, :] - P[i, :])
+    print("Shape of", name, P.shape, "with physical length", P_length)
+    return
+

utils/plotting.py

+import numpy as np
+import matplotlib
+from matplotlib import pyplot as plt
+from matplotlib.animation import FuncAnimation
+
+
+def ss_var_aniplot(theta, K, phi, R, tau, interval=500):
+    
+    #Initialise a figure with five subplots
+    fig, (ax1, ax2, ax3, ax4, ax5) = plt.subplots(5,1)
+    fig.subplots_adjust(hspace=0.8)
+    
+    nt, ns = K.shape[0], K.shape[1]
+    xs = np.linspace(0, ns, ns)
+
+
+    #Initialise line objects with independent axes
+    line1, = ax1.plot([], [], label='theta')
+    line2, = ax2.plot([], [], label='K')
+    line3, = ax3.plot([], [], label='phi')
+    line4, = ax4.plot([], [], label='R')
+    line5, = ax5.plot([], [], label='tau')
+
+    line = [line1, line2, line3, line4, line5]
+    
+    ax1.set(ylim=(np.nanmin(theta),np.nanmax(theta)))
+    ax2.set(ylim=(np.nanmin(K),np.nanmax(K)))
+    ax3.set(ylim=(np.nanmin(phi),np.nanmax(phi)))
+    ax4.set(ylim=(np.nanmin(R[-1,100:700]),np.nanmax(R[-1,100:700])))
+    ax5.set(ylim=(np.nanmin(tau),np.nanmax(tau)))
+    
+    for ax in [ax1, ax2, ax3, ax4, ax5]:
+        ax.legend(loc='upper right')
+        ax.set(xlim=(0,ns))
+        
+    def animate(i):
+        line[0].set_data(xs, theta[i, :])
+        line[1].set_data(xs, K[i, :])
+        line[2].set_data(xs, phi[i, :])
+        line[3].set_data(xs, R[i, :])
+        line[4].set_data(xs, tau[i, :])    
+        ax1.set_title('Time step: ' + str(i))
+        return line
+    anim = FuncAnimation(fig, animate, interval=interval, frames=nt-1, repeat=True, blit=False)
+    return anim
+
+
+def anim_2Dplot(data, ymin=-1000, ymax=1000, interval=200):
+    
+    #Initialise figure and set limits
+    fig, ax = plt.subplots(figsize=(4, 3))
+    ax.set(ylim=(ymin, ymax))
+    
+    #For a single line
+    if data.ndim == 2:
+        #Extract the number of lines to plot, number of time iterations and the number of samples in each lines
+        nt, ns = data.shape[0], data.shape[1]
+        
+        #Initialise 2Dline first iteration
+        xs = np.linspace(0, ns, ns)
+        line = ax.plot(xs, data[0, :], color='k')[0]
+        
+        def animate(i):
+            line.set_ydata(data[i, :])
+            ax.set_title('Time step ' + str(i))
+    
+    #For a set of lines       
+    else:        
+        #Extract the number of lines to plot, number of time iterations and the number of samples in each lines
+        n_channel, nt, ns = len(data), data.shape[1], data.shape[2]
+    
+        #Initialise iterable 2Dline instances
+        xs = np.linspace(0, ns, ns)
+        lines = [ax.plot(xs, data[i, 0, :])[0] for i in range(n_channel)]
+        
+        #Create animation function to cycle through the data
+        def animate(t):
+            ax.set_title('Time step ' + str(t))
+            for i, line in enumerate(lines):
+                line.set_ydata(data[i, t, :])
+            
+    #Create the animation
+    anim = FuncAnimation(fig, animate, interval=interval, frames=nt-1, repeat=True, blit=True)
+    return anim
+
+def xyzdif_aniplots(curve1, curve2, interval=500):
+    
+    #Initialise a figure with five subplots
+    fig, (ax1, ax2, ax3) = plt.subplots(3,1)
+    fig.subplots_adjust(hspace=1.2)
+    
+    nt, ns, ax = curve1.shape[0], curve1.shape[1], curve1.shape[2]
+    xs = np.linspace(0, ns, ns)
+
+    #Initialise line objects with independent axes
+    x_FS, = ax1.plot(xs, xs, label='Frenet-Serret')
+    x_MT, = ax1.plot(xs, xs, label='Transformation Matrix')
+
+    y_FS, = ax2.plot(xs, xs, label='Frenet-Serret')
+    y_MT, = ax2.plot(xs, xs, label='Transformation Matrix')
+    
+    z_FS, = ax3.plot(xs, xs, label='Frenet-Serret')
+    z_MT, = ax3.plot(xs, xs, label='Transformation Matrix')
+    
+    line = [x_FS, x_MT, y_FS, y_MT, z_FS, z_MT]
+    
+    ax1.set(ylim=(0,210))
+    ax2.set(ylim=(-1,2))
+    ax3.set(ylim=(0,2.5))
+    
+    ax1.legend(loc='center left', bbox_to_anchor=(-0.1, 1))
+    ax1.set_title('x-values')
+    ax2.set_title('y-values')
+    ax3.set_title('z-values')
+    ax3.set_yticks((0,2.5))
+   
+    for ax in [ax1, ax2, ax3]:
+        ax.set(xlim=(0,ns))
+        ax.set_xlabel('gauge number')
+        ax.set_ylabel('cm')
+
+        
+    def animate(i):
+        line[0].set_ydata(curve1[i, :, 0])
+        line[1].set_ydata(curve2[i, :, 0])
+
+        line[2].set_ydata(curve1[i, :, 1])
+        line[3].set_ydata(curve2[i, :, 1])
+
+        line[4].set_ydata(curve1[i, :, 2])
+        line[5].set_ydata(curve2[i, :, 2])
+       
+        ax1.set_title('Time step: ' + str(i))
+        return line
+    
+    anim = FuncAnimation(fig, animate, interval=interval, frames=nt-1, repeat=True, blit=False)
+    return anim
+
+
+
+
+################################# TRANSFORM TO 3D ANIMATED PLOTS ##############################
+def plot_curve(P, P1=None, P2=None, TNB=None, name=None, name1=None, name2=None):
+    if TNB == 'TNB':
+        r_0, T, N, B = get_rTNB(P)
+
+        N_0 = N[0, :]
+        T_0 = T[0, :]
+        B_0 = B[0, :]
+
+        Path = pd.DataFrame({
+            "x": P[:, 0],
+            "y": P[:, 1],
+            "z": P[:, 2]})
+
+        Tpd = pd.DataFrame({
+            "x": [r_0[0], T_0[0]],
+            "y": [r_0[1], T_0[1]],
+            "z": [r_0[2], T_0[2]]})
+
+        Npd = pd.DataFrame({
+            "x": [r_0[0], N_0[0]],
+            "y": [r_0[1], N_0[1]],
+            "z": [r_0[2], N_0[2]]})
+
+        Bpd = pd.DataFrame({
+            "x": [r_0[0], B_0[0]],
+            "y": [r_0[1], B_0[1]],
+            "z": [r_0[2], B_0[2]]})
+
+        Path['Path'] = name
+        Tpd['Path'] = 'T'
+        Npd['Path'] = 'N'
+        Bpd['Path'] = 'B'
+
+        df = pd.concat([Tpd, Npd, Bpd, Path])
+
+    else:
+        P = np.abs(P)
+        Path = pd.DataFrame({
+            "x": P[:, 0],
+            "y": P[:, 1],
+            "z": P[:, 2]})
+        Path['Path'] = name
+        df = Path
+
+    if P1 is not None:
+        P1 = np.abs(P1)
+        Path1 = pd.DataFrame({
+            "x": P1[:, 0],
+            "y": P1[:, 1],
+            "z": P1[:, 2]})
+        Path1['Path'] = name1
+        df = pd.concat([df, Path1])
+
+    if P2 is not None:
+        P2 = np.abs(P2)
+        Path2 = pd.DataFrame({
+            "x": P2[:, 0],
+            "y": P2[:, 1],
+            "z": P2[:, 2]})
+        Path2['Path'] = name2
+        df = pd.concat([df, Path2])
+    fig = px.line_3d(df, x="x", y="y", z="z", color="Path")
+    #fig.update_layout(
+    #    scene=dict(
+    #        xaxis=dict(nticks=4, range=[0, 1], ),
+    #        yaxis=dict(nticks=4, range=[0, 1], ),
+    #        zaxis=dict(nticks=4, range=[0, 50], ), ),
+    #    width=700,
+    #    margin=dict(r=20, l=10, b=10, t=10))
+    fig.show()
+
+    return
+
+def dif_xyz(P0, P1, P2, name):
+    n = int(P1.shape[0])
+    Coumpound_dif_1 = np.zeros((n))
+    Coumpound_dif_2 = np.zeros((n))
+
+    for i in np.arange(n - 1):
+        Coumpound_dif_1[i + 1] = Coumpound_dif_1[i] + np.abs(P0[i, 0] - P1[i, 0]) + np.abs(
+            P0[i, 1] - P1[i, 1]) + np.abs(P0[i, 2] - P1[i, 2])
+        Coumpound_dif_2[i + 1] = Coumpound_dif_2[i] + np.abs(P0[i, 0] - P2[i, 0]) + np.abs(
+            P0[i, 1] - P2[i, 1]) + np.abs(P0[i, 2] - P2[i, 2])
+
+    fig, axs = plt.subplots(2, 2)
+    fig.suptitle('Difference from true path')
+    axs[0, 0].plot(P0[:, 0] - P1[:-1, 0])
+    axs[0, 0].plot(P0[:, 0] - P2[:, 0])
+    axs[0, 0].set_title('x-axis')
+    #  axs[0,0].set_ylabel('mm')
+    #  axs[0,0].set_xlabel('measurement #')
+
+    axs[1, 0].plot(P0[:, 1] - P1[:-1, 1])
+    axs[1, 0].plot(P0[:, 0] - P2[:, 0])
+    axs[1, 0].set_title('y-axis')
+    # axs[1,0].set_ylabel('mm')
+    # axs[1,0].set_xlabel('measurement #')
+    #    axs[1,0].set_ylim([-0.25,0.25])
+
+    axs[0, 1].plot(P0[:, 2] - P1[:-1, 2])
+    axs[0, 1].plot(P0[:, 2] - P2[:, 2])
+    axs[0, 1].set_title('z-axis')
+    # axs[0,1].set_ylabel('mm')
+    # axs[0,1].set_xlabel('measurement #')
+    #  axs[0,1].set_ylim([-0.25,0.25])
+
+    axs[1, 1].plot(Coumpound_dif_1[:], label='Frenet_serret')
+    axs[1, 1].plot(Coumpound_dif_2[:], label='Transformation Matrix')
+    axs[1, 1].set_title('Compound')
+    # axs[1,1].set_ylabel('mm')
+    # axs[1,1].set_xlabel('measurement #')
+
+    fig.legend(loc=4)
+    fig.tight_layout()
\ No newline at end of file

utils/unused_functions.py

+def interpolate_strain(length, eps, ds):
+    n_cores = eps.shape[0]  # virtual number of measurements
+    n_measurements = eps.shape[1]
+    spl_eps = np.zeros((n_cores, n_measurements * length))
+
+    ds = ds / length  # adjust the virtual distance between measurements
+
+    # interpolate the strain using univariate spline
+    x = np.linspace(0, n_measurements, n_measurements)
+    xs = np.linspace(0, n_measurements, n_measurements * length)
+    for i in range(n_cores):
+        spl_eps[i, :] = CubicSpline(x, eps[i, :])(xs)
+
+    # plots of interpolation
+    plt.title('core 1 strain interpolation (m/m)')
+    plt.plot(x, eps[0, :], 'ro', ms=5)
+    plt.plot(xs, spl_eps[0, :], 'g', lw=2, alpha=0.9)
+    plt.show()
+    plt.cla()
+
+    plt.title('core 2 strain interpolation (m/m)')
+    plt.plot(x, eps[1, :], 'ro', ms=5)
+    plt.plot(xs, spl_eps[1, :], 'g', lw=2, alpha=0.9)
+    plt.show()
+    plt.cla()
+
+    plt.title('core 3 strain interpolation (m/m)')
+    plt.plot(x, eps[2, :], 'ro', ms=5)
+    plt.plot(xs, spl_eps[2, :], 'g', lw=2, alpha=0.9)
+    plt.show()
+    return spl_eps, ds
+
+def theta_cores(r_in, n_cores):
+    n_cores = 3
+    r_in = 2 * (10 ** -3)
+    theta_cores = np.zeros((n_cores))
+    for i in np.arange(n_cores):
+        theta_cores[i] = ((2 * np.pi) / n_cores) * i
+
+    return theta_cores
+
+def get_rTNB(P):
+    r0 = P[0, :]
+    dP = np.apply_along_axis(np.gradient, axis=0, arr=P)
+    ddP = np.apply_along_axis(np.gradient, axis=0, arr=dP)
+
+    f = lambda m: m / np.linalg.norm(m)  # calculate normalized tangents
+    T = np.apply_along_axis(f, axis=1, arr=dP)
+
+    B = np.cross(dP, ddP)  # calculate normalized binormal
+    B = np.apply_along_axis(f, axis=1, arr=B)
+
+    N = np.cross(B, T)  # calculate normalized normals
+    return r0, T, N, B
+
+
+def align_curves(P1, P2, ds):  # aligning P2 to TNB frame of P1[0] at origin
+    r1, T1, N1, B1 = get_rTNB(P1)
+    r2, T2, N2, B2 = get_rTNB(P2)
+
+    F1 = [T1[0, :], N1[0, :], B1[0, :]]  # extract the TNB at origin
+    F2 = [T2[0, :], N2[0, :], B2[0, :]]
+
+    R = np.dot(np.linalg.inv(F2), F1)  # compute rotation matrix between TNB axese
+
+    r_out = np.dot(P2, R)  # rotate curve P2
+
+    return r_out