feat: 🎸 add radius-search-base loop closure module

.gitignore

@@ -1,8 +1,5 @@
 build
-FAST-LIO/Log/*.png
-FAST-LIO/Log/*.txt
-FAST-LIO/Log/*.csv
-FAST-LIO/Log/*.pdf
+Log/
 .vscode/c_cpp_properties.json
 .vscode/settings.json
 PCD/*.pcd

FAST-LIO/config/velodyne16.yaml

@@ -0,0 +1,28 @@
+common:
+    lid_topic:  "/velodyne_points"
+    imu_topic:  "/imu/data"
+    keyFrame_topic: "/aft_pgo_path"
+    keyFrame_id_topic: "/key_frames_ids"
+    time_sync_en: true         # ONLY turn on when external time synchronization is really not possible
+
+preprocess:
+    lidar_type: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
+    scan_line: 16
+    blind: 1                     # blind是min_radius^2
+
+mapping:
+    acc_cov: 0.1
+    gyr_cov: 0.1
+    b_acc_cov: 0.0001
+    b_gyr_cov: 0.0001
+    fov_degree:    180
+    det_range:     100.0
+    extrinsic_T: [ 0, 0, 0.28]
+    extrinsic_R: [ 1, 0, 0, 
+                   0, 1, 0, 
+                   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

FAST-LIO/include/ikd-Tree/ikd_Tree.cpp

@@ -1292,6 +1292,23 @@ bool KD_TREE::point_cmp_x(PointType a, PointType b) { return a.x < b.x;}
 bool KD_TREE::point_cmp_y(PointType a, PointType b) { return a.y < b.y;}
 bool KD_TREE::point_cmp_z(PointType a, PointType b) { return a.z < b.z;}
 
+/**
+ * @brief 删除当前所有的点云缓存，根据输入的点云，重新构造ikdtree
+ * @param point_cloud 输入的点云
+ */
+void KD_TREE::reconstruct(PointVector point_cloud){
+    Delete_Storage_Disabled = true;
+    delete_tree_nodes(&Root_Node);
+    PointVector ().swap(PCL_Storage);
+    Rebuild_Logger.clear();           
+
+    if(Root_Node == nullptr){
+        Build(point_cloud);
+    } else {
+        Add_Points(point_cloud, true);
+    }
+}
+
 // Manual heap
 MANUAL_HEAP::MANUAL_HEAP(int max_capacity){
     cap = max_capacity;

FAST-LIO/include/ikd-Tree/ikd_Tree.h

@@ -180,6 +180,7 @@ class KD_TREE
     int Delete_Point_Boxes(vector<BoxPointType> & BoxPoints);
     void flatten(KD_TREE_NODE * root, PointVector &Storage, delete_point_storage_set storage_type);
     void acquire_removed_points(PointVector & removed_points);
+    void reconstruct(PointVector point_cloud);
     BoxPointType tree_range();
     PointVector PCL_Storage;     
     KD_TREE_NODE * Root_Node = nullptr;

FAST-LIO/launch/mapping_velodyne.launch

@@ -3,22 +3,33 @@
 
     <arg name="rviz" default="true" />
 
-    <rosparam command="load" file="$(find fast_lio)/config/velodyne.yaml" />
+    <rosparam command="load" file="$(find fast_lio)/config/velodyne16.yaml" />
 
+    <!-- 调参说明：  -->
+    <!-- point_filter_num，和filter_size_surf是滤波参数，越小前端效果越好，但运行越慢 -->
     <param name="feature_extract_enable" type="bool" value="0"/>
     <param name="point_filter_num" type="int" value="4"/>
     <param name="max_iteration" type="int" value="3" />
     <param name="scan_publish_enable" type="bool" value="1" />
 	  <param name="dense_publish_enable" type="bool" value="1" />
-    <param name="filter_size_surf" type="double" value="0.5" />
-    <param name="filter_size_map" type="double" value="0.5" />
+    <param name="filter_size_surf" type="double" value="0.5" />   <!-- 对当前点云的filter -->
+    <param name="filter_size_map" type="double" value="0.2" />    <!-- 对地图的filter -->
     <param name="cube_side_length" type="double" value="1000" />
     <param name="runtime_pos_log_enable" type="bool" value="0" />
     <param name="pcd_save_enable" type="bool" value="0" />
+
+    <!-- visualization -->
+    <param name="visulize_map" value="true"/>
+
+    <!--optimization  -->
+    <param name="recontructKdTree" value="true"/> <!-- 打开更新ikdtree -->
+    <param name="updateState" value="true"/>   <!-- 打开更新状态 -->
+    <param name="updateFrequency" value="30"/> <!-- 每接受updateFrequency个点云，更新一次， 不能太大-->
+
     <node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" /> 
 
     <group if="$(arg rviz)">
-    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
+    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz_fast_lio" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
     </group>
 
 </launch>

FAST-LIO/rviz_cfg/loam_livox.rviz

@@ -5,18 +5,11 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
-        - /mapping1
-        - /mapping1/surround1
-        - /mapping1/currPoints1
         - /mapping1/currPoints1/Autocompute Value Bounds1
-        - /Odometry1/Odometry1
-        - /Odometry1/Odometry1/Shape1
-        - /Odometry1/Odometry1/Covariance1
         - /Odometry1/Odometry1/Covariance1/Position1
         - /Odometry1/Odometry1/Covariance1/Orientation1
-        - /MarkerArray1/Namespaces1
       Splitter Ratio: 0.6432291865348816
-    Tree Height: 1137
+    Tree Height: 424
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -61,8 +54,7 @@ Visualization Manager:
       Plane Cell Count: 40
       Reference Frame: <Fixed Frame>
       Value: false
-    - Alpha: 1
-      Class: rviz/Axes
+    - Class: rviz/Axes
       Enabled: false
       Length: 0.699999988079071
       Name: Axes
@@ -143,12 +135,12 @@ Visualization Manager:
           Invert Rainbow: false
           Max Color: 255; 255; 255
           Min Color: 0; 0; 0
-          Name: PointCloud2
+          Name: Laser_map_from_kdtree
           Position Transformer: XYZ
           Queue Size: 10
           Selectable: true
           Size (Pixels): 3
-          Size (m): 0.10000000149011612
+          Size (m): 0.029999999329447746
           Style: Flat Squares
           Topic: /Laser_map
           Unreliable: false
@@ -180,7 +172,6 @@ Visualization Manager:
           Keep: 1
           Name: Odometry
           Position Tolerance: 0.0010000000474974513
-          Queue Size: 10
           Shape:
             Alpha: 1
             Axes Length: 1
@@ -194,106 +185,46 @@ Visualization Manager:
           Topic: /Odometry
           Unreliable: false
           Value: true
+        - Alpha: 0
+          Buffer Length: 2
+          Class: rviz/Path
+          Color: 25; 255; 255
+          Enabled: true
+          Head Diameter: 0
+          Head Length: 0
+          Length: 0.30000001192092896
+          Line Style: Billboards
+          Line Width: 0.10000000149011612
+          Name: Path
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 25; 255; 255
+          Pose Style: None
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.4000000059604645
+          Shaft Length: 0.4000000059604645
+          Topic: /path
+          Unreliable: false
+          Value: true
       Enabled: true
       Name: Odometry
-    - Alpha: 1
-      Class: rviz/Axes
+    - Class: rviz/Axes
       Enabled: true
       Length: 0.699999988079071
       Name: Axes
       Radius: 0.10000000149011612
       Reference Frame: <Fixed Frame>
       Value: true
-    - Alpha: 0
-      Buffer Length: 2
-      Class: rviz/Path
-      Color: 25; 255; 255
-      Enabled: true
-      Head Diameter: 0
-      Head Length: 0
-      Length: 0.30000001192092896
-      Line Style: Billboards
-      Line Width: 0.20000000298023224
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 25; 255; 255
-      Pose Style: None
-      Queue Size: 10
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.4000000059604645
-      Shaft Length: 0.4000000059604645
-      Topic: /path
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: false
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 239; 41; 41
-      Max Intensity: 0
-      Min Color: 239; 41; 41
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 4
-      Size (m): 0.30000001192092896
-      Style: Spheres
-      Topic: /cloud_effected
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 13.139549255371094
-        Min Value: -32.08251953125
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 138; 226; 52
-      Color Transformer: FlatColor
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 138; 226; 52
-      Min Color: 138; 226; 52
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.10000000149011612
-      Style: Flat Squares
-      Topic: /Laser_map
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
     - Class: rviz/MarkerArray
-      Enabled: false
-      Marker Topic: /MarkerArray
-      Name: MarkerArray
+      Enabled: true
+      Marker Topic: /loop_closure_constraints
+      Name: loop
       Namespaces:
         {}
       Queue Size: 100
-      Value: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 0; 0; 0
@@ -321,42 +252,42 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Class: rviz/ThirdPersonFollower
-      Distance: 76.899658203125
+      Class: rviz/Orbit
+      Distance: 47.928932189941406
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
-      Field of View: 0.7853981852531433
       Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
-      Focal Shape Fixed Size: false
+        X: 14.201390266418457
+        Y: 8.872049331665039
+        Z: 3.232368230819702
+      Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
       Pitch: 1.5697963237762451
       Target Frame: global
-      Yaw: 2.980398416519165
+      Value: Orbit (rviz)
+      Yaw: 4.031837463378906
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: false
-  Height: 1376
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001c8000004aefc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004ae000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004aefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004ae000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b800000052fc0100000002fb0000000800540069006d00650100000000000009b8000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000692000004ae00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+    collapsed: true
+  Height: 1050
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001c80000039efc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000008600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000560000039e0000012600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000000000010000015200000368fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000368000000ec00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a400000052fc0100000002fb0000000800540069006d00650000000000000003a4000006d800fffffffb0000000800540069006d006501000000000000045000000000000000000000039f0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
-  Width: 2488
-  X: 72
-  Y: 27
+    collapsed: true
+  Width: 927
+  X: 2627
+  Y: 0

FAST-LIO/src/IMU_Processing.hpp

@@ -192,7 +192,12 @@ void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom,
   }
   state_ikfom init_state = kf_state.get_x();
   init_state.grav = S2(- mean_acc / mean_acc.norm() * G_m_s2);
-
+  const auto& orientation = meas.imu.front()->orientation;
+  // 如果orientation消息可用的话,以IMU的orientaion作为初始姿态
+  if( orientation.x != 0 || orientation.y != 0 || orientation.z != 0 || orientation.w != 0 ){
+    Eigen::Quaterniond qua(orientation.w, orientation.x, orientation.y, orientation.z);
+    init_state.rot = qua;
+  }
   //state_inout.rot = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity)));
   init_state.bg  = mean_gyr;
   init_state.offset_T_L_I = Lidar_T_wrt_IMU;
@@ -253,7 +258,7 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikf
                 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y),
                 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z);
 
-    // fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl;
+    fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl;
 
     acc_avr     = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba;