Commit 39e4e005 authored by aakhoja15's avatar aakhoja15
Browse files

Deleted the old MergeAndCorrect, and divided the process into two separate...

Deleted the old MergeAndCorrect, and divided the process into two separate processes. The correct file also uses Quaternions now, and is creating a good point cloud
parent 35d59b0c
#!/usr/bin/env python
# Script to correct the merged lidar data
# Author: Ahsan Ali Khoja, Jacob Barahona Kamsvaag, Nic Arnold
# Email: ahsan.khoja@gmail.com; jacobbarahona@gmail.com; niarnold14@gmail.com
# Date: June 13, 2018
from numpy import array, dot
from math import sin, cos
from pyquaternion import Quaternion
from csv import reader, writer
import argparse
MiliMetersPerDegree = 111320000
USE = "Script to correct merged lidar data.\nExamples: $ python CorrectMerged.py -m lidar_merged.csv"
SUFFIX = "_merged.csv"
def argumentHandler():
parser = argparse.ArgumentParser(description=USE, formatter_class=argparse.RawTextHelpFormatter)
parser.add_argument('-m', '--merged_file', help='Enter the input lidar csv file', required=True)
# parser.add_argument('-o', '--output_dir', help='Enter the output directory path', default='edited_lidar_time', required=False)
return parser.parse_args()
def getPitchYawRollMat(yaw, pitch, roll):
mat_yaw = array([[cos(yaw), -sin(yaw), 0], [sin(yaw), cos(yaw), 0], [0,0,1]])
mat_pitch = array([[cos(pitch), 0, sin(pitch)],[0,1,0],[-sin(pitch),0,cos(pitch)]])
mat_roll = array([[1,0,0], [0, cos(roll), -sin(roll)], [0, sin(roll), cos(roll)]])
return mat_roll.dot(mat_pitch.dot(mat_yaw))
def pointCorrection(t, lat, lon, alt, yaw, pitch, roll, Y_l, Z_l):
# Lidar points in a plane
lp = array([-140, Y_l, -Z_l-53])
# Rotation correction
# Rotation matrix that consists of all the formulas for conversion, R[n] where n is the objects position within the array
R = getPitchYawRollMat(yaw, pitch, roll)
# The points we want.
# Matrix multiplacation for points
points = array([])
points = R.dot(lp)
final=[]
final.append(points[0] + lon)
final.append(points[1] + lat)
final.append(points[2] + alt)
return final
def getPitchYawRollQuat(yaw, pitch, roll):
q1 = Quaternion(axis = (0,0,1), angle = -yaw)
q2 = Quaternion(axis = q1.rotate((0,1,0)), angle = pitch)
q3 = Quaternion(axis = q2.rotate(q1.rotate((1,0,0))), angle = roll)
return q3
def quatCorrection(t, lat, lon, alt, yaw, pitch, roll, Y_l, Z_l):
lp = array([-140, Y_l, -Z_l-53])
Q = getPitchYawRollQuat(yaw, pitch, roll)
points = Q.rotate(lp)
final=[]
final.append(points[0] + lon)
final.append(points[1] + lat)
final.append(points[2] + alt)
return final
def getMeanLat(pList):
sumLat = 0
count = 0
for pnt in pList:
sumLat += pnt[1]
count += 1
return sumLat/count
def shrinkPoints(pList, angleRatio):
"""Returns the list of shrunk/stretched points"""
shrinkMat = [[angleRatio, 0],[0, 1]]
return list(map(lambda x: tuple(dot(shrinkMat, x)), pList))
if __name__ == "__main__":
args = vars(argumentHandler())
merged_file = args['merged_file']
corrected_file = merged_file.rstrip(SUFFIX) + "_corrected.csv"
with open(merged_file, 'r') as merged, \
open(corrected_file, 'w') as corrected:
merged_reader = reader(merged, delimiter=",")
merged_header = next(merged_reader, None)
corrected.write("x, y, z, intensity\n")
corrected_writer = writer(corrected)
corrected_rows = 0
for row_merged in merged_reader:
float_row_merged = [float(i) for i in row_merged]
meterCoords = [float_row_merged[11]*MiliMetersPerDegree, float_row_merged[12]*MiliMetersPerDegree]
correctedPoints = quatCorrection(float_row_merged[3],\
meterCoords[1], meterCoords[0], float_row_merged[2],\
float_row_merged[6], float_row_merged[4], float_row_merged[5],\
float_row_merged[8], float_row_merged[7])
correctedPoints.append(float_row_merged[10])
corrected_writer.writerow(correctedPoints)
corrected_rows += 1
merged.close()
corrected.close()
print("Rows corrected: ",corrected_rows)
print("Point Correction Complete. Saved to:", corrected.name)
#!/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 #Global Variables # Initialise distance between points d_i=np.array([0,0,0]) d_ii = 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+0.03,Y_d,Z_d]) #print("v = 0") #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]) #print (final) #final = points + dist # Set current point to previous point d_i = dist t_i = lt return(final) def main(): global merge_only #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] #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 intensity = float(row_l[3]) 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) correctedPoints.append(intensity) #print(correctedPoints) 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
# Script to merge lidar output with UAV telemetry data, and correct the flight
# Author: Ahsan Ali Khoja, Jacob Barahona Kamsvaag, Nic Arnold
# Email: ahsan.khoja@gmail.com; jacobbarahona@gmail.com; niarnold14@gmail.com
# Date: June 13, 2018
import sys
from csv import reader, writer
from math import radians, cos
from bisect import bisect_left
from CorrectMerged import getPitchYawRollQuat, quatCorrection, getMeanLat, shrinkPoints, MiliMetersPerDegree
import argparse
USE = "Script to merge lidar output with UAV telemetry data.\nExamples: $ python MergeTelemLidar.py -t kia_flightlog.csv -l lidar_raw.csv\n{0}$ python MergeTelemLidar.py -t kia_flightlog.csv -l lidar_raw.csv -o edited_lidar_time --correct_flight".format(" "*len("Examples: "))
EXT = ".csv"
def takeClosest(telemList, lidarNum):
# Optimize this
""" 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
elif pos == len(telemList):
return pos-1
before = telemList[pos-1]
after = telemList[pos]
if (after + before) < (2 * lidarNum): # after - lidarNum < lidarNum - before
return pos
else:
return pos-1
def argumentHandler():
parser = argparse.ArgumentParser(description=USE, formatter_class=argparse.RawTextHelpFormatter)
parser.add_argument('-l', '--lidar_file', help='Enter the input lidar csv file', required=True)
parser.add_argument('-t', '--telem_file', help='Enter the input telemetry csv file', required=True)
parser.add_argument('-o', '--output_dir', help='Enter the output directory path', default='edited_lidar_time', required=False)
parser.add_argument('-c', '--correct_flight', action='store_true')
return parser.parse_args()
def timeChange(decTime):
timeSplit = [int(i) for i in decTime.split(".")]
if len(timeSplit) == 5:
timeInMs = (timeSplit[0]*3600000) + (timeSplit[1]*60000) + (timeSplit[2]*1000) + timeSplit[3] + timeSplit[4]*0.001 # Conversion to milisecond
else:
timeInMs = (timeSplit[0]*3600000) + (timeSplit[1]*60000) + (timeSplit[2]*1000) + timeSplit[3]
return timeInMs
def kiaTimeChange(decTime):
pass
def main():
args = vars(argumentHandler())
lidar_file, telem_file, output_dir, correct_flight = args["lidar_file"], args["telem_file"], args["output_dir"], args["correct_flight"]
output_file = output_dir+(lidar_file.split('/')[-1]).rstrip(EXT)
#open files for read/write
with open(telem_file, 'r') as drone_telem,\
open(lidar_file, 'r') as lidar_points, \
open(output_file+"_merged.csv", 'w') as merged:
# open(str(f2).strip(EXT)+"_corrected.csv", "w") as corrected:
# remove headers if present
merged_writer = writer(merged)
reader_telem = reader(drone_telem, delimiter=",")
telem_header = next(reader_telem, None)
reader_lidar = reader(lidar_points, delimiter=",")
lidar_header = next(reader_lidar, None)
# Print headers for verification
print(telem_header)
print(lidar_header)
# initialise indexes
telem_list = []
telem_time = []
lonLat = []
# merged.write(', '.join(header1) + ', ' + ', '.join(header2) + "\n")
merged.write("lat[0], lon[1], alt[2], telem_time[3], pitch[4], roll[5], yaw[6], z[7], y[8], lidar_time[9], intensity[10], stretchedLon[11], stretchedLat[12]\n")
# corrected.write("x, y, z, intensity\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 = timeChange(row[6])
pitch = radians(float(row[9]))
yaw = radians(float(row[11]))
roll = radians(float(row[10]))
lonLat.append([float(row[1]), float(row[0])])
telem = [float(row[0]), float(row[1]), float(row[2]), timeT, pitch, roll, yaw]
telem_time.append(timeT)
telem_list.append(telem)
print("Rows in telem: ",len(telem_list))
telemStart = telem_time[0]
telemEnd = telem_time[-1]
meanLat = getMeanLat(lonLat)
shrunk = shrinkPoints(lonLat, cos(radians(meanLat)))
meterCoords = [[pnt[0] * MiliMetersPerDegree, pnt[1] * MiliMetersPerDegree] for pnt in shrunk]
if correct_flight:
corrected = open(output_file+"_corrected.csv", "w")
corrected.write("x, y, z, intensity\n")
corrected_writer = writer(corrected)
# 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
time_out_of_range = 0
for row_l in reader_lidar:
try:
timeL = timeChange(row_l[8])
except:
print(row_l)
continue
if telemStart < timeL < telemEnd:
intensity = float(row_l[3])
lidar = [float(row_l[0]), float(row_l[1]), timeL, intensity]
pointsToMerge = takeClosest(telem_time,timeL)
# if timeL + 10 >= float(telem_time[pointsToMerge]) <= timeL - 10: # What??
# failed_rows+=1
# else:
mergedPoints = telem_list[pointsToMerge] + lidar + list(shrunk[pointsToMerge])
merged_writer.writerow(mergedPoints)
#pointCorrection(t, lat, lon, alt, yaw, pitch, roll, Y_l, Z_l)
# X=roll=yaw, Y=yaw=pitch, Z=pitch=roll
# mergedPoints = [lat[0], lon[1], alt[2], telem_time[3], pitch[4], roll[5], yaw[6], z[7], y[8], lidar_time[9]]
if correct_flight:
correctedPoints = quatCorrection(mergedPoints[3],\
meterCoords[pointsToMerge][1], meterCoords[pointsToMerge][0], mergedPoints[2],\
mergedPoints[6], mergedPoints[4], mergedPoints[5],\
mergedPoints[8], mergedPoints[7])
correctedPoints.append(intensity)
corrected_writer.writerow(correctedPoints)
corrected_rows += 1
else:
time_out_of_range += 1
lidar_rows += 1
print("Lidar rows: ",lidar_rows)
print("Failed rows: ", failed_rows)
print("Time Out of Range rows: ", time_out_of_range)
drone_telem.close()
lidar_points.close()
merged.close()
if correct_flight:
corrected.close()
print("File Merge Complete. Saved to:", merged.name)
if correct_flight:
print("Rows corrected: ",corrected_rows)
print("Point Correction Complete. Saved to:", corrected.name)
if __name__ == '__main__':
main()
# args = vars(argumentHandler())
# print(args)
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