Commit 4c6abca6 authored by Nic Arnold's avatar Nic Arnold
Browse files

Updated README

parent 73c4456c
Strongly suggest downloading cloudcompare as a point cloud vis tool
\ No newline at end of file
Strongly suggest downloading cloudcompare.app as a point cloud vis tool.
See README in urg_library-1.2.0 for install.
In ./data-processing/ all files in Archive/ , lidar\_files/ , and telem\_files/ are local and not uploaded or saved to git.
Use MergeAndCorrect.py -h for instructions
\ No newline at end of file
#!/usr/bin/env python # use to merge lidar output and UAV telemetry stream import sys import os import csv import math import numpy as np from bisect import bisect_left USE = "Examples: $ python3 MergeAndCorrect.py kia_flightlog.csv lidar_raw.csv\n$ python3 MergeAndCorrect.py --merge-only kia_flightlog.csv lidar_raw.csv " EXT = ".csv" merge_only=0 #I/O Variables # flightLog.csv for i in range(len(sys.argv)): if str(sys.argv[i]) == "--merge-only": merge_only=1 print("Merging files only") if len(sys.argv) < 2 or str(sys.argv[1]) == "-h": print(" Use to merge UAV telemetry stream and raw LiDAR output.\n Use -merge-only option for only merging files\n", USE) quit() else: f1 = sys.argv[1] # lidarPoints.csv if len(sys.argv) < 3: print(" Input file with lidar points.\n", USE) quit() else: f2 = sys.argv[2] #Global Variables # Initialise distance between points d_i=np.array([0,0,0]) # Initialise time to 0 t_i=0 def takeClosest(telemList, lidarNum): # Assumes telemList is sorted b/c time. Returns closest value to lidarList. # If two numbers are equally close, return the smallest number. pos = bisect_left(telemList, lidarNum) if pos == 0: return pos if pos == len(telemList): return pos-1 before = telemList[pos - 1] after = telemList[pos] if after - lidarNum < lidarNum - before: return pos else: return pos-1 def pointCorrection(t,V_x, V_y, V_z, Th, Ps, ph, Y_l, Z_l, lt): # Lidar points in a plane lp = np.array([0,Y_l,Z_l]) global d_i #print("d_i ",d_i) global t_i # current_distance = lp[0] + V_x * t # past distance = d_i[1] #V_x += V_x * -1 # Calculate current distance by adding velocity*delta time with previous distance X_d = d_i[0] + (V_x*(lt - t_i)) Y_d = d_i[1] + (V_y*(lt - t_i)) Z_d = d_i[2] + (V_z*(lt - t_i)) # distance from one point to another #print("X_d ", X_d) dist = np.array([X_d,Y_d,Z_d]) #print("dist ",dist) #Rotation correction # Rotation matrix that consists of all the formulas for conversion, R[n] where n is the objects position within the arra # ToDo:// doesnt seem to be working # X=ph=yaw, Y=Th=pitch, Z=Ps=roll """ #from wikipedia R = np.array([ [math.cos(Th)*math.cos(ph), -math.cos(ph)*math.sin(Ps)+math.sin(ph)*math.sin(Th)*math.cos(Ps), math.sin(ph)*math.sin(Ps)+math.cos(ph)*math.sin(Th)*math.cos(Ps)], [math.cos(Th)*math.sin(Ps), math.cos(ph)*math.cos(Ps)+math.sin(ph)*math.sin(Th)*math.sin(Ps), -math.sin(ph)*math.cos(Ps)+math.cos(ph)*math.sin(Th)*math.sin(Ps)], [-math.sin(Th), math.sin(ph)*math.cos(Th), math.cos(ph)*math.cos(Th)] ]) """ """ #original R = np.array([ [math.cos(Th)*math.cos(ph), math.cos(Th)*math.sin(ph), -math.sin(Th)], [math.sin(Ps)*math.sin(Th)*math.cos(ph) - math.cos(Ps)*math.sin(ph), math.sin(Ps)*math.sin(Th)*math.sin(ph) + math.cos(Ps)*math.cos(ph), math.cos(Th)*math.sin(Ps)], [math.cos(Ps)*math.sin(Th)*math.cos(ph) + math.sin(Ps)*math.sin(ph), math.cos(Ps)*math.sin(Th)*math.sin(ph) - math.sin(Ps)*math.cos(ph), math.cos(Th)*math.cos(Ps)] ]) """ """ #from website R = np.array([ [math.cos(Th)*math.cos(ph), math.cos(Ps)*math.sin(ph)+math.sin(Ps)*math.sin(Th)*math.cos(ph), math.sin(Ps)*math.sin(ph) - math.cos(Ps)*math.sin(Th)*math.cos(ph)], [-math.cos(Th)*math.sin(ph), math.cos(Ps)*math.cos(ph) - math.sin(Ps)*math.sin(Th)*math.sin(ph), math.sin(Ps)*math.cos(ph) + math.cos(Ps)*math.sin(Th)*math.sin(ph)], [math.sin(Th), -math.sin(Ps)*math.cos(Th), math.cos(Ps)*math.cos(Th)] ]) """ # Kellan R = np.array([ [math.cos(ph)*math.cos(Ps), -math.cos(ph)*math.sin(Ps), math.sin(ph)], [math.sin(Th)*math.sin(ph)*math.cos(Ps) + math.cos(Th)*math.sin(Ps), -math.sin(Th)*math.sin(ph)*math.sin(ph) + math.cos(Th)*math.cos(Ps), -math.sin(Th)*math.cos(ph)], [-math.cos(Th)*math.sin(ph)*math.cos(Ps) + math.sin(Th)*math.sin(Ps), math.cos(Th)*math.sin(ph)*math.sin(Ps)+math.sin(Th)*math.cos(Ps), math.cos(Th)*math.cos(ph)] ]) # The points we want. # Matrix multiplacation for points points = np.array([]) #print(points) points = R.dot(lp) final=[] final.append(points[0] + dist[0]) final.append(points[1] + dist[1]) final.append(points[2] + dist[2]) #final = points + dist # Set current point to previous point d_i = dist t_i = lt return(final) def main(): #open files for read/write with open(f1, 'r') as drone_telem,\ open(f2, 'r') as lidar_points, \ open(str(f2).strip(EXT)+"_merged.csv", 'w') as merged, \ open(str(f2).strip(EXT)+"_corrected.csv", "w") as corrected: # remove headers if present merged_writer = csv.writer(merged) corrected_writer = csv.writer(corrected) reader_telem = csv.reader(drone_telem, delimiter=",") header1 = next(reader_telem, None) reader_lidar = csv.reader(lidar_points, delimiter=",") header2 = next(reader_lidar, None) # Print headers for verification print(header1) print(header2) # initialise indexes telem_list = [] telem_time = [] #merged.write(', '.join(header1) + ', ' + ', '.join(header2) + "\n") merged.write("vX[0], vY[1], vZ[2], telem_time[3], pitch[4], roll[5], yaw[6], z[7], y[8], lidar_time[9]\n") # lati = row[0], longi = row[1], alti = row[2], # vX = row[3], vY = row[4], vZ = row[5], # time = row[6], heading = row[7], height = row[8], # pitch = row[9], roll = row[10], yaw = row[11] for row in reader_telem: timeT = float(str(row[6]).replace(".","")) pitch = math.pi * float(row[9]) / 180 yaw = math.pi * float(row[11]) / 180 roll = math.pi * float(row[10]) / 180 telem = [float(row[3]), float(row[4]), float(row[5]), timeT, pitch, roll, yaw] telem_time.append(timeT) telem_list.append(telem) print("Rows in telem: ",len(telem_list)) # z = row_l[0], y = row_l[1] # urg_time = row_l[2], i = row_l[3] # pc = row_l[4], rot = row_l[5] # sensor = row_l[6], realtime = row_l[7] lidar_rows = 0 corrected_rows = 0 failed_rows =0 for row_l in reader_lidar: lidar_rows += 1 timeL = float(str(row_l[7]).replace(".","")) #- 051100000.0 lidar = [float(row_l[0]), float(row_l[1]), timeL] pointsToMerge = takeClosest(telem_time,timeL) print(telem_time[pointsToMerge], " ", timeL, " ", telem_list[pointsToMerge][3]) #print("timeL: ", timeL) #print("telem_time: ",telem_time[pointsToMerge]) #print("diff: ", telem_time[pointsToMerge]-timeL) if timeL + 10 >= float(telem_time[pointsToMerge]) <= timeL - 10: failed_rows+=1 else: mergedPoints = telem_list[pointsToMerge] + lidar merged_writer.writerow(mergedPoints) #pointCorrection(t,V_x, V_y, V_z, Th, Ps, ph, Y_l, Z_l) # X=ph=yaw, Y=Th=pitch, Z=Ps=roll if merge_only == 0: correctedPoints = pointCorrection(mergedPoints[3],\ mergedPoints[0], mergedPoints[1], mergedPoints[2],\ mergedPoints[4], mergedPoints[6], mergedPoints[5],\ mergedPoints[8], mergedPoints[7], timeL) corrected_writer.writerow(correctedPoints) corrected_rows += 1 print("Rows corrected: ",corrected_rows) print("Lidar rows: ",lidar_rows) print("Failed rows: ", failed_rows) drone_telem.close() lidar_points.close() merged.close() corrected.close() if corrected_rows == 0: print("Files ") print("File Merge Complete. Saved to:", merged.name) if merge_only == 0: print("Point Correction Complete. Saved to:", corrected.name) if __name__ == "__main__": main()
\ No newline at end of file
#!/usr/bin/env python # use to merge lidar output and UAV telemetry stream import sys import os import csv import math import numpy as np from bisect import bisect_left USE = "Examples: $ python3 MergeAndCorrect.py kia_flightlog.csv lidar_raw.csv\n$ python3 MergeAndCorrect.py --merge-only kia_flightlog.csv lidar_raw.csv " EXT = ".csv" merge_only=0 #I/O Variables # flightLog.csv for i in range(len(sys.argv)): if str(sys.argv[i]) == "--merge-only": merge_only=1 print("Merging files only") if len(sys.argv) < 2 or str(sys.argv[1]) == "-h": print(" Use to merge UAV telemetry stream and raw LiDAR output.\n Use -merge-only option for only merging files\n", USE) quit() else: d = sys.argv[1] # lidarPoints.csv if len(sys.argv) < 3: print(" Input file with lidar points.\n", USE) quit() else: f2 = sys.argv[2] #Global Variables # Initialise distance between points d_i=np.array([0,0,0]) # Initialise time to 0 t_i=0 def takeClosest(telemList, lidarNum): # Assumes telemList is sorted b/c time. Returns closest value to lidarList. # If two numbers are equally close, return the smallest number. pos = bisect_left(telemList, lidarNum) if pos == 0: return pos if pos == len(telemList): return pos-1 before = telemList[pos - 1] after = telemList[pos] if after - lidarNum < lidarNum - before: return pos else: return pos-1 def pointCorrection(t,V_x, V_y, V_z, Th, Ps, ph, Y_l, Z_l, lt): # Lidar points in a plane lp = np.array([0,Y_l,Z_l]) global d_i #print("d_i ",d_i) global t_i # current_distance = lp[0] + V_x * t # past distance = d_i[1] #V_x += V_x * -1 # Calculate current distance by adding velocity*delta time with previous distance X_d = d_i[0] + (V_x*(lt - t_i)) Y_d = d_i[1] + (V_y*(lt - t_i)) Z_d = d_i[2] + (V_z*(lt - t_i)) # distance from one point to another #print("X_d ", X_d) dist = np.array([X_d,Y_d,Z_d]) #print("dist ",dist) #Rotation correction # Rotation matrix that consists of all the formulas for conversion, R[n] where n is the objects position within the arra # ToDo:// doesnt seem to be working # X=ph=yaw, Y=Th=pitch, Z=Ps=roll """ #from wikipedia R = np.array([ [math.cos(Th)*math.cos(ph), -math.cos(ph)*math.sin(Ps)+math.sin(ph)*math.sin(Th)*math.cos(Ps), math.sin(ph)*math.sin(Ps)+math.cos(ph)*math.sin(Th)*math.cos(Ps)], [math.cos(Th)*math.sin(Ps), math.cos(ph)*math.cos(Ps)+math.sin(ph)*math.sin(Th)*math.sin(Ps), -math.sin(ph)*math.cos(Ps)+math.cos(ph)*math.sin(Th)*math.sin(Ps)], [-math.sin(Th), math.sin(ph)*math.cos(Th), math.cos(ph)*math.cos(Th)] ]) """ """ #original R = np.array([ [math.cos(Th)*math.cos(ph), math.cos(Th)*math.sin(ph), -math.sin(Th)], [math.sin(Ps)*math.sin(Th)*math.cos(ph) - math.cos(Ps)*math.sin(ph), math.sin(Ps)*math.sin(Th)*math.sin(ph) + math.cos(Ps)*math.cos(ph), math.cos(Th)*math.sin(Ps)], [math.cos(Ps)*math.sin(Th)*math.cos(ph) + math.sin(Ps)*math.sin(ph), math.cos(Ps)*math.sin(Th)*math.sin(ph) - math.sin(Ps)*math.cos(ph), math.cos(Th)*math.cos(Ps)] ]) """ """ #from website R = np.array([ [math.cos(Th)*math.cos(ph), math.cos(Ps)*math.sin(ph)+math.sin(Ps)*math.sin(Th)*math.cos(ph), math.sin(Ps)*math.sin(ph) - math.cos(Ps)*math.sin(Th)*math.cos(ph)], [-math.cos(Th)*math.sin(ph), math.cos(Ps)*math.cos(ph) - math.sin(Ps)*math.sin(Th)*math.sin(ph), math.sin(Ps)*math.cos(ph) + math.cos(Ps)*math.sin(Th)*math.sin(ph)], [math.sin(Th), -math.sin(Ps)*math.cos(Th), math.cos(Ps)*math.cos(Th)] ]) """ # Kellan R = np.array([ [math.cos(ph)*math.cos(Ps), -math.cos(ph)*math.sin(Ps), math.sin(ph)], [math.sin(Th)*math.sin(ph)*math.cos(Ps) + math.cos(Th)*math.sin(Ps), -math.sin(Th)*math.sin(ph)*math.sin(ph) + math.cos(Th)*math.cos(Ps), -math.sin(Th)*math.cos(ph)], [-math.cos(Th)*math.sin(ph)*math.cos(Ps) + math.sin(Th)*math.sin(Ps), math.cos(Th)*math.sin(ph)*math.sin(Ps)+math.sin(Th)*math.cos(Ps), math.cos(Th)*math.cos(ph)] ]) # The points we want. # Matrix multiplacation for points points = np.array([]) #print(points) points = R.dot(lp) final=[] final.append(points[0] + dist[0]) final.append(points[1] + dist[1]) final.append(points[2] + dist[2]) #final = points + dist # Set current point to previous point d_i = dist t_i = lt return(final) def main(): #open files for read/write with open(f1, 'r') as drone_telem,\ open(f2, 'r') as lidar_points, \ open(str(f2).strip(EXT)+"_merged.csv", 'w') as merged, \ open(str(f2).strip(EXT)+"_corrected.csv", "w") as corrected: # remove headers if present merged_writer = csv.writer(merged) corrected_writer = csv.writer(corrected) reader_telem = csv.reader(drone_telem, delimiter=",") header1 = next(reader_telem, None) reader_lidar = csv.reader(lidar_points, delimiter=",") header2 = next(reader_lidar, None) # Print headers for verification print(header1) print(header2) # initialise indexes telem_list = [] telem_time = [] #merged.write(', '.join(header1) + ', ' + ', '.join(header2) + "\n") merged.write("vX[0], vY[1], vZ[2], telem_time[3], pitch[4], roll[5], yaw[6], z[7], y[8], lidar_time[9]\n") # lati = row[0], longi = row[1], alti = row[2], # vX = row[3], vY = row[4], vZ = row[5], # time = row[6], heading = row[7], height = row[8], # pitch = row[9], roll = row[10], yaw = row[11] for row in reader_telem: timeT = float(str(row[6]).replace(".","")) pitch = math.pi * float(row[9]) / 180 yaw = math.pi * float(row[11]) / 180 roll = math.pi * float(row[10]) / 180 telem = [float(row[3]), float(row[4]), float(row[5]), timeT, pitch, roll, yaw] telem_time.append(timeT) telem_list.append(telem) print("Rows in telem: ",len(telem_list)) # z = row_l[0], y = row_l[1] # urg_time = row_l[2], i = row_l[3] # pc = row_l[4], rot = row_l[5] # sensor = row_l[6], realtime = row_l[7] lidar_rows = 0 corrected_rows = 0 failed_rows =0 for row_l in reader_lidar: lidar_rows += 1 timeL = float(str(row_l[7]).replace(".","")) #- 051100000.0 lidar = [float(row_l[0]), float(row_l[1]), timeL] pointsToMerge = takeClosest(telem_time,timeL) print(telem_time[pointsToMerge], " ", timeL, " ", telem_list[pointsToMerge][3]) #print("timeL: ", timeL) #print("telem_time: ",telem_time[pointsToMerge]) #print("diff: ", telem_time[pointsToMerge]-timeL) if timeL + 10 >= float(telem_time[pointsToMerge]) <= timeL - 10: failed_rows+=1 else: mergedPoints = telem_list[pointsToMerge] + lidar merged_writer.writerow(mergedPoints) #pointCorrection(t,V_x, V_y, V_z, Th, Ps, ph, Y_l, Z_l) # X=ph=yaw, Y=Th=pitch, Z=Ps=roll if merge_only == 0: correctedPoints = pointCorrection(mergedPoints[3],\ mergedPoints[0], mergedPoints[1], mergedPoints[2],\ mergedPoints[4], mergedPoints[6], mergedPoints[5],\ mergedPoints[8], mergedPoints[7], timeL) corrected_writer.writerow(correctedPoints) corrected_rows += 1 print("Rows corrected: ",corrected_rows) print("Lidar rows: ",lidar_rows) print("Failed rows: ", failed_rows) drone_telem.close() lidar_points.close() merged.close() corrected.close() if corrected_rows == 0: print("Files ") print("File Merge Complete. Saved to:", merged.name) if merge_only == 0: print("Point Correction Complete. Saved to:", corrected.name) if __name__ == "__main__": main()
\ No newline at end of file
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment