# -*- coding: utf-8 -*- ############################################################################ # OpenMMS Boresight Calibration (Livox) # ############################################################################ # Version: 1.3.0 # # Date: October 2020 # # Author: Ryan Brazeal # # Email: ryan.brazeal@ufl.edu # # # # OPEN-SOURCE LICENSE INFO: # # # # This file is part of OpenMMS_OSS. # # # # OpenMMS_OSS is free software: you can redistribute it and/or modify # # it under the terms of the GNU General Public License as published by # # the Free Software Foundation, either version 3 of the License, or # # (at your option) any later version. # # # # OpenMMS_OSS is distributed in the hope that it will be useful, # # but WITHOUT ANY WARRANTY; without even the implied warranty of # # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # # GNU General Public License for more details. # # # # You should have received a copy of the GNU General Public License # # along with OpenMMS_OSS. If not, see . # # # ############################################################################ import pycuda.autoinit import pycuda.gpuarray as gpuarray import skcuda.linalg as cudalinalg from numba import cuda, vectorize import numdifftools as nd import numpy as np import time import os import sys import math from pathlib import Path def sin(a): return np.sin(a) def cos(a): return np.cos(a) def tan(a): return np.tan(a) @vectorize(['float32(float32, float32, float32)'], target='parallel', nopython=True) def geoRefCalcSocsX(azi, vert, dist): return math.sin(vert) * math.cos(azi) * dist @vectorize(['float32(float32, float32, float32)'], target='parallel', nopython=True) def geoRefCalcSocsY(azi, vert, dist): return math.sin(vert) * math.sin(azi) * dist @vectorize(['float32(float32, float32)'], target='parallel', nopython=True) def geoRefCalcSocsZ(vert, dist): return math.cos(vert) * dist @vectorize(['float64(float32,float32,float32,float32,float32,float32,float32,float32,float32,float32,float32,float64)'], target='parallel', nopython=True) def geoRefCalcGlcsX2(x0,y0,z0,R,P,H,X,Y,Z,Lat,Lon,Xg): sinR = math.sin(R) cosR = math.cos(R) sinP = math.sin(P) cosP = math.cos(P) sinH = math.sin(H) cosH = math.cos(H) sinX = math.sin(X) cosX = math.cos(X) sinY = math.sin(Y) cosY = math.cos(Y) sinZ = math.sin(Z) cosZ = math.cos(Z) sinLat = math.sin(Lat) cosLat = math.cos(Lat) sinLon = math.sin(Lon) cosLon = math.cos(Lon) return (Xg + x0*(cosY*(cosZ*(cosLat*cosLon*sinP + cosP*(-cosH*cosLon*sinLat - sinH*sinLon)) + sinZ*(cosR*(-cosH*sinLon + cosLon*sinH*sinLat) + sinR*(-cosLat*cosLon*cosP + sinP*(-cosH*cosLon*sinLat - sinH*sinLon)))) - sinY*(cosR*(-cosLat*cosLon*cosP + sinP*(-cosH*cosLon*sinLat - sinH*sinLon)) - sinR*(-cosH*sinLon + cosLon*sinH*sinLat))) + y0*(cosX*(cosZ*(cosR*(-cosH*sinLon + cosLon*sinH*sinLat) + sinR*(-cosLat*cosLon*cosP + sinP*(-cosH*cosLon*sinLat - sinH*sinLon))) - sinZ*(cosLat*cosLon*sinP + cosP*(-cosH*cosLon*sinLat - sinH*sinLon))) + sinX*(cosY*(cosR*(-cosLat*cosLon*cosP + sinP*(-cosH*cosLon*sinLat - sinH*sinLon)) - sinR*(-cosH*sinLon + cosLon*sinH*sinLat)) + sinY*(cosZ*(cosLat*cosLon*sinP + cosP*(-cosH*cosLon*sinLat - sinH*sinLon)) + sinZ*(cosR*(-cosH*sinLon + cosLon*sinH*sinLat) + sinR*(-cosLat*cosLon*cosP + sinP*(-cosH*cosLon*sinLat - sinH*sinLon)))))) + z0*(cosX*(cosY*(cosR*(-cosLat*cosLon*cosP + sinP*(-cosH*cosLon*sinLat - sinH*sinLon)) - sinR*(-cosH*sinLon + cosLon*sinH*sinLat)) + sinY*(cosZ*(cosLat*cosLon*sinP + cosP*(-cosH*cosLon*sinLat - sinH*sinLon)) + sinZ*(cosR*(-cosH*sinLon + cosLon*sinH*sinLat) + sinR*(-cosLat*cosLon*cosP + sinP*(-cosH*cosLon*sinLat - sinH*sinLon))))) - sinX*(cosZ*(cosR*(-cosH*sinLon + cosLon*sinH*sinLat) + sinR*(-cosLat*cosLon*cosP + sinP*(-cosH*cosLon*sinLat - sinH*sinLon))) - sinZ*(cosLat*cosLon*sinP + cosP*(-cosH*cosLon*sinLat - sinH*sinLon))))) @vectorize(['float64(float32,float32,float32,float32,float32,float32,float32,float32,float32,float32,float32,float64)'], target='parallel', nopython=True) def geoRefCalcGlcsY2(x0,y0,z0,R,P,H,X,Y,Z,Lat,Lon,Yg): sinR = math.sin(R) cosR = math.cos(R) sinP = math.sin(P) cosP = math.cos(P) sinH = math.sin(H) cosH = math.cos(H) sinX = math.sin(X) cosX = math.cos(X) sinY = math.sin(Y) cosY = math.cos(Y) sinZ = math.sin(Z) cosZ = math.cos(Z) sinLat = math.sin(Lat) cosLat = math.cos(Lat) sinLon = math.sin(Lon) cosLon = math.cos(Lon) return (Yg + x0*(cosY*(cosZ*(cosLat*sinLon*sinP + cosP*(-cosH*sinLat*sinLon + cosLon*sinH)) + sinZ*(cosR*(cosH*cosLon + sinH*sinLat*sinLon) + sinR*(-cosLat*cosP*sinLon + sinP*(-cosH*sinLat*sinLon + cosLon*sinH)))) - sinY*(cosR*(-cosLat*cosP*sinLon + sinP*(-cosH*sinLat*sinLon + cosLon*sinH)) - sinR*(cosH*cosLon + sinH*sinLat*sinLon))) + y0*(cosX*(cosZ*(cosR*(cosH*cosLon + sinH*sinLat*sinLon) + sinR*(-cosLat*cosP*sinLon + sinP*(-cosH*sinLat*sinLon + cosLon*sinH))) - sinZ*(cosLat*sinLon*sinP + cosP*(-cosH*sinLat*sinLon + cosLon*sinH))) + sinX*(cosY*(cosR*(-cosLat*cosP*sinLon + sinP*(-cosH*sinLat*sinLon + cosLon*sinH)) - sinR*(cosH*cosLon + sinH*sinLat*sinLon)) + sinY*(cosZ*(cosLat*sinLon*sinP + cosP*(-cosH*sinLat*sinLon + cosLon*sinH)) + sinZ*(cosR*(cosH*cosLon + sinH*sinLat*sinLon) + sinR*(-cosLat*cosP*sinLon + sinP*(-cosH*sinLat*sinLon + cosLon*sinH)))))) + z0*(cosX*(cosY*(cosR*(-cosLat*cosP*sinLon + sinP*(-cosH*sinLat*sinLon + cosLon*sinH)) - sinR*(cosH*cosLon + sinH*sinLat*sinLon)) + sinY*(cosZ*(cosLat*sinLon*sinP + cosP*(-cosH*sinLat*sinLon + cosLon*sinH)) + sinZ*(cosR*(cosH*cosLon + sinH*sinLat*sinLon) + sinR*(-cosLat*cosP*sinLon + sinP*(-cosH*sinLat*sinLon + cosLon*sinH))))) - sinX*(cosZ*(cosR*(cosH*cosLon + sinH*sinLat*sinLon) + sinR*(-cosLat*cosP*sinLon + sinP*(-cosH*sinLat*sinLon + cosLon*sinH))) - sinZ*(cosLat*sinLon*sinP + cosP*(-cosH*sinLat*sinLon + cosLon*sinH))))) @vectorize(['float64(float32,float32,float32,float32,float32,float32,float32,float32,float32,float32,float64)'], target='parallel', nopython=True) def geoRefCalcGlcsZ2(x0,y0,z0,R,P,H,X,Y,Z,Lat,Zg): sinR = math.sin(R) cosR = math.cos(R) sinP = math.sin(P) cosP = math.cos(P) sinH = math.sin(H) cosH = math.cos(H) sinX = math.sin(X) cosX = math.cos(X) sinY = math.sin(Y) cosY = math.cos(Y) sinZ = math.sin(Z) cosZ = math.cos(Z) sinLat = math.sin(Lat) cosLat = math.cos(Lat) return (Zg + x0*(cosY*(cosZ*(cosH*cosLat*cosP + sinLat*sinP) + sinZ*(-cosLat*cosR*sinH + sinR*(cosH*cosLat*sinP - cosP*sinLat))) - sinY*(cosLat*sinH*sinR + cosR*(cosH*cosLat*sinP - cosP*sinLat))) + y0*(cosX*(cosZ*(-cosLat*cosR*sinH + sinR*(cosH*cosLat*sinP - cosP*sinLat)) - sinZ*(cosH*cosLat*cosP + sinLat*sinP)) + sinX*(cosY*(cosLat*sinH*sinR + cosR*(cosH*cosLat*sinP - cosP*sinLat)) + sinY*(cosZ*(cosH*cosLat*cosP + sinLat*sinP) + sinZ*(-cosLat*cosR*sinH + sinR*(cosH*cosLat*sinP - cosP*sinLat))))) + z0*(cosX*(cosY*(cosLat*sinH*sinR + cosR*(cosH*cosLat*sinP - cosP*sinLat)) + sinY*(cosZ*(cosH*cosLat*cosP + sinLat*sinP) + sinZ*(-cosLat*cosR*sinH + sinR*(cosH*cosLat*sinP - cosP*sinLat)))) - sinX*(cosZ*(-cosLat*cosR*sinH + sinR*(cosH*cosLat*sinP - cosP*sinLat)) - sinZ*(cosH*cosLat*cosP + sinLat*sinP)))) #read in set of points from a text file (comma delimited), data order needs to be x,y,z [in socs], Omega, Phi, Kappa, X,Y,Z [of scanner in object space] #offset value allows for an integer offset from the start of the line to where the x_socs value is located def readPlaneData(filename,offsets): count = 0 foundZeroLonCount = 0 with open(filename,"r") as csvData: for record in csvData: if len(record) != 0: if record[0:2] != "//": elements = record.strip("\n").split(",") if float(elements[3]) != 0: count += 1 else: foundZeroLonCount += 1 data = np.zeros((count,14)) index = 0 if foundZeroLonCount > 0: print(" *** WARNING: Skipped using " + str(foundZeroLonCount) + " points in " + Path(filename).name + " with Longitude = 0 values ***") with open(filename,"r") as csvData: for record in csvData: if len(record) != 0: if record[0:2] != "//": elements = record.strip("\n").split(",") if float(elements[3]) != 0: data[index][0] = float(elements[0]) #original Xp in M data[index][1] = float(elements[1]) #original Yp in M data[index][2] = float(elements[2]) #original Zp in M data[index][3] = float(elements[13]) #dist_socs data[index][4] = np.radians(float(elements[12])) #azi_socs [radians] data[index][5] = np.radians(float(elements[11])) #vert_socs [radians] data[index][6] = float(elements[10]) #Roll data[index][7] = float(elements[9]) #Pitch data[index][8] = float(elements[8]) #Heading data[index][9] = float(elements[7]) + offsets[0] #Xs in M data[index][10] = float(elements[6]) + offsets[1] #Ys in M data[index][11] = float(elements[5]) + offsets[2] #Zs in M data[index][12] = float(elements[4]) #lat data[index][13] = float(elements[3]) #lon index+=1 return count, data def readPlaneFiles(curDir, planeFiles): if not os.path.isfile(curDir / "verbose_offsets.csv"): print("\n *** ERROR: The 'verbose_offsets.csv' file is missing from the current directory, program is stopping\n ***") sys.exit() offsets_file = open(curDir / "verbose_offsets.csv","r") offsets = offsets_file.readline().strip("\n").split(",") offsets[0] = float(offsets[0]) offsets[1] = float(offsets[1]) offsets[2] = float(offsets[2]) planes = [] totalPoints = 0 for i in range(0,len(planeFiles)): count, planeData = readPlaneData(planeFiles[i][0], offsets) totalPoints += count planes.append(planeData) return totalPoints, planes def planeFit(Xs, Ys, Zs): count = len(Xs) Q = np.zeros((count,3)) Q[:,0] = Xs Q[:,1] = Ys Q[:,2] = Zs #GPU processing Qa = np.ascontiguousarray(Q.T,dtype=np.float32) Qcuda = gpuarray.to_gpu(Qa) vt2, _, _ = cudalinalg.svd(Qcuda) vt2 = vt2.T planeNormal = -vt2.get()[2] rmsi = 0.0 pVec = np.empty(3) for ii in range(0,count): pVec[0] = Q[ii,0] pVec[1] = Q[ii,1] pVec[2] = Q[ii,2] d = planeNormal.dot(pVec) rmsi += d**2 rmsErr = np.sqrt(rmsi / float(count)) return rmsErr def planeFiti(X_norm, Y_norm, Z_norm): count = len(X_norm) Q = np.empty((count,3)) Q[:,0] = X_norm Q[:,1] = Y_norm Q[:,2] = Z_norm #GPU processing Qa = np.ascontiguousarray(Q.T,dtype=np.float32) Qcuda = gpuarray.to_gpu(Qa) vt2, _, _ = cudalinalg.svd(Qcuda) vt2 = vt2.T planeNormal = -vt2.get()[2] pVec = np.empty(3) d_values = np.empty(count) for ii in range(0,count): pVec[0] = Q[ii,0] pVec[1] = Q[ii,1] pVec[2] = Q[ii,2] d = planeNormal.dot(pVec) d_values[ii] = d return d_values def reCalcPlanePoints(cd,bx,by,bz,fileName="",export=False,timeStr=""): dist = cd[0] azi = cd[1] vert = cd[2] distSF = 1.0 distCorr = 0.0 aziCorr = 0.0 x0 = geoRefCalcSocsX(azi, vert, dist, distSF, aziCorr, distCorr) y0 = geoRefCalcSocsY(azi, vert, dist, distSF, aziCorr, distCorr) z0 = geoRefCalcSocsZ(vert, dist, distSF, distCorr) #x0,y0,z0,R,P,H,X,Y,Z,Lat,Lon,Xg newPts_X = geoRefCalcGlcsX2(x0,y0,z0,cd[3],cd[4],cd[5],bx,by,bz,cd[6],cd[7],cd[8]) newPts_Y = geoRefCalcGlcsY2(x0,y0,z0,cd[3],cd[4],cd[5],bx,by,bz,cd[6],cd[7],cd[9]) newPts_Z = geoRefCalcGlcsZ2(x0,y0,z0,cd[3],cd[4],cd[5],bx,by,bz,cd[6],cd[10]) avgx = np.average(newPts_X) avgy = np.average(newPts_Y) avgz = np.average(newPts_Z) newPts_X_norm = newPts_X - avgx newPts_Y_norm = newPts_Y - avgy newPts_Z_norm = newPts_Z - avgz if export: exportFile = open(fileName[:-4] + "_bore_" + timeStr + ".txt","w") exportFile.write("//X,Y,Z\n") for i in range(0,len(newPts_X)): exportFile.write(str(round(newPts_X[i],4))+","+str(round(newPts_Y[i],4))+","+str(round(newPts_Z[i],4))+"\n") exportFile.close() return np.asarray(newPts_X_norm, np.float32), np.asarray(newPts_Y_norm, np.float32), np.asarray(newPts_Z_norm, np.float32) def reCalcPlanePoints2(cd,bx,by,bz,fileName="",export=False,timeStr=""): dist = cd[0] azi = cd[1] vert = cd[2] x0 = geoRefCalcSocsX(azi, vert, dist) y0 = geoRefCalcSocsY(azi, vert, dist) z0 = geoRefCalcSocsZ(vert, dist) #x0,y0,z0,R,P,H,X,Y,Z,Lat,Lon,Xg newPts_X = geoRefCalcGlcsX2(x0,y0,z0,cd[3],cd[4],cd[5],bx,by,bz,cd[6],cd[7],cd[8]) newPts_Y = geoRefCalcGlcsY2(x0,y0,z0,cd[3],cd[4],cd[5],bx,by,bz,cd[6],cd[7],cd[9]) newPts_Z = geoRefCalcGlcsZ2(x0,y0,z0,cd[3],cd[4],cd[5],bx,by,bz,cd[6],cd[10]) avgx = np.average(newPts_X) avgy = np.average(newPts_Y) avgz = np.average(newPts_Z) newPts_X_norm = newPts_X - avgx newPts_Y_norm = newPts_Y - avgy newPts_Z_norm = newPts_Z - avgz if export: exportFile = open(fileName[:-4] + "_ECEF_" + timeStr + ".txt","w") exportFile.write("//X,Y,Z\n") for i in range(0,len(newPts_X)): exportFile.write(str(round(newPts_X[i],4))+","+str(round(newPts_Y[i],4))+","+str(round(newPts_Z[i],4))+"\n") exportFile.close() return np.asarray(newPts_X_norm, np.float32), np.asarray(newPts_Y_norm, np.float32), np.asarray(newPts_Z_norm, np.float32) def planesRMS(cuda_data,boresights,doPrint): bx = boresights[0] by = boresights[1] bz = boresights[2] if doPrint: print() initialRMSvalues = [] for i in range(0,len(cuda_data)): Xs, Ys, Zs = reCalcPlanePoints(cuda_data[i],bx,by,bz) rmsErr = planeFit(Xs, Ys, Zs) initialRMSvalues.append(rmsErr**2) if doPrint: print(" " + str(i) + "_rms:" + str(round(rmsErr,4))) rms = np.sqrt(np.average(np.asarray(initialRMSvalues))) if doPrint: print() return rms def planesRMS2(cuda_data,boresights,planeFiles,export=False,timeStr=""): bx = boresights[0] by = boresights[1] bz = boresights[2] rmsValues = [] pts_per_plane = [] for i in range(0,len(cuda_data)): Xs, Ys, Zs = reCalcPlanePoints(cuda_data[i],bx,by,bz,planeFiles[i][0],export,timeStr) rmsErr = planeFit(Xs, Ys, Zs) rmsValues.append(rmsErr) pts_per_plane.append(len(Xs)) rms_avg = np.sqrt(np.average(np.asarray(rmsValues)**2)) weights = np.asarray(pts_per_plane) / np.sum(np.asarray(pts_per_plane)) rms_w_avg = np.sqrt(np.average(np.asarray(rmsValues)**2,weights=weights)) return rmsValues, rms_avg, rms_w_avg def planesRMS3(planes): rmsValues = [] for i in range(0,len(planes)): plane = planes[i] Xs = plane[:,0] Ys = plane[:,1] Zs = plane[:,2] avgx = np.average(Xs) avgy = np.average(Ys) avgz = np.average(Zs) X_norm = Xs - avgx Y_norm = Ys - avgy Z_norm = Zs - avgz rmsErr = planeFit(X_norm, Y_norm, Z_norm) rmsValues.append(rmsErr) return rmsValues def planesRMS4(cuda_data,boresights,planeFiles,export=False,timeStr=""): bx = boresights[0] by = boresights[1] bz = boresights[2] rmsValues = [] pts_per_plane = [] for i in range(0,len(cuda_data)): Xs, Ys, Zs = reCalcPlanePoints2(cuda_data[i],bx,by,bz,planeFiles[i][0],export,timeStr) rmsErr = planeFit(Xs, Ys, Zs) rmsValues.append(rmsErr) pts_per_plane.append(len(Xs)) rms_avg = np.sqrt(np.average(np.asarray(rmsValues)**2)) weights = np.asarray(pts_per_plane) / np.sum(np.asarray(pts_per_plane)) rms_w_avg = np.sqrt(np.average(np.asarray(rmsValues)**2,weights=weights)) return rmsValues, rms_avg, rms_w_avg def planesRMSi(cuda_data,boresights,doPrint): bx = boresights[0] by = boresights[1] bz = boresights[2] if doPrint: print() count = 0 pts_per_plane = [] pts_per_plane.append(0) for i in range(0,len(cuda_data)): count += len(cuda_data[i][0]) pts_per_plane.append(count) resValues = np.empty(count) for i in range(0,len(cuda_data)): X_norm, Y_norm, Z_norm = reCalcPlanePoints(cuda_data[i],bx,by,bz) d_values = planeFiti(X_norm, Y_norm, Z_norm) pts = 0 for j in range(int(pts_per_plane[i]),int(pts_per_plane[i+1])): resValues[j] = d_values[pts] pts += 1 return resValues def planesRMSi2(cuda_data,boresights,doPrint): bx = boresights[0] by = boresights[1] bz = boresights[2] if doPrint: print() count = 0 pts_per_plane = [] pts_per_plane.append(0) for i in range(0,len(cuda_data)): count += len(cuda_data[i][0]) pts_per_plane.append(count) resValues = np.empty(count) for i in range(0,len(cuda_data)): X_norm, Y_norm, Z_norm = reCalcPlanePoints2(cuda_data[i],bx,by,bz) d_values = planeFiti(X_norm, Y_norm, Z_norm) pts = 0 for j in range(int(pts_per_plane[i]),int(pts_per_plane[i+1])): resValues[j] = d_values[pts] pts += 1 return resValues def tightenPlanes(plane1,plane2,angleIncrement,bx,by,bz,iteration,doPrint=False): optimal_bx = 0.0 optimal_by = 0.0 plusMinus = 1.0 if True: #iteration % 2 == 1: plane_a = reCalcPlanePoints(plane1,bx,by,bz) _, rmsErr, _ = planeFit(plane_a) latest_rms = rmsErr count = 1 dontStop = True print("\n Optimizing the boresight X and Y parameters (" + str(iteration) + ")") while dontStop: bx += (plusMinus * angleIncrement) plane_a = reCalcPlanePoints(plane1,bx,by,bz) _, rmsErr_a, _ = planeFit(plane_a) if count == 1: if rmsErr_a > latest_rms: plusMinus *= -1.0 bx += (plusMinus * angleIncrement) if rmsErr_a > latest_rms and count > 1: dontStop = False else: latest_rms = rmsErr_a optimal_bx = bx if doPrint: print(" RMS_X = " + str(round(latest_rms,4))) count += 1 plane_a = reCalcPlanePoints(plane2,bx,by,bz) _, rmsErr, _ = planeFit(plane_a) latest_rms = rmsErr count = 1 dontStop = True # print("\n Optimizing the boresight Y parameter...") while dontStop: by += (plusMinus * angleIncrement) plane_a = reCalcPlanePoints(plane2,bx,by,bz) _, rmsErr, _ = planeFit(plane_a) if count == 1: if rmsErr > latest_rms: plusMinus *= -1.0 by += (plusMinus * angleIncrement) if rmsErr > latest_rms and count > 1: dontStop = False else: latest_rms = rmsErr optimal_by = by if doPrint: print(" RMS_Y = " + str(round(latest_rms,4))) count += 1 if True: #iteration % 2 == 1: plane_a = reCalcPlanePoints(plane1,bx,by,bz) _, rmsErr_a, _ = planeFit(plane_a) if False: #iteration % 2 == 0: plane_a = reCalcPlanePoints(plane1,bx,by,bz) _, rmsErr, _ = planeFit(plane_a) latest_rms = rmsErr count = 1 dontStop = True print("\n Optimizing the boresight X and Y parameters (" + str(iteration) + ")") while dontStop: bx += (plusMinus * angleIncrement) plane_a = reCalcPlanePoints(plane1,bx,by,bz) _, rmsErr_a, _ = planeFit(plane_a) if count == 1: if rmsErr_a > latest_rms: plusMinus *= -1.0 bx += (plusMinus * angleIncrement) if rmsErr_a > latest_rms and count > 1: dontStop = False else: latest_rms = rmsErr_a optimal_bx = bx if doPrint: print(" RMS_X = " + str(round(latest_rms,4))) count += 1 plane_a = reCalcPlanePoints(plane2,bx,by,bz) _, rmsErr_a, _ = planeFit(plane_a) return round(optimal_bx,3), round(optimal_by,3), rmsErr_a, latest_rms def tightenPlanes2(planes,angleIncrement,bx,by,bz,iteration,doPrint=False): optimal_bx = 0.0 optimal_by = 0.0 plusMinus = 1.0 latest_rms = planesRMS(planes,bx,by,bz,False) if doPrint: print("\n Optimizing the boresight X parameter (" + str(iteration) + ")") else: print("\n Optimizing the boresight X and Y parameters (" + str(iteration) + ")") count = 1 dontStop = True while dontStop: bx += (plusMinus * angleIncrement) rmsErr = planesRMS(planes,bx,by,bz,doPrint) if count == 1: if rmsErr > latest_rms: plusMinus *= -1.0 bx += (plusMinus * angleIncrement) if rmsErr > latest_rms and count > 1: dontStop = False bx = optimal_bx else: latest_rms = rmsErr optimal_bx = bx if doPrint: print(" RMS_X = " + str(round(latest_rms,4))) count += 1 if doPrint: print("\n Optimizing the boresight Y parameter...") count = 1 dontStop = True while dontStop: by += (plusMinus * angleIncrement) rmsErr = planesRMS(planes,bx,by,bz,doPrint) if count == 1: if rmsErr > latest_rms: plusMinus *= -1.0 by += (plusMinus * angleIncrement) if rmsErr > latest_rms and count > 1: dontStop = False else: latest_rms = rmsErr optimal_by = by if doPrint: print(" RMS_Y = " + str(round(latest_rms,4))) count += 1 return round(optimal_bx,3), round(optimal_by,3), latest_rms def tightenPlanes3(cuda_data,angleIncrement,bx,by,bz,iteration,doPrint=False): optimal_bx = 0.0 optimal_by = 0.0 optimal_bz = 0.0 plusMinus = 1.0 latest_rms = planesRMS(cuda_data,bx,by,bz,False) if doPrint: print("\n Optimizing the boresight Z parameter (" + str(iteration) + ")") count = 1 dontStop = True while dontStop: bz += (plusMinus * angleIncrement) rmsErr = planesRMS(cuda_data,bx,by,bz,doPrint) if count == 1: if rmsErr > latest_rms: plusMinus *= -1.0 bz += (plusMinus * angleIncrement) if rmsErr > latest_rms and count > 1: dontStop = False else: latest_rms = rmsErr optimal_bz = bz if doPrint: print(" RMS_Z = " + str(round(latest_rms,4))) count += 1 if doPrint: print("\n Optimizing the boresight Y parameter...") count = 1 dontStop = True while dontStop: by += (plusMinus * angleIncrement) rmsErr = planesRMS(cuda_data,bx,by,bz,doPrint) if count == 1: if rmsErr > latest_rms: plusMinus *= -1.0 by += (plusMinus * angleIncrement) if rmsErr > latest_rms and count > 1: dontStop = False else: latest_rms = rmsErr optimal_by = by if doPrint: print(" RMS_Y = " + str(round(latest_rms,4))) count += 1 if doPrint: print("\n Optimizing the boresight X parameter...") count = 1 dontStop = True while dontStop: bx += (plusMinus * angleIncrement) rmsErr = planesRMS(cuda_data,bx,by,bz,doPrint) if count == 1: if rmsErr > latest_rms: plusMinus *= -1.0 bx += (plusMinus * angleIncrement) if rmsErr > latest_rms and count > 1: dontStop = False bx = optimal_bx else: latest_rms = rmsErr optimal_bx = bx if doPrint: print(" RMS_X = " + str(round(latest_rms,4))) count += 1 return round(optimal_bx,3), round(optimal_by,3), round(optimal_bz,3), latest_rms def coincidentPlanesRMS(planeNormalA, planeCentroidA, planeNormalB, planeCentroidB): S = planeCentroidB - planeCentroidA dA = np.dot(S,planeNormalA) dB = np.dot(S,planeNormalB) rms = np.sqrt(((np.abs(np.asscalar(dA))+np.abs(np.asscalar(dB)))/2.)**2) return rms def convergePlanesRMS(planes,matches,bx,by,bz,doPrint): if doPrint: print() newPlanes = [] for i in range(0,len(planes)): newPlanes.append(reCalcPlanePoints(planes[i],bx,by,bz)) initialRMSvalues = [] completed = 0 for i in range(0,len(matches)): if i > 0: completed += len(matches[i-1]) for j in range(0,len(matches[i])-1): planeA = newPlanes[int(matches[i][j])-1] planeNormalA, _, planeCentroidA = planeFit(planeA) for k in range(j+1,len(matches[i])): planeB = newPlanes[int(matches[i][k])-1] planeNormalB, _, planeCentroidB = planeFit(planeB) rmsErr = coincidentPlanesRMS(planeNormalA, planeCentroidA, planeNormalB, planeCentroidB) initialRMSvalues.append(rmsErr**2) if doPrint: print(" " + str(completed+j+1) + "-" + str(completed+k+1) + " dist : " + str(round(rmsErr,4))) rms = np.sqrt(np.average(np.asarray(initialRMSvalues))) if doPrint: print() return rms def convergePlanesX(planes,matches,angleIncrement,bx,by,bz,iteration,doPrint=False): optimal_bx = 0.0 plusMinus = 1.0 latest_rms = convergePlanesRMS(planes,matches,bx,by,bz,False) count = 1 dontStop = True print("\n Optimizing the boresight X parameter (" + str(iteration) + ")") while dontStop: bx += (plusMinus * angleIncrement) rmsErr = convergePlanesRMS(planes,matches,bx,by,bz,False) if count == 1: if rmsErr > latest_rms: plusMinus *= -1.0 bx += (plusMinus * angleIncrement) if rmsErr > latest_rms and count > 1: dontStop = False else: latest_rms = rmsErr optimal_bx = bx if doPrint: print(" RMS_X = " + str(round(latest_rms,4))) count += 1 return round(optimal_bx,4), round(latest_rms,3) def convergePlanesY(planes,matches,angleIncrement,bx,by,bz,iteration,doPrint=False): optimal_by = 0.0 plusMinus = 1.0 latest_rms = convergePlanesRMS(planes,matches,bx,by,bz,False) count = 1 dontStop = True print("\n Optimizing the boresight Y parameter (" + str(iteration) + ")") while dontStop: by += (plusMinus * angleIncrement) rmsErr = convergePlanesRMS(planes,matches,bx,by,bz,False) if count == 1: if rmsErr > latest_rms: plusMinus *= -1.0 by += (plusMinus * angleIncrement) if rmsErr > latest_rms and count > 1: dontStop = False else: latest_rms = rmsErr optimal_by = by if doPrint: print(" RMS_Y = " + str(round(latest_rms,4))) count += 1 return round(optimal_by,4), round(latest_rms,3) def convergePlanesZ(planes,matches,angleIncrement,bx,by,bz,iteration,doPrint=False): optimal_bz = 0.0 plusMinus = 1.0 latest_rms = convergePlanesRMS(planes,matches,bx,by,bz,False) count = 1 dontStop = True print("\n Optimizing the boresight Z parameter (" + str(iteration) + ")") while dontStop: bz += (plusMinus * angleIncrement) rmsErr = convergePlanesRMS(planes,matches,bx,by,bz,False) if count == 1: if rmsErr > latest_rms: plusMinus *= -1.0 bz += (plusMinus * angleIncrement) if rmsErr > latest_rms and count > 1: dontStop = False else: latest_rms = rmsErr optimal_bz = bz if doPrint: print(" RMS_Z = " + str(round(latest_rms,4))) count += 1 return round(optimal_bz,4), round(latest_rms,3) def planesTightAndDirectionRMS(planes,matches,bx,by,bz): initialTightRMSvalues = [] newPlanes = [] for i in range(0,len(planes)): newPlane = reCalcPlanePoints(planes[i],bx,by,bz) newPlanes.append(newPlane) _,rmsErr,_ = planeFit(newPlane) initialTightRMSvalues.append(rmsErr**2) tightRMS = np.sqrt(np.average(np.asarray(initialTightRMSvalues))) initialRMSvalues = [] for i in range(0,len(matches)): for j in range(0,len(matches[i])-1): planeA = newPlanes[int(matches[i][j])-1] planeNormalA, _, planeCentroidA = planeFit(planeA) for k in range(j+1,len(matches[i])): planeB = newPlanes[int(matches[i][k])-1] planeNormalB, _, planeCentroidB = planeFit(planeB) rmsErr = np.degrees(np.arccos(np.dot(planeNormalA,planeNormalB)/(np.linalg.norm(planeNormalA) * np.linalg.norm(planeNormalB)))) rmsErr2 = np.degrees(np.arccos(np.dot(-planeNormalA,planeNormalB)/(np.linalg.norm(planeNormalA) * np.linalg.norm(planeNormalB)))) if rmsErr2 < rmsErr: rmsErr = rmsErr2 initialRMSvalues.append(rmsErr**2) directionRMS = np.sqrt(np.average(np.asarray(initialRMSvalues))) return tightRMS, directionRMS def corrFromCov(Cov): Corr = np.array(Cov) for ii in range(0, len(Cov)): for jj in range(0, len(Cov)): Corr[ii][jj] = Cov[ii][jj]/(np.sqrt(Cov[ii][ii])*np.sqrt(Cov[jj][jj])) Corr[jj][ii] = Cov[jj][ii]/(np.sqrt(Cov[ii][ii])*np.sqrt(Cov[jj][jj])) return Corr def formatLine(message,lineLen,ending): newStr = message num = lineLen - len(message) if num > 0: for i in range(0,num-1): newStr += " " newStr += ending return newStr def iHeartLidar1(): print(" XXX XXX") time.sleep(0.1) print(" IIIII X XX XX X L DDDD AAAA RRRRR") time.sleep(0.1) print(" I X X X L i D D A A R R") time.sleep(0.1) print(" I X X L D D A A R R") time.sleep(0.1) print(" I XX XX L i D D AAAAAA RRRRR") time.sleep(0.1) print(" I XX XX L i D D A A R R") time.sleep(0.1) print(" I XX XX L i D D A A R R") time.sleep(0.1) print(" IIIII XXX LLLLLL i DDDD A A R R") time.sleep(0.1) print(" X\n\n") def iHeartLidar2(logFile): logFile.write(" XXX XXX\n") logFile.write(" IIIII X XX XX X L DDDD AAAA RRRRR\n") logFile.write(" I X X X L i D D A A R R\n") logFile.write(" I X X L D D A A R R\n") logFile.write(" I XX XX L i D D AAAAAA RRRRR\n") logFile.write(" I XX XX L i D D A A R R\n") logFile.write(" I XX XX L i D D A A R R\n") logFile.write(" IIIII XXX LLLLLL i DDDD A A R R\n") logFile.write(" X\n\n\n") def OpenMMS1(): print("\n _________ _______ _____ ______ ______ ________") time.sleep(0.1) print("|\\ ___ \\ ________ ________ ________ |\\ __ \\ __ \\|\\ __ \\ __ \\|\\ ____\\") time.sleep(0.1) print("\\|\\ \\_|\\ \\|\\ __ \\|\\ __ \\|\\ ___ \\|\\ \\|\\ \\|\\ \\|\\ \\|\\ \\|\\ \\|\\ \\___|_") time.sleep(0.1) print(" \\|\\ \\\\|\\ \\|\\ \\|\\ \\|\\ \\|\\ \\|\\ \\\\|\\ \\|\\ \\|\\__\\|\\ \\|\\ \\|\\__\\|\\ \\|\\_____ \\") time.sleep(0.1) print(" \\|\\ \\\\|\\ \\|\\ ____\\|\\ ____\\|\\ \\\\|\\ \\|\\ \\|__|\\|\\ \\|\\ \\|__|\\|\\ \\|____|\\ \\") time.sleep(0.1) print(" \\|\\ \\\\_\\ \\|\\ \\___|\\|\\ \\___|\\|\\ \\\\|\\ \\|\\ \\ \\|\\ \\|\\ \\ \\|\\ \\ __\\_\\ \\") time.sleep(0.1) print(" \\|\\________\\|\\__\\ \\|\\______\\\\|\\__\\\\|\\__\\|\\__\\ \\|\\__\\|\\__\\ \\|\\__\\|\\_______\\") time.sleep(0.1) print(" \\|________|\\|___| \\|_______|\\|___|\\|__|\\|___| \\|__|\\|___| \\|__|\\|________|\n\n") def OpenMMS2(logFile): logFile.write("\n _________ _______ _____ ______ ______ ________\n") logFile.write("|\\ ___ \\ ________ ________ ________ |\\ __ \\ __ \\|\\ __ \\ __ \\|\\ ____\\\n") logFile.write("\\|\\ \\_|\\ \\|\\ __ \\|\\ __ \\|\\ ___ \\|\\ \\|\\ \\|\\ \\|\\ \\|\\ \\|\\ \\|\\ \\___|_\n") logFile.write(" \\|\\ \\\\|\\ \\|\\ \\|\\ \\|\\ \\|\\ \\|\\ \\\\|\\ \\|\\ \\|\\__\\|\\ \\|\\ \\|\\__\\|\\ \\|\\_____ \\\n") logFile.write(" \\|\\ \\\\|\\ \\|\\ ____\\|\\ ____\\|\\ \\\\|\\ \\|\\ \\|__|\\|\\ \\|\\ \\|__|\\|\\ \\|____|\\ \\\n") logFile.write(" \\|\\ \\\\_\\ \\|\\ \\___|\\|\\ \\___|\\|\\ \\\\|\\ \\|\\ \\ \\|\\ \\|\\ \\ \\|\\ \\ __\\_\\ \\\n") logFile.write(" \\|\\________\\|\\__\\ \\|\\______\\\\|\\__\\\\|\\__\\|\\__\\ \\|\\__\\|\\__\\ \\|\\__\\|\\_______\\\n") logFile.write(" \\|________|\\|___| \\|_______|\\|___|\\|__|\\|___| \\|__|\\|___| \\|__|\\|________|\n\n\n") def main(planeFiles,logFile): cuda.select_device(0) cudalinalg.init() curDir = Path(planeFiles[0][0]).parents[0] startProcessing = time.time() #initial boresight values [degrees] Bx = 0.0 By = 0.0 Bz = 0.0 boresights = np.array((np.radians(Bx),np.radians(By),np.radians(Bz))) print(" Reading in point observations from " + str(len(planeFiles)) + " different planes") logFile.write(" Reading in point observations from " + str(len(planeFiles)) + " different planes\n") totalPts, planes = readPlaneFiles(curDir, planeFiles) print(" Analyzing a total of " + str(totalPts) + " point observations\n") logFile.write(" Analyzing a total of " + str(totalPts) + " point observations\n\n") rmsValuesInit = planesRMS3(planes) cuda_data = [] for i in range(0,len(planes)): plane = planes[i] a1 = np.ascontiguousarray(plane[:,3],dtype=np.float32) a2 = np.ascontiguousarray(plane[:,4],dtype=np.float32) a3 = np.ascontiguousarray(plane[:,5],dtype=np.float32) a4 = np.ascontiguousarray(np.radians(plane[:,6]),dtype=np.float32) a5 = np.ascontiguousarray(np.radians(plane[:,7]),dtype=np.float32) a6 = np.ascontiguousarray(np.radians(plane[:,8]),dtype=np.float32) a7 = np.ascontiguousarray(np.radians(plane[:,12]),dtype=np.float32) a8 = np.ascontiguousarray(np.radians(plane[:,13]),dtype=np.float32) a9 = np.ascontiguousarray(plane[:,9],dtype=np.float64) a10 = np.ascontiguousarray(plane[:,10],dtype=np.float64) a11 = np.ascontiguousarray(plane[:,11],dtype=np.float64) cuda_data.append([a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,a11]) converged = False for itr in range(0,1): print("************************* Livox Lidar Boresight Calibration *************************\n") logFile.write("************************* Livox Lidar Boresight Calibration *************************\n\n") n = 16 converged = False l = None s0_prev = 1e10 apriori = 1.0 / (0.05 ** 2) for j in range(0,10): if j == 1: n = 10 print(" ___________________________________________________________________") print(" Iteration " + str(j+1) + ":") print(" Computing misclosure vector") l = -planesRMSi2(cuda_data,boresights,False) print(" Computing Jacobian matrix (please wait...)") fun = lambda boresights: (planesRMSi2(cuda_data,boresights,False)) Jfun = nd.Jacobian(fun,step=0.0001,step_nom=1,order=n) fullJac = Jfun(boresights) print(" Computing least-squares estimate") cor = np.linalg.lstsq(fullJac, l,rcond=None)[0] boresights[0] += cor[0] boresights[1] += cor[1] boresights[2] += cor[2] print("\n New lidar boresight estimates") print(" Bx = " + str(np.round(np.degrees(boresights[0]),3)) + " degs") print(" By = " + str(np.round(np.degrees(boresights[1]),3)) + " degs") print(" Bz = " + str(np.round(np.degrees(boresights[2]),3)) + " degs") s02 = (np.dot((apriori * l.T),l)).item() / (len(l) - 3) s0 = np.sqrt(s02) if np.abs(s0 - s0_prev) <= 0.01: converged = True else: print("\n s0 = " + str(np.round(s0,4))) s0_prev = s0 endProcessing = time.time() timeE = endProcessing-startProcessing print("\n Elapsed Time: " + str(round((timeE)/60.,2)) + " mins\n") logFile.write(" ___________________________________________________________________\n") logFile.write(" Iteration " + str(j+1) + ":\n") logFile.write(" Computing misclosure vector\n") logFile.write(" Computing Jacobian matrix (please wait...)\n") logFile.write(" Computing least-squares estimate\n") logFile.write("\n New lidar boresight estimates\n") logFile.write(" Bx = " + str(np.round(np.degrees(boresights[0]),3)) + " degs\n") logFile.write(" By = " + str(np.round(np.degrees(boresights[1]),3)) + " degs\n") logFile.write(" Bz = " + str(np.round(np.degrees(boresights[2]),3)) + " degs\n") if np.abs(s0 - s0_prev) <= 0.01: pass else: logFile.write("\n s0 = " + str(np.round(s0,4)) + "\n") logFile.write("\n Elapsed Time: " + str(round((timeE)/60.,2)) + " mins\n\n") if converged: break if converged: N = np.dot(np.transpose(apriori*fullJac), fullJac) Cov = s02 * np.linalg.inv(N) rmsValues, rms_avg, rms_w_avg = planesRMS4(cuda_data,boresights,planeFiles,False,timeStr) std = [np.degrees(np.sqrt(Cov[ii][ii])) for ii in range(0, len(fullJac[0,:]))] Corr = corrFromCov(Cov) boreFile = open(curDir / ("boresight_params_" + timeStr + ".bor"),"w") boreFile.write(str(np.round(np.degrees(boresights[0]),3)) + "\n") boreFile.write(str(np.round(np.degrees(boresights[1]),3)) + "\n") boreFile.write(str(np.round(np.degrees(boresights[2]),3)) + "\n") boreFile.close() lineLength = 74 print(" ********** LIDAR BORESIGHT CALIBRATION CONVERGED **********") print(formatLine(" *",lineLength,"*")) print(formatLine(" * Bx: " + str(np.round(np.degrees(boresights[0]),3)) + " deg",lineLength,"*")) print(formatLine(" * By: " + str(np.round(np.degrees(boresights[1]),3)) + " deg",lineLength,"*")) print(formatLine(" * Bz: " + str(np.round(np.degrees(boresights[2]),3)) + " deg",lineLength,"*")) print(formatLine(" *",lineLength,"*")) print(formatLine(" * Std.Dev of Unit Weight (s0): " + str(np.round(np.sqrt(s02),4)),lineLength,"*")) print(formatLine(" * (NOTE: Obs. accuracy assumed = 0.05 m)",lineLength,"*")) print(formatLine(" *",lineLength,"*")) print(formatLine(" * Std.Devs:",lineLength,"*")) print(formatLine(" * s_Bx: +/- " + str(np.round(np.degrees(std[0]),3)) + " deg",lineLength,"*")) print(formatLine(" * s_By: +/- " + str(np.round(np.degrees(std[1]),3)) + " deg",lineLength,"*")) print(formatLine(" * s_Bz: +/- " + str(np.round(np.degrees(std[2]),3)) + " deg",lineLength,"*")) print(formatLine(" *",lineLength,"*")) print(formatLine(" * Correlations:",lineLength,"*")) print(formatLine(" * Bx <--> By: " + str(np.round(Corr[0,1],2)),lineLength,"*")) print(formatLine(" * Bx <--> Bz: " + str(np.round(Corr[0,2],2)),lineLength,"*")) print(formatLine(" * By <--> Bz: " + str(np.round(Corr[1,2],2)),lineLength,"*")) print(formatLine(" *",lineLength,"*")) print(formatLine(" * Average RMS: " + str(np.round(rms_avg,3)) + " m",lineLength,"*")) print(formatLine(" * Weighted Average RMS: " + str(np.round(rms_w_avg,3)) + " m",lineLength,"*")) print(formatLine(" *",lineLength,"*")) print(formatLine(" * RMS for each plane:",lineLength,"*")) print(formatLine(" * Plane: Initial Adjusted",lineLength,"*")) logFile.write(" ********** LIDAR BORESIGHT CALIBRATION CONVERGED **********" + "\n") logFile.write(formatLine(" *",lineLength,"*") + "\n") logFile.write(formatLine(" * Bx: " + str(np.round(np.degrees(boresights[0]),3)) + " deg",lineLength,"*") + "\n") logFile.write(formatLine(" * By: " + str(np.round(np.degrees(boresights[1]),3)) + " deg",lineLength,"*") + "\n") logFile.write(formatLine(" * Bz: " + str(np.round(np.degrees(boresights[2]),3)) + " deg",lineLength,"*") + "\n") logFile.write(formatLine(" *",lineLength,"*") + "\n") logFile.write(formatLine(" * Std.Dev of Unit Weight (s0): " + str(np.round(np.sqrt(s02),4)),lineLength,"*") + "\n") logFile.write(formatLine(" * (NOTE: Obs. accuracy assumed = 0.05 m)",lineLength,"*") + "\n") logFile.write(formatLine(" *",lineLength,"*") + "\n") logFile.write(formatLine(" * Std.Devs:",lineLength,"*") + "\n") logFile.write(formatLine(" * s_Bx: +/- " + str(np.round(np.degrees(std[0]),3)) + " deg",lineLength,"*") + "\n") logFile.write(formatLine(" * s_By: +/- " + str(np.round(np.degrees(std[1]),3)) + " deg",lineLength,"*") + "\n") logFile.write(formatLine(" * s_Bz: +/- " + str(np.round(np.degrees(std[2]),3)) + " deg",lineLength,"*") + "\n") logFile.write(formatLine(" *",lineLength,"*") + "\n") logFile.write(formatLine(" * Correlations:",lineLength,"*") + "\n") logFile.write(formatLine(" * Bx <--> By: " + str(np.round(Corr[0,1],2)),lineLength,"*") + "\n") logFile.write(formatLine(" * Bx <--> Bz: " + str(np.round(Corr[0,2],2)),lineLength,"*") + "\n") logFile.write(formatLine(" * By <--> Bz: " + str(np.round(Corr[1,2],2)),lineLength,"*") + "\n") logFile.write(formatLine(" *",lineLength,"*") + "\n") logFile.write(formatLine(" * Average RMS: " + str(np.round(rms_avg,3)) + " m",lineLength,"*") + "\n") logFile.write(formatLine(" * Weighted Average RMS: " + str(np.round(rms_w_avg,3)) + " m",lineLength,"*") + "\n") logFile.write(formatLine(" *",lineLength,"*") + "\n") logFile.write(formatLine(" * RMS for each plane:",lineLength,"*") + "\n") logFile.write(formatLine(" * Plane: Initial Adjusted",lineLength,"*") + "\n") for k in range(0,len(rmsValues)): print(formatLine(formatLine(" * " + str(Path(planeFiles[k][0]).name),39,"") + formatLine(str(np.round(rmsValuesInit[k],3)) + " m",13,"") + str(np.round(rmsValues[k],3)) + " m" ,lineLength,"*")) logFile.write(formatLine(formatLine(" * " + str(Path(planeFiles[k][0]).name),39,"") + formatLine(str(np.round(rmsValuesInit[k],3)) + " m",13,"") + str(np.round(rmsValues[k],3)) + " m" ,lineLength,"*") + "\n") print(" *************************************************************\n\n") logFile.write(" *************************************************************\n\n\n") else: print(" %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%") print(" %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%") print(" %% LIDAR BORESIGHT CALIBRATION DID NOT CONVERGE AFTER 10 ITERATIONS %%") print(" %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%") print(" %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\n") logFile.write(" %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" + "\n") logFile.write(" %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" + "\n") logFile.write(" %% LIDAR BORESIGHT CALIBRATION DID NOT CONVERGE AFTER 10 ITERATIONS %%" + "\n") logFile.write(" %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" + "\n") logFile.write(" %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\n" + "\n") ##### command line start if __name__ == "__main__": print("\nLivox Lidar Sensor Calibration Started...") OpenMMS1() plane_files = "" curPath = Path(sys.argv[1]) timeNow = time.localtime() timeStr = str(timeNow[0]) + "_" + str(timeNow[1]) + "_" + str(timeNow[2]) + "_" + str(timeNow[3]) + "_" + str(timeNow[4]) + "_" + str(timeNow[5]) logFilename = curPath / ("LIDAR_CALIBRATION_" + timeStr + ".log") print(" LOG FILE: " + logFilename.name + "\n") logFile = open(logFilename,'w') OpenMMS2(logFile) curFiles = [] for (dirpath, dirnames, filenames) in os.walk(str(curPath)): if str(curPath) == dirpath: curFiles = filenames for i in range(0,len(curFiles)): filename = curPath / str(curFiles[i]) extension = filename.suffix if extension.upper() == ".TXT": checkFile = open(filename,"r") firstLine = checkFile.readline() checkFile.close() if firstLine.startswith("//X,Y,Z,scanner_lon,scanner_lat,scanner_z,scanner_y,scanner_x,heading,pitch,roll,vert_socs,azi_socs,dist_socs"): plane_files += str(curPath / filename) + "," plane_files = plane_files[:-1] plane_files = plane_files.split(",") planeFiles = [] for i in range(0,len(plane_files)): fileAsList = [plane_files[i]] planeFiles.append(fileAsList) if len(plane_files) > 0: main(planeFiles,logFile) iHeartLidar1() iHeartLidar2(logFile) else: print("\n\n************ NO VALID PLANE .TXT FILES WERE FOUND IN THE CURRENT DIRECTORY ***********\n\n") logFile.close()