fix a minor bug for writting estimation results to files

Log/plot.py

@@ -1,46 +0,0 @@
-# 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

@@ -1,125 +0,0 @@
-# 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

@@ -1,178 +0,0 @@
-# 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

@@ -1,38 +0,0 @@
-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()

src/laserMapping.cpp

@@ -1040,11 +1040,13 @@ int main(int argc, char** argv)
                 {
                     if (!use_imu_as_input)
                     {
+                        euler_cur = SO3ToEuler(kf_output.x_.rot);
                         fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << kf_output.x_.pos.transpose() << " " << kf_output.x_.vel.transpose() \
                         <<" "<<kf_output.x_.omg.transpose()<<" "<<kf_output.x_.acc.transpose()<<" "<<kf_output.x_.gravity.transpose()<<" "<<kf_output.x_.bg.transpose()<<" "<<kf_output.x_.ba.transpose()<<" "<<feats_undistort->points.size()<<endl;
                     }
                     else
                     {
+                        euler_cur = SO3ToEuler(kf_input.x_.rot);
                         fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << kf_input.x_.pos.transpose() << " " << kf_input.x_.vel.transpose() \
                         <<" "<<kf_input.x_.bg.transpose()<<" "<<kf_input.x_.ba.transpose()<<" "<<kf_input.x_.gravity.transpose()<<" "<<feats_undistort->points.size()<<endl;
                     }

src/parameters.cpp

@@ -42,7 +42,7 @@ bool cut_frame_init = false; // true;
 
 MeasureGroup Measures;
 
-ofstream fout_out, fout_imu_pbp, fout_rtk;
+ofstream fout_out, fout_imu_pbp;
 
 void readParameters(ros::NodeHandle &nh)
 {

src/parameters.h

@@ -75,7 +75,7 @@ extern double time_update_last, time_current, time_predict_last_const, t_last;
 
 extern MeasureGroup Measures;
 
-extern ofstream fout_out, fout_imu_pbp, fout_rtk;
+extern ofstream fout_out, fout_imu_pbp;
 void readParameters(ros::NodeHandle &n);
 void open_file();
 Eigen::Matrix<double, 3, 1> SO3ToEuler(const SO3 &orient);