add the switch for the online extrinsic estimation

Log/plot.py

@@ -33,22 +33,22 @@
 
 
 #### Draw IMU data
-fig, axs = plt.subplots(2)
-imu=np.loadtxt('imu.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(2)
+# imu=np.loadtxt('imu.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()
 
 # #### Draw time calculation
 # plt.figure(3)

README.md

@@ -6,6 +6,7 @@
 4. [R2LIVE](https://github.com/hku-mars/r2live): A high-precision LiDAR-inertial-Vision fusion work using FAST-LIO as LiDAR-inertial front-end.
 5. [UGV Demo](https://www.youtube.com/watch?v=wikgrQbE6Cs): Model Predictive Control for Trajectory Tracking on Differentiable Manifolds.
 6. [FAST-LIO-SLAM](https://github.com/gisbi-kim/FAST_LIO_SLAM): The integration of FAST-LIO with [Scan-Context](https://github.com/irapkaist/scancontext) **loop closure** module.
+7. [FAST-LIO-LOCALIZATION](https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION): The integration of FAST-LIO with **Re-localization** function module.
 
 ## FAST-LIO
 **FAST-LIO** (Fast LiDAR-Inertial Odometry) is a computationally efficient and robust LiDAR-inertial odometry package. It fuses LiDAR feature points with IMU data using a tightly-coupled iterated extended Kalman filter to allow robust navigation in fast-motion, noisy or cluttered environments where degeneration occurs. Our package address many key issues:

config/avia.yaml

@@ -15,12 +15,13 @@ mapping:
     b_gyr_cov: 0.0001
     fov_degree:    90
     det_range:     450.0
+    extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
     extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
     extrinsic_R: [ 1, 0, 0,
                    0, 1, 0,
                    0, 0, 1]
 
 publish:     
-    scan_publish_en:  1       # 'false' will close all the point cloud output
-    dense_publish_en: 1       # false will low down the points number in a global-frame point clouds scan.
-    scan_bodyframe_pub_en: 1  # output the point cloud scans in IMU-body-frame
+    scan_publish_en:  true       # false: close all the point cloud output
+    dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
+    scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame

config/horizon.yaml

@@ -15,12 +15,13 @@ mapping:
     b_gyr_cov: 0.0001
     fov_degree:    100
     det_range:     260.0
+    extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic
     extrinsic_T: [ 0.05512, 0.02226, -0.0297 ]
     extrinsic_R: [ 1, 0, 0,
                    0, 1, 0,
                    0, 0, 1]
 
 publish:     
-    scan_publish_en:  1       # 'false' will close all the point cloud output
-    dense_publish_en: 1       # false will low down the points number in a global-frame point clouds scan.
-    scan_bodyframe_pub_en: 1  # output the point cloud scans in IMU-body-frame
+    scan_publish_en:  true       # false: close all the point cloud output
+    dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
+    scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame

config/ouster64.yaml

@@ -15,12 +15,13 @@ mapping:
     b_gyr_cov: 0.0001
     fov_degree:    180
     det_range:     150.0
+    extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic
     extrinsic_T: [ 0.0, 0.0, 0.0 ]
     extrinsic_R: [1, 0, 0,
                   0, 1, 0,
                   0, 0, 1]
 
 publish:     
-    scan_publish_en:  1       # 'false' will close all the point cloud output
-    dense_publish_en: 1       # false will low down the points number in a global-frame point clouds scan.
-    scan_bodyframe_pub_en: 1  # output the point cloud scans in IMU-body-frame
+    scan_publish_en:  true       # false: close all the point cloud output
+    dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
+    scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame

config/velodyne.yaml

@@ -15,12 +15,13 @@ mapping:
     b_gyr_cov: 0.0001
     fov_degree:    180
     det_range:     100.0
+    extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic
     extrinsic_T: [ 0, 0, 0.28]
     extrinsic_R: [ 1, 0, 0, 
                    0, 1, 0, 
                    0, 0, 1]
 
 publish:     
-    scan_publish_en:  1       # 'false' will close all the point cloud output
-    dense_publish_en: 1       # false will low down the points number in a global-frame point clouds scan.
-    scan_bodyframe_pub_en: 1  # output the point cloud scans in IMU-body-frame
+    scan_publish_en:  true       # false: close all the point cloud output
+    dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
+    scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame

src/laserMapping.cpp

@@ -70,7 +70,7 @@ double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_ti
 double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot4[MAXN], s_plot5[MAXN], s_plot6[MAXN], s_plot7[MAXN], s_plot8[MAXN], s_plot9[MAXN], s_plot10[MAXN], s_plot11[MAXN];
 double match_time = 0, solve_time = 0, solve_const_H_time = 0;
 int    kdtree_size_st = 0, kdtree_size_end = 0, add_point_size = 0, kdtree_delete_counter = 0;
-bool   runtime_pos_log = false, pcd_save_en = false, time_sync_en = false;
+bool   runtime_pos_log = false, pcd_save_en = false, time_sync_en = false, extrinsic_est_en = true;
 /**************************/
 
 float res_last[100000] = {0.0};
@@ -696,8 +696,15 @@ void h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_
         /*** calculate the Measuremnt Jacobian matrix H ***/
         V3D C(s.rot.conjugate() *norm_vec);
         V3D A(point_crossmat * C);
-        V3D B(point_be_crossmat * s.offset_R_L_I.conjugate() * C); //s.rot.conjugate()*norm_vec);
-        ekfom_data.h_x.block<1, 12>(i,0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C);
+        if (extrinsic_est_en)
+        {
+            V3D B(point_be_crossmat * s.offset_R_L_I.conjugate() * C); //s.rot.conjugate()*norm_vec);
+            ekfom_data.h_x.block<1, 12>(i,0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C);
+        }
+        else
+        {
+            ekfom_data.h_x.block<1, 12>(i,0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+        }
 
         /*** Measuremnt: distance to the closest surface/corner ***/
         ekfom_data.h(i) = -norm_p.intensity;
@@ -735,6 +742,7 @@ int main(int argc, char** argv)
     nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, 0);
     nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0);
     nh.param<bool>("pcd_save_enable", pcd_save_en, 0);
+    nh.param<bool>("mapping/extrinsic_est_en", extrinsic_est_en, 1);
     nh.param<vector<double>>("mapping/extrinsic_T", extrinT, vector<double>());
     nh.param<vector<double>>("mapping/extrinsic_R", extrinR, vector<double>());
     cout<<"p_pre->lidar_type "<<p_pre->lidar_type<<endl;