12345qiupeng
/
Fast-Lio
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add the switch for the online extrinsic estimation

main
xw 2021-08-09 07:05:19 -04:00
parent f159d57da8
commit 53ee209c12
7 changed files with 44 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
Log/plot.py
View File

@ -33,22 +33,22 @@ plt.grid()
#### 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)

1
README.md
View File

@ -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:

7
config/avia.yaml
View File

@ -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

7
config/horizon.yaml
View File

@ -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

7
config/ouster64.yaml
View File

@ -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

7
config/velodyne.yaml
View File

@ -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

14
src/laserMapping.cpp
View File

@ -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;