export_xyz.py

#############################################################################
# UAV Thermal Data Processing Pipeline (GRYFN+TeAx)                         #
#############################################################################
#                                                                           #
# This user guide details the processing procedures for thermal imagery     #
# acquired by the TeAx sensor onboard the GRYFN platform.                   #
# Process all sensor data except thermal using the most recent version      #
# of the GRYFN Processing Tool before proceeding.                           #
#                                                                           #
# Manual by Sungchan Oh (Sun)                                               #
# First Created: August 31, 2023                                            #
# Last Updated: April 30, 2024                                              #
# Contact: oh231@purdue.edu                                                 #
#                                                                           #
#############################################################################


############################### USER INPUT ##################################
# File path to GRYFN Export Trajectory file
path_apx = "/depot/smarterag/fy2024_drone_data/yang1527/Field 58/2024_5_10_F58/GRYFN/2024_5_10_F58.graw/APX-15/flight_1/gryfnBatch_flight1_2024_5_10_F58/2024_5_10_F58/2024_5_10_F58/Export/export_2024_5_10_F58.txt"
# Directory where raw thermal images are located (*.TMC)
dir_tmc = "/depot/smarterag/fy2024_drone_data/yang1527/Field 58/2024_5_10_F58/Raw/Thermal"
#############################################################################



##################### MISCELLANEOUS PARAMETERS ##############################
# Maxtime difference btw thermal time stamp and GPS signal reception (sec)
max_timediff = 2          # 2 or 3 sec recommended
# GPS leap seconds
# https://en.racelogic.support/Knowledge_Base/What_are_GPS_Leap_Seconds%3F
leap_sec = 18             # 18 sec on 2023, chcanges to 19 on June 2024
# Geographic coordinate system
utm_zone_num = 16         # UTM zone number
utm_zone_letter = 'N'     # UTM zone letter
#############################################################################





# Set file path to output csv file
import os
path_out = os.path.join(dir_tmc, "xyz.csv")

# Create subdirectory named "nouse" to separate unnecessary data
dir_nouse = os.path.join(dir_tmc, "nouse")
if not os.path.exists(dir_nouse):
    os.makedirs(dir_nouse)

# Move "00000000.TMC" to nouse data if exists
tmc_zeros = os.path.join(dir_tmc, "00000000.TMC")
tmc_zeros_nouse = os.path.join(dir_nouse, "00000000.TMC")
if os.path.exists(tmc_zeros):
    os.replace(tmc_zeros, tmc_zeros_nouse)

# Get the list of thermal images
print("Checking thermal images...")
import glob, os, time
if dir_tmc.endswith("/"):
    path_tmc = dir_tmc + "*.TMC"
else:
    path_tmc = dir_tmc + "/*.TMC"
list_tmc = glob.glob(path_tmc)
list_tmc.sort()

# Get list of GPRMC (GPS specific info) for each thermal image
print("Loading GPS info from thermal images...")
import sys
def get_tmc_gprmc(f):
    fp = open(f, encoding='latin-1')
    while True:
        nstr = fp.readline()
        if len(nstr)==0:
            break
        if (len(nstr)<70) or (len(nstr)>80):
            continue
        if ('\x01' in nstr) or ('\x00' in nstr):
            continue
        if nstr.startswith("$GPRMC") and ("," in nstr):
            gprmc = nstr.rstrip()
    return gprmc
list_tmc_gprmc = [get_tmc_gprmc(f) for f in list_tmc]

# Get timestamp of thermal images
def get_tmc_time(gprmc):
    words = gprmc.split(',')
    return words[1]
list_tmc_time = [get_tmc_time(g) for g in list_tmc_gprmc]
list_tmc_h = [t[0:(t.find('.')-4)] for t in list_tmc_time]
list_tmc_m = [t[(t.find('.')-4):(t.find('.')-2)] for t in list_tmc_time]
list_tmc_s = [t[(t.find('.')-2):] for t in list_tmc_time]
list_tmc_h = [float(t)*60*60 for t in list_tmc_h]
list_tmc_m = [float(t)*60 for t in list_tmc_m]
list_tmc_s = [float(t) for t in list_tmc_s]
zipped = list(zip(list_tmc_h, list_tmc_m, list_tmc_s))
list_tmc_timestamp = [sum(z)+leap_sec for z in zipped]

# Create dataframe to record thermal timestamp
import pandas as pd
df_tmc = pd.DataFrame({"Path": list_tmc,
                       "Timestamp": list_tmc_timestamp})

# Load GRYFN Export Trajectory file (APX events log)
print("Loading GPS info from GRYFN Export Trajectory file (APX events)...")
df_apx = pd.read_csv(path_apx, delimiter="\t", index_col=False, skiprows=26)

# Filter out unstable measurements in APX log
print("Filtering out noisy data in GRYFN Export Trajectory file...")
cond_E = df_apx["EASTING_SD(m)"]   < 0.2
cond_N = df_apx["NORTHING_SD(m)"]  < 0.2
cond_Z = df_apx["HEIGHT_SD(m)"]    < 0.2
cond_R = df_apx["ROLL_SD(deg)"]    < 1.0
cond_P = df_apx["PITCH_SD(deg)"]   < 1.0
cond_H = df_apx["HEADING_SD(deg)"] < 1.0
cond = cond_E * cond_N * cond_Z * cond_R * cond_P * cond_H
df_apx_ = df_apx[cond]

# Drop columns
cols_drop = ['ANGLERATE_X', 'ANGLERATE_Y', 'ANGLERATE_Z']
df_apx_ = df_apx_.drop(cols_drop, axis=1)

# Rename columns
dict_col = {"GPS_TIME(SoD)":       "Timestamp_APX",
            "EASTING(m)":          "X",
            "NORTHING(m)":         "Y",
            "ELLIPSOID_HEIGHT(m)": "Z",
            "ROLL(deg)":           "Roll",
            "PITCH (deg)":         "Pitch",
            "HEADING(deg)":        "Yaw",
            "EASTING_SD(m)":       "Accuracy_X",
            "NORTHING_SD(m)":      "Accuracy_Y",
            "HEIGHT_SD(m)":        "Accuracy_Vert",
            "ROLL_SD(deg)":        "Accuracy_Roll",
            "PITCH_SD(deg)":       "Accuracy_Pitch",
            "HEADING_SD(deg)":     "Accuracy_Yaw"}
df_apx_ = df_apx_.rename(columns=dict_col, errors="raise")

# Find the closest APX event to each thermal image
print("Matching thermal timestamp with closest GPS signal reception")
print("    from GRYFN Export Trajectory file...")
import numpy as np
def join_tmc_apx(df_tmc, df_apx, max_timediff):
    len_tmc = df_tmc.shape[0]
    Timestamp_ = [np.nan] * len_tmc
    X_ = [np.nan] * len_tmc;
    Y_ = [np.nan] * len_tmc;
    Z_ = [np.nan] * len_tmc
    Roll_  = [np.nan] * len_tmc;
    Pitch_ = [np.nan] * len_tmc;
    Yaw_   = [np.nan] * len_tmc
    X_sd_ = [np.nan] * len_tmc;
    Y_sd_ = [np.nan] * len_tmc;
    Z_sd_ = [np.nan] * len_tmc;
    Roll_sd_  = [np.nan] * len_tmc;
    Pitch_sd_ = [np.nan] * len_tmc;
    Yaw_sd_   = [np.nan] * len_tmc;
    for i, row in df_tmc.iterrows():
        # Find APX event
        diff = row["Timestamp"] - df_apx["Timestamp_APX"]
        diff_min = diff.abs().min()
        j = diff.abs().argmin()
        
        # Uncomment this for quality check
        #diff_sign_change = (np.diff(np.sign(diff))!=0)*1
        #if diff_min > max_timediff or diff_sign_change.sum()==0:
        #    print("nan")
        #    continue
        #elif diff_sign_change.sum()==1:
        #    i_change = diff_sign_change.argmax()
        #    print(i, i_change)
        #    print(diff[i_change], diff[i_change+1])
        #if i>300:
        #    break

        if diff_min < max_timediff:
            # Get APX attributes
            Timestamp_[i] = df_apx["Timestamp_APX"][j]
            X_[i]  = df_apx["X"][j]
            Y_[i]  = df_apx["Y"][j]
            Z_[i]  = df_apx["Z"][j]
            Roll_[i]  = df_apx["Roll"][j]
            Pitch_[i] = df_apx["Pitch"][j]
            Yaw_[i]   = df_apx["Yaw"][j]
            X_sd_[i]  = df_apx["Accuracy_X"][j]
            Y_sd_[i]  = df_apx["Accuracy_Y"][j]
            Z_sd_[i]  = df_apx["Accuracy_Vert"][j]
            Roll_sd_[i]  = df_apx["Accuracy_Roll"][j]
            Pitch_sd_[i] = df_apx["Accuracy_Pitch"][j]
            Yaw_sd_[i]   = df_apx["Accuracy_Yaw"][j]
        else:
            # For images not taken inside the timewindow of the flight,
            # move those files to nouse directory
            bn = os.path.basename(df_tmc.Path[i])
            path_nouse = os.path.join(dir_nouse, bn)
            os.replace(df_tmc.Path[i], path_nouse)
            
    # Create output dataframe
    df_out = df_tmc.copy()
    df_out['Timestamp_APX']  = Timestamp_
    df_out['X']              = X_
    df_out['Y']              = Y_
    df_out['Z']              = Z_
    df_out['Roll']           = Roll_
    df_out['Pitch']          = Pitch_
    df_out['Yaw']            = Yaw_
    df_out['Accuracy_X']     = X_sd_
    df_out['Accuracy_Y']     = Y_sd_
    df_out['Accuracy_Vert']  = Z_sd_
    df_out['Accuracy_Roll']  = Roll_sd_
    df_out['Accuracy_Pitch'] = Pitch_sd_
    df_out['Accuracy_Yaw']   = Yaw_sd_
    df_out = df_out.dropna()  # Drop invalid results
    df_out = df_out.rename(columns={"Path": "Path_TMC"})
    df_out = df_out.rename(columns={"Timestamp": "Timestamp_TMC"})
    return df_out
df_out = join_tmc_apx(df_tmc, df_apx_, max_timediff)

# Convert easting, northing to latitude, longitude
from pyproj import Proj
from pyproj import Transformer
#from pyproj import transform
def utm2latlon_df(df, zone_number, zone_letter):
    utm_proj = Proj(proj='utm',
                    zone=zone_number,
                    ellps='WGS84',
                    south=(zone_letter < 'N'))
    lonlat_proj = Proj(proj='latlong', datum='WGS84')
    transformer = Transformer.from_proj(utm_proj, lonlat_proj)
    lon, lat = transformer.transform(df['X'].values,df['Y'].values)
    # Deprecated
    #lon, lat = transform(utm_proj, lonlat_proj, df['X'].values, df['Y'].values)
    return pd.DataFrame({'Lon': lon, 'Lat': lat})
            
df_latlon = utm2latlon_df(df_out, utm_zone_num, utm_zone_letter)
df_out = pd.concat([df_out, df_latlon], axis=1)

# Calculate time difference
df_out["t_diff"] = df_out['Timestamp_TMC'] - df_out['Timestamp_APX']

# Calculate XY error
XY_sd_ = df_out[['Accuracy_X','Accuracy_Y']].max(axis=1)
df_out["Accuracy_Horz"] = XY_sd_

# Calculate rmse of time difference
import math
n = df_out.shape[0]
rmse = math.sqrt(sum([d*d for d in df_out["t_diff"]])/n)
print("Average time difference btw thermal timestamp and UAV GPS (APX event): {:.4f} sec.".format(rmse))

# Reorder columns
col_export = ['X', 'Y', 'Z']
df_out = df_out[col_export]

# Export dataframe as a csv file
print("Exporting csv file...")
df_out.to_csv(path_out, index=False)





# EOF