From c8aea0dd95e4f78203b79329dd59ccdd687c1bd7 Mon Sep 17 00:00:00 2001 From: xw Date: Wed, 14 Jul 2021 06:51:30 -0400 Subject: [PATCH] support for Scan context loop closure --- README.md | 8 ++--- config/avia.yaml | 7 +++- config/horizon.yaml | 7 +++- config/ouster64.yaml | 7 +++- config/velodyne.yaml | 7 +++- rviz_cfg/loam_livox.rviz | 22 ++++++------- src/laserMapping.cpp | 69 +++++++++++++++++++++++++++++----------- 7 files changed, 89 insertions(+), 38 deletions(-) diff --git a/README.md b/README.md index 228bd69..d42577c 100644 --- a/README.md +++ b/README.md @@ -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). diff --git a/config/avia.yaml b/config/avia.yaml index 2f4c6be..824b0a5 100644 --- a/config/avia.yaml +++ b/config/avia.yaml @@ -18,4 +18,9 @@ mapping: extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] extrinsic_R: [ 1, 0, 0, 0, 1, 0, - 0, 0, 1] \ No newline at end of file + 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 \ No newline at end of file diff --git a/config/horizon.yaml b/config/horizon.yaml index 1b34be6..58adc5a 100644 --- a/config/horizon.yaml +++ b/config/horizon.yaml @@ -18,4 +18,9 @@ mapping: extrinsic_T: [ 0.05512, 0.02226, -0.0297 ] extrinsic_R: [ 1, 0, 0, 0, 1, 0, - 0, 0, 1] \ No newline at end of file + 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 \ No newline at end of file diff --git a/config/ouster64.yaml b/config/ouster64.yaml index 55b0e44..c0bb8be 100644 --- a/config/ouster64.yaml +++ b/config/ouster64.yaml @@ -18,4 +18,9 @@ mapping: extrinsic_T: [ 0.0, 0.0, 0.0 ] extrinsic_R: [1, 0, 0, 0, 1, 0, - 0, 0, 1] \ No newline at end of file + 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 \ No newline at end of file diff --git a/config/velodyne.yaml b/config/velodyne.yaml index b280b0c..a97653f 100644 --- a/config/velodyne.yaml +++ b/config/velodyne.yaml @@ -18,4 +18,9 @@ mapping: extrinsic_T: [ 0, 0, 0.28] extrinsic_R: [ 1, 0, 0, 0, 1, 0, - 0, 0, 1] \ No newline at end of file + 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 \ No newline at end of file diff --git a/rviz_cfg/loam_livox.rviz b/rviz_cfg/loam_livox.rviz index 2af664d..0219dee 100644 --- a/rviz_cfg/loam_livox.rviz +++ b/rviz_cfg/loam_livox.rviz @@ -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: diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp index 51267bb..6bb28ee 100644 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -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> pointSearchInd_surf; vector 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("scan_publish_enable",scan_pub_en,1); - nh.param("dense_publish_enable",dense_pub_en,0); + nh.param("publish/scan_publish_en",scan_pub_en,1); + nh.param("publish/dense_publish_en",dense_pub_en,0); + nh.param("publish/scan_bodyframe_pub_en",scan_body_pub_en,1); nh.param("max_iteration",NUM_MAX_ITERATIONS,4); nh.param("map_file_path",map_file_path,""); nh.param("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 + ros::Publisher pubLaserCloudFull = nh.advertise ("/cloud_registered", 100000); - ros::Publisher pubLaserCloudEffect = nh.advertise + ros::Publisher pubLaserCloudFull_body = nh.advertise + ("/cloud_registered_body", 100000); + ros::Publisher pubLaserCloudEffect = nh.advertise ("/cloud_effected", 100000); ros::Publisher pubLaserCloudMap = nh.advertise ("/Laser_map", 100000); ros::Publisher pubOdomAftMapped = nh.advertise - ("/aft_mapped_to_init", 100000); + ("/Odometry", 100000); ros::Publisher pubPath = nh.advertise ("/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);