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

support for Scan context loop closure

main
xw 2021-07-14 06:51:30 -04:00
parent 96774ef3e9
commit c8aea0dd95
7 changed files with 89 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
README.md
View File

@ -5,7 +5,7 @@
3. [UAV Avoiding Dynamic Obstacles](https://github.com/hku-mars/dyn_small_obs_avoidance): One of the implementation of FAST-LIO in robot's planning. 3. [UAV Avoiding Dynamic Obstacles](https://github.com/hku-mars/dyn_small_obs_avoidance): One of the implementation of FAST-LIO in robot's planning.
4. [R2LIVE](https://github.com/hku-mars/r2live): A high-precision LiDAR-inertial-Vision fusion work using FAST-LIO as LiDAR-inertial front-end. 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. 5. [UGV Demo](https://www.youtube.com/watch?v=wikgrQbE6Cs): Model Predictive Control for Trajectory Tracking on Differentiable Manifolds.
6. [SC-A-LOAM](https://github.com/gisbi-kim/SC-A-LOAM#for-livox-lidar): A [Scan-Context](https://github.com/irapkaist/scancontext) loop closure module that can directly work with FAST-LIO1 (The support for FAST-LIO2 is under developing). 6. [SC-A-LOAM](https://github.com/gisbi-kim/SC-A-LOAM#for-livox-lidar): A [Scan-Context](https://github.com/irapkaist/scancontext) loop closure module that can directly work with FAST-LIO.
## FAST-LIO ## 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: **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:
@ -30,8 +30,8 @@
**New Features:** **New Features:**
1. Incremental mapping using [ikd-Tree](https://github.com/hku-mars/ikd-Tree), achieve faster speed and over 100Hz LiDAR rate. 1. Incremental mapping using [ikd-Tree](https://github.com/hku-mars/ikd-Tree), achieve faster speed and over 100Hz LiDAR rate.
2. Direct odometry (scan to map) on Raw LiDAR points (feature extraction can be closed), achieving better accuracy. 2. Direct odometry (scan to map) on Raw LiDAR points (feature extraction can be disabled), achieving better accuracy.
3. Since no need for feature extraction, FAST-LIO2 can support different LiDAR Types including spinning (Velodyne, Ouster) and solid-state (Avia, horizon) LiDARs, and can be easily extended to support more LiDAR. 3. Since no requirements for feature extraction, FAST-LIO2 can support may types of LiDAR including spinning (Velodyne, Ouster) and solid-state (Livox Avia, Horizon, MID-70) LiDARs, and can be easily extended to support more LiDARs.
4. Support external IMU. 4. Support external IMU.
5. Support ARM-based platforms including Khadas VIM3, Nivida TX2, Raspberry Pi 4B(8G RAM). 5. Support ARM-based platforms including Khadas VIM3, Nivida TX2, Raspberry Pi 4B(8G RAM).
@ -121,7 +121,7 @@ Edit ``` config/velodyne.yaml ``` to set the below parameters:
1. LiDAR point cloud topic name: ``` lid_topic ``` 1. LiDAR point cloud topic name: ``` lid_topic ```
2. IMU topic name: ``` imu_topic ``` (both internal and external, 6-aixes or 9-axies are fine) 2. IMU topic name: ``` imu_topic ``` (both internal and external, 6-aixes or 9-axies are fine)
3. Line number (we tested 16 and 32 line, but not tested 64 or above): ``` scan_line ``` 3. Line number (we tested 16, 32 and 64 line, but not tested 128 or above): ``` scan_line ```
4. Translational extrinsic: ``` extrinsic_T ``` 4. Translational extrinsic: ``` extrinsic_T ```
5. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix) 5. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix)
- The extrinsic parameters in FAST-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame). - The extrinsic parameters in FAST-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame).

7
config/avia.yaml
View File

@ -18,4 +18,9 @@ mapping:
extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
extrinsic_R: [ 1, 0, 0, extrinsic_R: [ 1, 0, 0,
0, 1, 0, 0, 1, 0,
0, 0, 1] 0, 0, 1]
publish:
scan_publish_en: true # 'false' will close all the point cloud output
dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame

7
config/horizon.yaml
View File

@ -18,4 +18,9 @@ mapping:
extrinsic_T: [ 0.05512, 0.02226, -0.0297 ] extrinsic_T: [ 0.05512, 0.02226, -0.0297 ]
extrinsic_R: [ 1, 0, 0, extrinsic_R: [ 1, 0, 0,
0, 1, 0, 0, 1, 0,
0, 0, 1] 0, 0, 1]
publish:
scan_publish_en: true # 'false' will close all the point cloud output
dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame

7
config/ouster64.yaml
View File

@ -18,4 +18,9 @@ mapping:
extrinsic_T: [ 0.0, 0.0, 0.0 ] extrinsic_T: [ 0.0, 0.0, 0.0 ]
extrinsic_R: [1, 0, 0, extrinsic_R: [1, 0, 0,
0, 1, 0, 0, 1, 0,
0, 0, 1] 0, 0, 1]
publish:
scan_publish_en: true # 'false' will close all the point cloud output
dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame

7
config/velodyne.yaml
View File

@ -18,4 +18,9 @@ mapping:
extrinsic_T: [ 0, 0, 0.28] extrinsic_T: [ 0, 0, 0.28]
extrinsic_R: [ 1, 0, 0, extrinsic_R: [ 1, 0, 0,
0, 1, 0, 0, 1, 0,
0, 0, 1] 0, 0, 1]
publish:
scan_publish_en: true # 'false' will close all the point cloud output
dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame

22
rviz_cfg/loam_livox.rviz
View File

@ -35,7 +35,7 @@ Panels:
Experimental: false Experimental: false
Name: Time Name: Time
SyncMode: 0 SyncMode: 0
SyncSource: "" SyncSource: surround
Preferences: Preferences:
PromptSaveOnExit: true PromptSaveOnExit: true
Toolbars: Toolbars:
@ -81,11 +81,11 @@ Visualization Manager:
Class: rviz/PointCloud2 Class: rviz/PointCloud2
Color: 238; 238; 236 Color: 238; 238; 236
Color Transformer: Intensity Color Transformer: Intensity
Decay Time: 0 Decay Time: 1000
Enabled: true Enabled: true
Invert Rainbow: false Invert Rainbow: false
Max Color: 255; 255; 255 Max Color: 255; 255; 255
Max Intensity: 150 Max Intensity: 158
Min Color: 0; 0; 0 Min Color: 0; 0; 0
Min Intensity: 0 Min Intensity: 0
Name: surround Name: surround
@ -195,7 +195,7 @@ Visualization Manager:
Shaft Length: 0.05000000074505806 Shaft Length: 0.05000000074505806
Shaft Radius: 0.05000000074505806 Shaft Radius: 0.05000000074505806
Value: Axes Value: Axes
Topic: /aft_mapped_to_init Topic: /Odometry
Unreliable: false Unreliable: false
Value: true Value: true
Enabled: true Enabled: true
@ -302,7 +302,7 @@ Visualization Manager:
Global Options: Global Options:
Background Color: 0; 0; 0 Background Color: 0; 0; 0
Default Light: true Default Light: true
Fixed Frame: camera_init Fixed Frame: global
Frame Rate: 10 Frame Rate: 10
Name: root Name: root
Tools: Tools:
@ -333,18 +333,18 @@ Visualization Manager:
Swap Stereo Eyes: false Swap Stereo Eyes: false
Value: false Value: false
Focal Point: Focal Point:
X: 5.488441467285156 X: 36.65506362915039
Y: -29.57037353515625 Y: 2.3543496131896973
Z: 2.2303884179564193e-6 Z: -1.0087411283166148e-5
Focal Shape Fixed Size: false Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806 Focal Shape Size: 0.05000000074505806
Invert Z Axis: false Invert Z Axis: false
Name: Current View Name: Current View
Near Clip Distance: 0.009999999776482582 Near Clip Distance: 0.009999999776482582
Pitch: 0.740202784538269 Pitch: 1.5697963237762451
Target Frame: camera_init Target Frame: global
Value: ThirdPersonFollower (rviz) Value: ThirdPersonFollower (rviz)
Yaw: 3.57222843170166 Yaw: 3.0072357654571533
Saved: ~ Saved: ~
Window Geometry: Window Geometry:
Displays: Displays:

69
src/laserMapping.cpp
View File

@ -92,7 +92,7 @@ int effct_feat_num = 0, time_log_counter = 0, scan_count = 0, publish_count =
int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0; int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0;
bool point_selected_surf[100000] = {0}; bool point_selected_surf[100000] = {0};
bool lidar_pushed, flg_reset, flg_exit = false, flg_EKF_inited; bool lidar_pushed, flg_reset, flg_exit = false, flg_EKF_inited;
bool scan_pub_en = false, dense_pub_en = false; bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false;
vector<vector<int>> pointSearchInd_surf; vector<vector<int>> pointSearchInd_surf;
vector<BoxPointType> cub_needrm; vector<BoxPointType> cub_needrm;
@ -206,6 +206,17 @@ void RGBpointBodyToWorld(PointType const * const pi, PointType * const po)
po->intensity = pi->intensity; po->intensity = pi->intensity;
} }
void RGBpointBodyLidarToIMU(PointType const * const pi, PointType * const po)
{
V3D p_body_lidar(pi->x, pi->y, pi->z);
V3D p_body_imu(state_point.offset_R_L_I*p_body_lidar + state_point.offset_T_L_I);
po->x = p_body_imu(0);
po->y = p_body_imu(1);
po->z = p_body_imu(2);
po->intensity = pi->intensity;
}
void points_cache_collect() void points_cache_collect()
{ {
PointVector points_history; PointVector points_history;
@ -443,9 +454,8 @@ void map_incremental()
PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1)); PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1));
PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI()); PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI());
void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes) void publish_frame_world(const ros::Publisher & pubLaserCloudFull)
{ {
// *pcl_wait_pub += *laserCloudWorld;
if(scan_pub_en) if(scan_pub_en)
{ {
PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body); PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body);
@ -461,11 +471,10 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
sensor_msgs::PointCloud2 laserCloudmsg; sensor_msgs::PointCloud2 laserCloudmsg;
pcl::toROSMsg(*laserCloudWorld, laserCloudmsg); pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);
laserCloudmsg.header.stamp = ros::Time::now(); laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudmsg.header.frame_id = "camera_init"; laserCloudmsg.header.frame_id = "camera_init";
pubLaserCloudFullRes.publish(laserCloudmsg); pubLaserCloudFull.publish(laserCloudmsg);
publish_count -= PUBFRAME_PERIOD; publish_count -= PUBFRAME_PERIOD;
// pcl_wait_pub->clear();
} }
/**************** save map ****************/ /**************** save map ****************/
@ -486,6 +495,25 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes)
} }
} }
void publish_frame_body(const ros::Publisher & pubLaserCloudFull_body)
{
int size = feats_undistort->points.size();
PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1));
for (int i = 0; i < size; i++)
{
RGBpointBodyLidarToIMU(&feats_undistort->points[i], \
&laserCloudIMUBody->points[i]);
}
sensor_msgs::PointCloud2 laserCloudmsg;
pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg);
laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudmsg.header.frame_id = "body";
pubLaserCloudFull_body.publish(laserCloudmsg);
publish_count -= PUBFRAME_PERIOD;
}
void publish_effect_world(const ros::Publisher & pubLaserCloudEffect) void publish_effect_world(const ros::Publisher & pubLaserCloudEffect)
{ {
PointCloudXYZI::Ptr laserCloudWorld( \ PointCloudXYZI::Ptr laserCloudWorld( \
@ -497,7 +525,7 @@ void publish_effect_world(const ros::Publisher & pubLaserCloudEffect)
} }
sensor_msgs::PointCloud2 laserCloudFullRes3; sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3); pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time::now(); laserCloudFullRes3.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudFullRes3.header.frame_id = "camera_init"; laserCloudFullRes3.header.frame_id = "camera_init";
pubLaserCloudEffect.publish(laserCloudFullRes3); pubLaserCloudEffect.publish(laserCloudFullRes3);
} }
@ -506,7 +534,7 @@ void publish_map(const ros::Publisher & pubLaserCloudMap)
{ {
sensor_msgs::PointCloud2 laserCloudMap; sensor_msgs::PointCloud2 laserCloudMap;
pcl::toROSMsg(*featsFromMap, laserCloudMap); pcl::toROSMsg(*featsFromMap, laserCloudMap);
laserCloudMap.header.stamp = ros::Time::now(); laserCloudMap.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudMap.header.frame_id = "camera_init"; laserCloudMap.header.frame_id = "camera_init";
pubLaserCloudMap.publish(laserCloudMap); pubLaserCloudMap.publish(laserCloudMap);
} }
@ -527,8 +555,8 @@ void set_posestamp(T & out)
void publish_odometry(const ros::Publisher & pubOdomAftMapped) void publish_odometry(const ros::Publisher & pubOdomAftMapped)
{ {
odomAftMapped.header.frame_id = "camera_init"; odomAftMapped.header.frame_id = "camera_init";
odomAftMapped.child_frame_id = "aft_mapped"; odomAftMapped.child_frame_id = "body";
odomAftMapped.header.stamp = ros::Time::now();//ros::Time().fromSec(last_timestamp_lidar); odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time);// ros::Time().fromSec(lidar_end_time);
set_posestamp(odomAftMapped.pose); set_posestamp(odomAftMapped.pose);
pubOdomAftMapped.publish(odomAftMapped); pubOdomAftMapped.publish(odomAftMapped);
auto P = kf.get_P(); auto P = kf.get_P();
@ -554,13 +582,13 @@ void publish_odometry(const ros::Publisher & pubOdomAftMapped)
q.setY(odomAftMapped.pose.pose.orientation.y); q.setY(odomAftMapped.pose.pose.orientation.y);
q.setZ(odomAftMapped.pose.pose.orientation.z); q.setZ(odomAftMapped.pose.pose.orientation.z);
transform.setRotation( q ); transform.setRotation( q );
br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped" ) ); br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "camera_init", "body" ) );
} }
void publish_path(const ros::Publisher pubPath) void publish_path(const ros::Publisher pubPath)
{ {
set_posestamp(msg_body_pose); set_posestamp(msg_body_pose);
msg_body_pose.header.stamp = ros::Time::now(); msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time);
msg_body_pose.header.frame_id = "camera_init"; msg_body_pose.header.frame_id = "camera_init";
/*** if path is too large, the rvis will crash ***/ /*** if path is too large, the rvis will crash ***/
@ -682,8 +710,9 @@ int main(int argc, char** argv)
ros::init(argc, argv, "laserMapping"); ros::init(argc, argv, "laserMapping");
ros::NodeHandle nh; ros::NodeHandle nh;
nh.param<bool>("scan_publish_enable",scan_pub_en,1); nh.param<bool>("publish/scan_publish_en",scan_pub_en,1);
nh.param<bool>("dense_publish_enable",dense_pub_en,0); nh.param<bool>("publish/dense_publish_en",dense_pub_en,0);
nh.param<bool>("publish/scan_bodyframe_pub_en",scan_body_pub_en,1);
nh.param<int>("max_iteration",NUM_MAX_ITERATIONS,4); nh.param<int>("max_iteration",NUM_MAX_ITERATIONS,4);
nh.param<string>("map_file_path",map_file_path,""); nh.param<string>("map_file_path",map_file_path,"");
nh.param<string>("common/lid_topic",lid_topic,"/livox/lidar"); nh.param<string>("common/lid_topic",lid_topic,"/livox/lidar");
@ -761,14 +790,16 @@ int main(int argc, char** argv)
nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \ nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \
nh.subscribe(lid_topic, 200000, standard_pcl_cbk); nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk); ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered", 100000); ("/cloud_registered", 100000);
ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2> ros::Publisher pubLaserCloudFull_body = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered_body", 100000);
ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_effected", 100000); ("/cloud_effected", 100000);
ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2> ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
("/Laser_map", 100000); ("/Laser_map", 100000);
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>
("/aft_mapped_to_init", 100000); ("/Odometry", 100000);
ros::Publisher pubPath = nh.advertise<nav_msgs::Path> ros::Publisher pubPath = nh.advertise<nav_msgs::Path>
("/path", 100000); ("/path", 100000);
//------------------------------------------------------------------------------------------------------ //------------------------------------------------------------------------------------------------------
@ -888,8 +919,8 @@ int main(int argc, char** argv)
/******* Publish points *******/ /******* Publish points *******/
publish_path(pubPath); publish_path(pubPath);
if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFullRes); if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFull);
if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFull_body);
// publish_effect_world(pubLaserCloudEffect); // publish_effect_world(pubLaserCloudEffect);
// publish_map(pubLaserCloudMap); // publish_map(pubLaserCloudMap);