2022 IB Diploma Extended Essays

path+="prepared/" f = open(path+'lidar_pose.txt', 'w') denormalize(yaw_pose) printToFile2(f,t_pose,x_pose, y_pose, yaw_pose) plt.show() Error calculation import matplotlib as mpl import matplotlib.pyplot as plt import numpy as np import rospkg from scipy.interpolate import interp1d from numpy import random, mean, var, std from math import * def prepareLatice(x, xnew): start = 0 end = 0 for i in range(len(xnew) - 1): if xnew[i] <= x[0]: start += 1 if x[len(x)-1] <= xnew[i]: end += 1

print(end.__str__() + ' ' + start.__str__() +"/n") return_x= xnew[(start + 1):(len(xnew) - end - 1)] return return_x # get an instance of RosPack with the default search paths rospack = rospkg.RosPack() # get the file path for sensor_fusion rospack.get_path('sensor_fusion') path = rospack.get_path('sensor_fusion') + '/dataTxt/laps/prepared/' data_lidar = np.loadtxt(path + "lidar_pose.txt", delimiter=' ', dtype=np.float) data_orb = np.loadtxt(path + "orb_pose.txt", delimiter=' ', dtype=np.float) data_zed = np.loadtxt(path + "zed_pose.txt", delimiter=' ', dtype=np.float) data_gyro = np.loadtxt(path + "gyro_yaw.txt", delimiter=' ', dtype=np.float) data_mag = np.loadtxt(path + "mag_yaw.txt", delimiter=' ', dtype=np.float) k=0 for i in range(len(data_lidar[:, 0])): if data_lidar[i, 0]<=1512232502.018991232: k+=1 e=0 for i in range(len(data_lidar[:, 0])): if data_lidar[i, 0]>=1512232712.210900068: e+=1 e = (len(data_lidar[:, 0]) - e - 20) k+=200