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.
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. [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 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:**
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.
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.
2. Direct odometry (scan to map) on Raw LiDAR points (feature extraction can be disabled), achieving better accuracy.
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.
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 ```
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 ```
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).

7
config/avia.yaml
View File

@ -18,4 +18,9 @@ mapping:
extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
extrinsic_R: [ 1, 0, 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_R: [ 1, 0, 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_R: [1, 0, 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_R: [ 1, 0, 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
Name: Time
SyncMode: 0
SyncSource: ""
SyncSource: surround
Preferences:
PromptSaveOnExit: true
Toolbars:
@ -81,11 +81,11 @@ Visualization Manager:
Class: rviz/PointCloud2
Color: 238; 238; 236
Color Transformer: Intensity
Decay Time: 0
Decay Time: 1000
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 150
Max Intensity: 158
Min Color: 0; 0; 0
Min Intensity: 0
Name: surround
@ -195,7 +195,7 @@ Visualization Manager:
Shaft Length: 0.05000000074505806
Shaft Radius: 0.05000000074505806
Value: Axes
Topic: /aft_mapped_to_init
Topic: /Odometry
Unreliable: false
Value: true
Enabled: true
@ -302,7 +302,7 @@ Visualization Manager:
Global Options:
Background Color: 0; 0; 0
Default Light: true
Fixed Frame: camera_init
Fixed Frame: global
Frame Rate: 10
Name: root
Tools:
@ -333,18 +333,18 @@ Visualization Manager:
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 5.488441467285156
Y: -29.57037353515625
Z: 2.2303884179564193e-6
X: 36.65506362915039
Y: 2.3543496131896973
Z: -1.0087411283166148e-5
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.740202784538269
Target Frame: camera_init
Pitch: 1.5697963237762451
Target Frame: global
Value: ThirdPersonFollower (rviz)
Yaw: 3.57222843170166
Yaw: 3.0072357654571533
Saved: ~
Window Geometry:
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;
bool point_selected_surf[100000] = {0};
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<BoxPointType> cub_needrm;
@ -206,6 +206,17 @@ void RGBpointBodyToWorld(PointType const * const pi, PointType * const po)
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()
{
PointVector points_history;
@ -443,9 +454,8 @@ void map_incremental()
PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1));
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)
{
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;
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";
pubLaserCloudFullRes.publish(laserCloudmsg);
pubLaserCloudFull.publish(laserCloudmsg);
publish_count -= PUBFRAME_PERIOD;
// pcl_wait_pub->clear();
}
/**************** 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)
{
PointCloudXYZI::Ptr laserCloudWorld( \
@ -497,7 +525,7 @@ void publish_effect_world(const ros::Publisher & pubLaserCloudEffect)
}
sensor_msgs::PointCloud2 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";
pubLaserCloudEffect.publish(laserCloudFullRes3);
}
@ -506,7 +534,7 @@ void publish_map(const ros::Publisher & pubLaserCloudMap)
{
sensor_msgs::PointCloud2 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";
pubLaserCloudMap.publish(laserCloudMap);
}
@ -527,8 +555,8 @@ void set_posestamp(T & out)
void publish_odometry(const ros::Publisher & pubOdomAftMapped)
{
odomAftMapped.header.frame_id = "camera_init";
odomAftMapped.child_frame_id = "aft_mapped";
odomAftMapped.header.stamp = ros::Time::now();//ros::Time().fromSec(last_timestamp_lidar);
odomAftMapped.child_frame_id = "body";
odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time);// ros::Time().fromSec(lidar_end_time);
set_posestamp(odomAftMapped.pose);
pubOdomAftMapped.publish(odomAftMapped);
auto P = kf.get_P();
@ -554,13 +582,13 @@ void publish_odometry(const ros::Publisher & pubOdomAftMapped)
q.setY(odomAftMapped.pose.pose.orientation.y);
q.setZ(odomAftMapped.pose.pose.orientation.z);
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)
{
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";
/*** 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::NodeHandle nh;
nh.param<bool>("scan_publish_enable",scan_pub_en,1);
nh.param<bool>("dense_publish_enable",dense_pub_en,0);
nh.param<bool>("publish/scan_publish_en",scan_pub_en,1);
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<string>("map_file_path",map_file_path,"");
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, standard_pcl_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);
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);
ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
("/Laser_map", 100000);
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>
("/aft_mapped_to_init", 100000);
("/Odometry", 100000);
ros::Publisher pubPath = nh.advertise<nav_msgs::Path>
("/path", 100000);
//------------------------------------------------------------------------------------------------------
@ -888,8 +919,8 @@ int main(int argc, char** argv)
/******* Publish points *******/
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_map(pubLaserCloudMap);