Commit b1c82a83 authored by aakhoja15's avatar aakhoja15
Browse files

Analysis stream is now based on velocity rather than GPS. Git will now ignore __pycache__ files.

parent af915f62
data-processing/telem_files/*
data-processing/lidar_files/*
data-processing/Archive/*
*/__pycache__/*
......@@ -30,7 +30,7 @@ def getPitchYawRollMat(yaw, pitch, roll):
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])
lp = array([-206, 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
......@@ -49,13 +49,13 @@ def pointCorrection(t, lat, lon, alt, yaw, pitch, roll, Y_l, Z_l):
return final
def getPitchYawRollQuat(yaw, pitch, roll):
q1 = Quaternion(axis = (0,0,1), angle = -yaw)
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])
lp = array([-206, -Y_l, -Z_l-53])
Q = getPitchYawRollQuat(yaw, pitch, roll)
points = Q.rotate(lp)
......@@ -71,13 +71,13 @@ def getMeanLat(pList):
sumLat = 0
count = 0
for pnt in pList:
sumLat += pnt[1]
sumLat += pnt[0]
count += 1
return sumLat/count
def shrinkPoints(pList, angleRatio):
"""Returns the list of shrunk/stretched points"""
shrinkMat = [[angleRatio, 0],[0, 1]]
shrinkMat = [[1, 0],[0, angleRatio]]
return list(map(lambda x: tuple(dot(shrinkMat, x)), pList))
if __name__ == "__main__":
......
......@@ -48,38 +48,25 @@ def timeChange(decTime):
timeInMs = (timeSplit[0]*3600000) + (timeSplit[1]*60000) + (timeSplit[2]*1000) + timeSplit[3]
return timeInMs
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:
def distFromVel(u, v, t):
return 0.5*(u+v)*t
# remove headers if present
merged_writer = writer(merged)
def getKiaPos(tChange, uList, vList):
return [distFromVel(uList[i], vList[i], tChange) for i in range(3)]
def readTelem(telemFile):
with open(telemFile, 'r') as drone_telem:
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")
# latLon = []
distList = []
velList = []
telem_data = []
timeOld = 0
# lati = row[0], longi = row[1], alti = row[2],
# vX = row[3], vY = row[4], vZ = row[5],
......@@ -89,37 +76,68 @@ def main():
for row in reader_telem:
try:
timeT = timeChange(row[6])
row[6] = timeT
except:
print("Unable to parse time - stopping operation. Failed on row:", row)
print("Time Element:",row[6])
quit()
pitch = radians(float(row[9]))
yaw = radians(float(row[11]))
roll = radians(float(row[10]))
lonLat.append([float(row[1]), float(row[0])])
float_telem = list(map(float, row))
float_telem[9:12] = list(map(radians, float_telem[9:12]))
if timeOld == 0:
distList = [0,0,0]
else:
changeInPos = getKiaPos(timeT-timeOld, velList, float_telem[3:6])
distList = [distList[i] + changeInPos[i] for i in range(3)]
telem = [float(row[0]), float(row[1]), float(row[2]), timeT, pitch, roll, yaw]
timeOld = timeT
velList, float_telem[3:6] = float_telem[3:6], distList
float_telem[-1], float_telem[6:-1] = float_telem[6], float_telem[7:]
# latLon.append(float_telem[0:2])
telem_time.append(timeT)
telem_list.append(telem)
print("Rows in telem: ",len(telem_list))
telemStart = telem_time[0]
telemEnd = telem_time[-1]
telem_data.append(float_telem)
print("Rows in telem: ",len(telem_time))
drone_telem.close()
# meanLat = getMeanLat(latLon)
# shrunk = shrinkPoints(latLon, 1/cos(radians(meanLat)))
# meterCoords = [[pnt[0] * MiliMetersPerDegree, pnt[1] * MiliMetersPerDegree] for pnt in shrunk]
meanLat = getMeanLat(lonLat)
shrunk = shrinkPoints(lonLat, cos(radians(meanLat)))
meterCoords = [[pnt[0] * MiliMetersPerDegree, pnt[1] * MiliMetersPerDegree] for pnt in shrunk]
return telem_time, telem_data #, meterCoords
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.rstrip('/') + '/' + (lidar_file.split('/')[-1]).rstrip(EXT)
telem_time, telem_data = readTelem(telem_file)
telemStart = telem_time[0]
telemEnd = telem_time[-1]
#open files for read/write
with 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:
merged_writer = writer(merged)
merged.write("lat[0], lon[1], alt[2], xKia[3], yKia[4], zKia[5], heading[6], height[7], pitch[8], roll[9], yaw[10], telem_time[11], zPoint[12], yPoint[13], intensityPoint[14], lidar_time[15], lengthLidar[16], radians[17], number_of_rotations[18], sample_number[19]\n")
# remove headers if present
reader_lidar = reader(lidar_points, delimiter=",")
lidar_header = next(reader_lidar, None)
# Print headers for verification
print(lidar_header)
if correct_flight:
corrected = open(output_file+"_corrected.csv", "w")
corrected.write("x, y, z, intensity\n")
corrected.write("x, y, z, intensity, lat, lon\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]
# z = row_l[0], y = row_l[1], lidar time stamp = row_l[2], intensity = row_l[3],
# length = row_l[4], radian = row_l[5], number of rotations = row_l[6],
# sample number = row_l[7], time stamp from print_data = row_l[8]
lidar_rows = 0
corrected_rows = 0
......@@ -128,29 +146,30 @@ def main():
for row_l in reader_lidar:
try:
timeL = timeChange(row_l[8])
row_l[8] = timeL
except:
print(row_l)
continue
print("Unable to parse time - stopping operation. Failed on row:", row_l)
print("Time Element:",row_l[6])
quit()
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)
float_lidar = list(map(float, row_l))
float_lidar[2:4] = float_lidar[3:1:-1] # interchanges the position of elements at position 2,3
# if timeL + 10 >= float(telem_time[pointsToMerge]) <= timeL - 10: # What??
# failed_rows+=1
# else:
mergedPoints = telem_list[pointsToMerge] + lidar + list(shrunk[pointsToMerge])
pointsToMerge = takeClosest(telem_time,timeL)
mergedPoints = telem_data[pointsToMerge] + float_lidar[:-1]
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]]
# lat[0], lon[1], alt[2], xKia[3], yKia[4], zKia[5], heading[6], height[7],
# pitch[8], roll[9], yaw[10], telem_time[11], zPoint[12], yPoint[13],
# intensityPoint[14], lidar_time[15], lengthLidar[16], radians[17],
# number_of_rotations[18], sample_number[19]
# quatCorrection(t, lat, lon, alt, yaw, pitch, roll, Y_l, Z_l)
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)
correctedPoints = quatCorrection(mergedPoints[11], mergedPoints[4], mergedPoints[3],\
mergedPoints[5], mergedPoints[10], mergedPoints[8], mergedPoints[9],\
mergedPoints[13], mergedPoints[12])
correctedPoints.append(mergedPoints[14])
corrected_writer.writerow(correctedPoints)
corrected_rows += 1
else:
......@@ -161,7 +180,6 @@ def main():
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:
......
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