point-lio with grid map

Log/guide.md

@@ -0,0 +1 @@
+Here saved the debug records which can be drew by the ../log/plot.py. The record function can be found frm the MACRO: DEBUG_FILE_DIR(name) in common_lib.h.

Log/plot.py

@@ -0,0 +1,46 @@
+# import matplotlib
+# matplotlib.use('Agg')
+import numpy as np
+import matplotlib.pyplot as plt
+
+a_out=np.loadtxt('mat_out.txt')
+#######for normal#######
+fig, axs = plt.subplots(3,2)
+lab_out = ['', 'out-x', 'out-y', 'out-z']
+plot_ind = range(7,10)
+time=a_out[:,0]
+axs[0,0].set_title('Attitude')
+axs[1,0].set_title('Translation')
+axs[2,0].set_title('Velocity')
+axs[0,1].set_title('bg')
+axs[1,1].set_title('ba')
+axs[2,1].set_title('Gravity')
+for i in range(1,4):
+	for j in range(6):
+	    axs[j%3, j//3].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i])
+	for j in range(6):
+		axs[j%3, j//3].grid()
+		axs[j%3, j//3].legend()
+plt.grid()
+    #######for normal#######
+
+
+#### Draw IMU data
+#fig, axs = plt.subplots(2)
+#imu=np.loadtxt('imu_pbp.txt')
+#time=imu[:,0]
+#axs[0].set_title('Gyroscope')
+#axs[1].set_title('Accelerameter')
+#lab_1 = ['gyr-x', 'gyr-y', 'gyr-z']
+#lab_2 = ['acc-x', 'acc-y', 'acc-z']
+#for i in range(3):
+    #if i==1:
+#    	axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i])
+#    	axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i])
+#for i in range(2):
+    #axs[i].set_xlim(386,389)
+#    axs[i].grid()
+#    axs[i].legend()
+#plt.grid()
+
+plt.show()

Log/plot_imu.py

@@ -0,0 +1,125 @@
+# import matplotlib
+# matplotlib.use('Agg')
+import numpy as np
+import matplotlib.pyplot as plt
+
+#### Draw IMU data
+fig, axs = plt.subplots(2)
+imu=np.loadtxt('imu_pbp.txt')
+time=imu[:,0]
+axs[0].set_title('Gyroscope')
+axs[1].set_title('Accelerameter')
+lab_1 = ['gyr-x', 'gyr-y', 'gyr-z']
+lab_2 = ['acc-x', 'acc-y', 'acc-z']
+for i in range(3):
+    #if i==1:
+    	axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i])
+    	axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i])
+for i in range(2):
+    #axs[i].set_xlim(386,389)
+    axs[i].grid()
+    axs[i].legend()
+plt.grid()
+
+#fig, axs = plt.subplots(5)
+#axs[0].set_title('miss')
+#axs[1].set_title('miss')
+#axs[2].set_title('miss')
+#axs[3].set_title('miss')
+#axs[4].set_title('miss')
+#len_time1 = np.arange(0,1977)
+#len_time2 = np.arange(1977, 3954)
+#len_time3 = np.arange(3954,5931)
+#len_time4 = np.arange(5931,7908)
+#len_time5 = np.arange(7908,9885)
+    #if i==1:
+#axs[0].plot(len_time1, time[0:1977],'.-', label='check')
+#axs[1].plot(len_time2, time[1977:3954],'.-', label='check')
+#axs[2].plot(len_time3, time[3954:5931],'.-', label='check')
+#axs[3].plot(len_time4, time[5931:7908],'.-', label='check')
+#axs[4].plot(len_time5, time[7908:9885],'.-', label='check')
+    #axs[i].set_xlim(386,389)
+#axs[0].grid()
+#axs[0].legend()
+#axs[1].grid()
+#axs[1].legend()
+#axs[2].grid()
+#axs[2].legend()
+#axs[3].grid()
+#axs[3].legend()
+#axs[4].grid()
+#axs[4].legend()
+#plt.grid()
+
+#fig, axs = plt.subplots(5)
+#axs[0].set_title('miss')
+#axs[1].set_title('miss')
+#axs[2].set_title('miss')
+#axs[3].set_title('miss')
+#axs[4].set_title('miss')
+#len_time1 = np.arange(9885,9885+1977)
+#len_time2 = np.arange(9885+1977,9885+3954)
+#len_time3 = np.arange(9885+3954,9885+5931)
+#len_time4 = np.arange(9885+5931,9885+7908)
+#len_time5 = np.arange(9885+7908,9885+9885)
+    #if i==1:
+#axs[0].plot(len_time1, time[9885+0:9885+1977],'.-', label='check')
+#axs[1].plot(len_time2, time[9885+1977:9885+3954],'.-', label='check')
+#axs[2].plot(len_time3, time[9885+3954:9885+5931],'.-', label='check')
+#axs[3].plot(len_time4, time[9885+5931:9885+7908],'.-', label='check')
+#axs[4].plot(len_time5, time[9885+7908:9885+9885],'.-', label='check')
+    #axs[i].set_xlim(386,389)
+#axs[0].grid()
+#axs[0].legend()
+#axs[1].grid()
+#axs[1].legend()
+#axs[2].grid()
+#axs[2].legend()
+#axs[3].grid()
+#axs[3].legend()
+#axs[4].grid()
+#axs[4].legend()
+#plt.grid()
+
+# #### Draw time calculation
+# plt.figure(3)
+# fig = plt.figure()
+# font1 = {'family' : 'Times New Roman',
+# 'weight' : 'normal',
+# 'size'   : 12,
+# }
+# c="red"
+# a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt')
+# a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt')
+# a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt')
+# # n = a_out[:,1].size
+# # time_mean = a_out[:,1].mean()
+# # time_se   = a_out[:,1].std() / np.sqrt(n)
+# # time_err  = a_out[:,1] - time_mean
+# # feat_mean = a_out[:,2].mean()
+# # feat_err  = a_out[:,2] - feat_mean
+# # feat_se   = a_out[:,2].std() / np.sqrt(n)
+# ax1 = fig.add_subplot(111)
+# ax1.set_ylabel('Effective Feature Numbers',font1)
+# ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9])
+# ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9])
+# ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9])
+# ax1.set_ylim([0, 3000])
+
+# ax2 = ax1.twinx()
+# ax2.spines['right'].set_color('red')
+# ax2.set_ylabel('Compute Time (ms)',font1)
+# ax2.yaxis.label.set_color('red')
+# ax2.tick_params(axis='y', colors='red')
+# ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
+# ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
+# ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
+# ax2.set_xlim([0.5, 3.5])
+# ax2.set_ylim([0, 100])
+
+# plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2'))
+# # # print(time_se)
+# # # print(a_out3[:,2])
+# plt.grid()
+# plt.savefig("time.pdf", dpi=1200)
+plt.show()

Log/plot_out.py

@@ -0,0 +1,178 @@
+# import matplotlib
+# matplotlib.use('Agg')
+import numpy as np
+import matplotlib.pyplot as plt
+
+# a_pre=np.loadtxt('mat_pre.txt')
+a_out=np.loadtxt('mat_out.txt')
+if((a_out.shape[1] != 19) & (a_out.shape[1] != 20)):
+    ######for ikfom
+    fig, axs = plt.subplots(4,2)
+    #lab_pre = ['', 'pre-x', 'pre-y', 'pre-z']
+    lab_out = ['', 'out-x', 'out-y', 'out-z']
+    plot_ind = range(7,10)
+    time=a_out[:,0]
+    axs[0,0].set_title('Attitude')
+    axs[1,0].set_title('Translation')
+    axs[2,0].set_title('Velocity')
+    axs[3,0].set_title('Angular velocity')
+    axs[0,1].set_title('Acceleration')
+    axs[1,1].set_title('Gravity')
+    axs[2,1].set_title('bg')
+    axs[3,1].set_title('ba')
+    for i in range(1,4):
+        for j in range(8):
+            #axs[j%4, j//4].plot(time, a_pre[:,i+j*3],'.-', label=lab_pre[i])
+            axs[j%4, j//4].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i])
+    for j in range(8):
+        # axs[j].set_xlim(386,389)
+        axs[j%4, j//4].grid()
+        axs[j%4, j//4].legend()
+    plt.grid()
+    ######for ikfom#######
+else:
+    #######for normal#######
+    fig, axs = plt.subplots(3,2)
+    lab_pre = ['', 'pre-x', 'pre-y', 'pre-z']
+    lab_out = ['', 'out-x', 'out-y', 'out-z']
+    plot_ind = range(7,10)
+    time=a_out[:,0]
+    time1 = a_pre[:,0]
+    axs[0,0].set_title('Attitude')
+    axs[1,0].set_title('Translation')
+    axs[2,0].set_title('Velocity')
+    axs[0,1].set_title('bg')
+    axs[1,1].set_title('ba')
+    axs[2,1].set_title('Gravity')
+    for i in range(1,4):
+        for j in range(6):
+            axs[j%3, j/3].plot(time1, a_pre[:,i+j*3],'.-', label=lab_pre[i])
+            axs[j%3, j/3].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i])
+    for j in range(6):
+        # axs[j].set_xlim(386,389)
+        axs[j%3, j//3].grid()
+        axs[j%3, j//3].legend()
+    plt.grid()
+    #######for normal#######
+
+
+#### Draw IMU data
+fig, axs = plt.subplots(2)
+imu=np.loadtxt('imu_pbp.txt')
+time=imu[:,0]
+axs[0].set_title('Gyroscope')
+axs[1].set_title('Accelerameter')
+lab_1 = ['gyr-x', 'gyr-y', 'gyr-z']
+lab_2 = ['acc-x', 'acc-y', 'acc-z']
+for i in range(3):
+    #if i==1:
+    	axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i])
+    	axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i])
+for i in range(2):
+    #axs[i].set_xlim(386,389)
+    axs[i].grid()
+    axs[i].legend()
+plt.grid()
+
+#fig, axs = plt.subplots(5)
+#axs[0].set_title('miss')
+#axs[1].set_title('miss')
+#axs[2].set_title('miss')
+#axs[3].set_title('miss')
+#axs[4].set_title('miss')
+#len_time1 = np.arange(0,1977)
+#len_time2 = np.arange(1977, 3954)
+#len_time3 = np.arange(3954,5931)
+#len_time4 = np.arange(5931,7908)
+#len_time5 = np.arange(7908,9885)
+    #if i==1:
+#axs[0].plot(len_time1, time[0:1977],'.-', label='check')
+#axs[1].plot(len_time2, time[1977:3954],'.-', label='check')
+#axs[2].plot(len_time3, time[3954:5931],'.-', label='check')
+#axs[3].plot(len_time4, time[5931:7908],'.-', label='check')
+#axs[4].plot(len_time5, time[7908:9885],'.-', label='check')
+    #axs[i].set_xlim(386,389)
+#axs[0].grid()
+#axs[0].legend()
+#axs[1].grid()
+#axs[1].legend()
+#axs[2].grid()
+#axs[2].legend()
+#axs[3].grid()
+#axs[3].legend()
+#axs[4].grid()
+#axs[4].legend()
+#plt.grid()
+
+#fig, axs = plt.subplots(5)
+#axs[0].set_title('miss')
+#axs[1].set_title('miss')
+#axs[2].set_title('miss')
+#axs[3].set_title('miss')
+#axs[4].set_title('miss')
+#len_time1 = np.arange(9885,9885+1977)
+#len_time2 = np.arange(9885+1977,9885+3954)
+#len_time3 = np.arange(9885+3954,9885+5931)
+#len_time4 = np.arange(9885+5931,9885+7908)
+#len_time5 = np.arange(9885+7908,9885+9885)
+    #if i==1:
+#axs[0].plot(len_time1, time[9885+0:9885+1977],'.-', label='check')
+#axs[1].plot(len_time2, time[9885+1977:9885+3954],'.-', label='check')
+#axs[2].plot(len_time3, time[9885+3954:9885+5931],'.-', label='check')
+#axs[3].plot(len_time4, time[9885+5931:9885+7908],'.-', label='check')
+#axs[4].plot(len_time5, time[9885+7908:9885+9885],'.-', label='check')
+    #axs[i].set_xlim(386,389)
+#axs[0].grid()
+#axs[0].legend()
+#axs[1].grid()
+#axs[1].legend()
+#axs[2].grid()
+#axs[2].legend()
+#axs[3].grid()
+#axs[3].legend()
+#axs[4].grid()
+#axs[4].legend()
+#plt.grid()
+
+# #### Draw time calculation
+# plt.figure(3)
+# fig = plt.figure()
+# font1 = {'family' : 'Times New Roman',
+# 'weight' : 'normal',
+# 'size'   : 12,
+# }
+# c="red"
+# a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt')
+# a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt')
+# a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt')
+# # n = a_out[:,1].size
+# # time_mean = a_out[:,1].mean()
+# # time_se   = a_out[:,1].std() / np.sqrt(n)
+# # time_err  = a_out[:,1] - time_mean
+# # feat_mean = a_out[:,2].mean()
+# # feat_err  = a_out[:,2] - feat_mean
+# # feat_se   = a_out[:,2].std() / np.sqrt(n)
+# ax1 = fig.add_subplot(111)
+# ax1.set_ylabel('Effective Feature Numbers',font1)
+# ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9])
+# ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9])
+# ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9])
+# ax1.set_ylim([0, 3000])
+
+# ax2 = ax1.twinx()
+# ax2.spines['right'].set_color('red')
+# ax2.set_ylabel('Compute Time (ms)',font1)
+# ax2.yaxis.label.set_color('red')
+# ax2.tick_params(axis='y', colors='red')
+# ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
+# ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
+# ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
+# ax2.set_xlim([0.5, 3.5])
+# ax2.set_ylim([0, 100])
+
+# plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2'))
+# # # print(time_se)
+# # # print(a_out3[:,2])
+# plt.grid()
+# plt.savefig("time.pdf", dpi=1200)
+plt.show()

Log/plot_rtk.py

@@ -0,0 +1,38 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+a_grdt=np.loadtxt('pos_rtk.txt')
+a_out=np.loadtxt('mat_out.txt')
+#######for normal#######
+fig, axs = plt.subplots(3,1)
+lab_grdt = ['', 'gt-x', 'gt-y', 'gt-z']
+lab_out = ['', 'out-x', 'out-y', 'out-z']
+plot_ind = range(7,10)
+time=a_out[:,0]
+time1 = a_grdt[:,0]
+axs[0].set_title('East [m]')
+axs[1].set_title('North [m]')
+axs[2].set_title('Up [m]')
+for i in range(1,4):
+    axs[i-1].plot(time1, a_grdt[:,i],'.-', label=lab_grdt[i])
+    axs[i-1].plot(time, a_out[:,i+3],'.-', label=lab_out[i])
+for j in range(1,4):
+    axs[j-1].grid()
+    axs[j-1].legend()
+plt.grid()
+#######for normal#######
+
+fig, axs = plt.subplots(2,1)
+axs[0].set_title('clock drift')
+lab_1 = ['dt-g', 'dt-r', 'dt-e', 'dt-c']
+for i in range(3):
+    #if i==1:
+    	axs[0].plot(time, a_out[:,i+13],'.-', label=lab_1[i])
+axs[0].plot(time, a_out[:,19],'.-', label=lab_1[3])
+for i in range(1):
+    #axs[i].set_xlim(386,389)
+    axs[i].grid()
+    axs[i].legend()
+plt.grid()
+
+plt.show()