优化特征提取

README.md

@@ -32,6 +32,13 @@ roslaunch lio_sam run9axis.launch
 
 <img src="doc/indoorTest.gif" alt="indoorTest.gif" title="Indoor Test" style="zoom: 100%;"/>
 
+- 室外环境 倾斜手持
+
+<img src="doc/Outdoor.gif" alt="Outdoor.gif" title="Outdoor" style="zoom: 100%;"/>
+
+- 室内环境
+
+<img src="doc/indoor.gif" alt="indoor.gif" title="indoor" style="zoom: 100%;"/>
 ## Note
 
 - 注意对齐Lidar与IMU的时间戳。
@@ -43,7 +50,12 @@ roslaunch lio_sam run9axis.launch
 
 - [ ] 提取特征优化
 
+## 已知问题
+
+使用六轴IMU时初始姿态不水平的话IMU里程计会剧烈抖动，IMU预积分存在问题？
+
 ## Acknowledgement
 
 - [LIO-SAM](https://github.com/TixiaoShan/LIO-SAM/)
 - [LIO-SAM-DetailedNote](https://github.com/smilefacehh/LIO-SAM-DetailedNote)
+- [LIO_SAM_6AXIS](https://github.com/JokerJohn/LIO_SAM_6AXIS)

config/params9axisIMU.yaml

@@ -86,12 +86,12 @@ lio_sam:
   surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
   surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
   surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
-  surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)
+  surroundingKeyframeSearchRadius: 10.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)
 
   # Loop closure
   loopClosureEnableFlag: true
   loopClosureFrequency: 1.0                     # Hz, regulate loop closure constraint add frequency
-  surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
+  surroundingKeyframeSize: 25                   # submap size (when loop closure enabled)
   historyKeyframeSearchRadius: 15.0             # meters, key frame that is within n meters from current pose will be considerd for loop closure
   historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
   historyKeyframeSearchNum: 25                  # number of hostory key frames will be fused into a submap for loop closure

config/paramsLivoxIMU.yaml

@@ -48,7 +48,7 @@ lio_sam:
                   0, 0, 1]
 
   # LOAM feature threshold
-  edgeThreshold: 0.1
+  edgeThreshold: 0.5
   surfThreshold: 0.1
   edgeFeatureMinValidNum: 10
   surfFeatureMinValidNum: 100
@@ -75,7 +75,7 @@ lio_sam:
   # Loop closure
   loopClosureEnableFlag: true
   loopClosureFrequency: 1.0                     # Hz, regulate loop closure constraint add frequency
-  surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
+  surroundingKeyframeSize: 25                   # submap size (when loop closure enabled)
   historyKeyframeSearchRadius: 15.0             # meters, key frame that is within n meters from current pose will be considerd for loop closure
   historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
   historyKeyframeSearchNum: 25                  # number of hostory key frames will be fused into a submap for loop closure

launch/include/config/rviz.rviz

@@ -8,8 +8,8 @@ Panels:
         - /links1
         - /Odometry1
         - /Point Cloud1
-        - /Feature Cloud1
         - /Feature Cloud1/Edge Feature1
+        - /Mapping1/Map (cloud)1
       Splitter Ratio: 0.5600000023841858
     Tree Height: 710
   - Class: rviz/Selection
@@ -244,22 +244,22 @@ Visualization Manager:
           Color: 0; 255; 0
           Color Transformer: Intensity
           Decay Time: 0
-          Enabled: true
+          Enabled: false
           Invert Rainbow: false
           Max Color: 255; 255; 255
           Min Color: 0; 0; 0
           Name: Edge Feature
           Position Transformer: XYZ
           Queue Size: 10
           Selectable: true
-          Size (Pixels): 6
+          Size (Pixels): 7
           Size (m): 0.009999999776482582
           Style: Points
           Topic: /lio_sam/feature/cloud_corner
           Unreliable: false
           Use Fixed Frame: true
           Use rainbow: true
-          Value: true
+          Value: false
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
@@ -314,7 +314,7 @@ Visualization Manager:
           Position Transformer: XYZ
           Queue Size: 10
           Selectable: false
-          Size (Pixels): 2
+          Size (Pixels): 1
           Size (m): 0.009999999776482582
           Style: Points
           Topic: /lio_sam/mapping/cloud_registered
@@ -457,7 +457,8 @@ Visualization Manager:
           Marker Topic: /lio_sam/mapping/loop_closure_constraints
           Name: Loop constraint
           Namespaces:
-            {}
+            loop_edges: true
+            loop_nodes: true
           Queue Size: 100
           Value: true
       Enabled: true
@@ -478,25 +479,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 33.88191604614258
+      Distance: 81.79129028320312
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 10.746774673461914
-        Y: 6.741519927978516
-        Z: -0.6399549245834351
+        X: -10.631294250488281
+        Y: -19.241159439086914
+        Z: -0.6386321783065796
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4197956621646881
+      Pitch: 0.2547963261604309
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 0.9356471300125122
+      Yaw: 1.0387998819351196
     Saved: ~
 Window Geometry:
   Displays:

launch/run6axis.launch

@@ -3,7 +3,7 @@
     <arg name="project" default="lio_sam"/>
 
     <!-- 指定打印输出的日志等级 -->
-    <env name="ROSCONSOLE_CONFIG_FILE" value="$(find lio_sam)/launch/include/rosconsole/rosconsole_debug.conf"/>
+    <env name="ROSCONSOLE_CONFIG_FILE" value="$(find lio_sam)/launch/include/rosconsole/rosconsole_error.conf"/>
 
     <!-- Parameters -->
     <rosparam file="$(find lio_sam)/config/paramsLivoxIMU.yaml" command="load" />
@@ -15,7 +15,7 @@
     <include file="$(find lio_sam)/launch/include/module_robot_state_publisher.launch" />
 
     <!--- Run Navsat -->
-    <include file="$(find lio_sam)/launch/include/module_navsat.launch" />
+    <!-- <include file="$(find lio_sam)/launch/include/module_navsat.launch" /> -->
 
     <!--- Run Rviz-->
     <include file="$(find lio_sam)/launch/include/module_rviz.launch" />

launch/run9axis.launch

@@ -3,7 +3,7 @@
     <arg name="project" default="lio_sam"/>
 
     <!-- 指定打印输出的日志等级 -->
-    <env name="ROSCONSOLE_CONFIG_FILE" value="$(find lio_sam)/launch/include/rosconsole/rosconsole_debug.conf"/>
+    <env name="ROSCONSOLE_CONFIG_FILE" value="$(find lio_sam)/launch/include/rosconsole/rosconsole_error.conf"/>
 
     <!-- Parameters -->
     <rosparam file="$(find lio_sam)/config/params9axisIMU.yaml" command="load" />
@@ -15,7 +15,7 @@
     <include file="$(find lio_sam)/launch/include/module_robot_state_publisher.launch" />
 
     <!--- Run Navsat -->
-    <include file="$(find lio_sam)/launch/include/module_navsat.launch" />
+    <!-- <include file="$(find lio_sam)/launch/include/module_navsat.launch" /> -->
 
     <!--- Run Rviz-->
     <include file="$(find lio_sam)/launch/include/module_rviz.launch" />

src/featureExtraction.cpp

@@ -7,6 +7,8 @@
 struct smoothness_t{ 
     float value;
     size_t ind;
+    size_t line;
+    size_t ind_line;
 };
 
 struct by_value{ 
@@ -102,6 +104,8 @@ class FeatureExtraction : public ParamServer
                 // cloudSmoothness for sorting
                 cloudSmoothness[cloudInfo.Line2point[i].index[j]].value = cloudCurvature[cloudInfo.Line2point[i].index[j]];
                 cloudSmoothness[cloudInfo.Line2point[i].index[j]].ind = cloudInfo.Line2point[i].index[j];
+                cloudSmoothness[cloudInfo.Line2point[i].index[j]].line = i;
+                cloudSmoothness[cloudInfo.Line2point[i].index[j]].ind_line = j;
             }
         }
 
@@ -130,20 +134,20 @@ class FeatureExtraction : public ParamServer
                 float depth2 = cloudInfo.pointRange[cloudInfo.Line2point[k].index[i+1]];
 
                 // 10 pixel diff in range image
-                if (depth1 - depth2 > 3){
+                if (depth1 - depth2 > 1){
                     cloudNeighborPicked[cloudInfo.Line2point[k].index[i-1]] = 1;
                     cloudNeighborPicked[cloudInfo.Line2point[k].index[i]] = 1;
-                }else if (depth2 - depth1 > 3){
+                }else if (depth2 - depth1 > 1){
                     cloudNeighborPicked[cloudInfo.Line2point[k].index[i+1]] = 1;
                 }
 
                 // parallel beam
                 float diff1 = float(cloudInfo.pointRange[cloudInfo.Line2point[k].index[i-1]] - cloudInfo.pointRange[cloudInfo.Line2point[k].index[i]]);
                 float diff2 = float(cloudInfo.pointRange[cloudInfo.Line2point[k].index[i]] - cloudInfo.pointRange[cloudInfo.Line2point[k].index[i+1]]);
 
-                if (diff1 > 0.2 * cloudInfo.pointRange[cloudInfo.Line2point[k].index[i]] && diff2 >0.2 * cloudInfo.pointRange[cloudInfo.Line2point[k].index[i]])
+                if (diff1 > 0.01 * cloudInfo.pointRange[cloudInfo.Line2point[k].index[i]] && diff2 >0.01 * cloudInfo.pointRange[cloudInfo.Line2point[k].index[i]])
                     cloudNeighborPicked[cloudInfo.Line2point[k].index[i]] = 1;
-                if ((-diff1) > 0.2 * cloudInfo.pointRange[cloudInfo.Line2point[k].index[i]] && (-diff2) >0.2 * cloudInfo.pointRange[cloudInfo.Line2point[k].index[i]])
+                if ((-diff1) > 0.01 * cloudInfo.pointRange[cloudInfo.Line2point[k].index[i]] && (-diff2) >0.01 * cloudInfo.pointRange[cloudInfo.Line2point[k].index[i]])
                     cloudNeighborPicked[cloudInfo.Line2point[k].index[i]] = 1;
             }
         }
@@ -178,6 +182,8 @@ class FeatureExtraction : public ParamServer
             for (int k =ep; k >= sp; k--)
             {
                 int ind = cloudSmoothness[k].ind;
+                int line = cloudSmoothness[k].line;
+                int ind_line = cloudSmoothness[k].ind_line;
                 if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
                 {
                     largestPickedNum++;
@@ -192,15 +198,16 @@ class FeatureExtraction : public ParamServer
                     }
 
                     cloudNeighborPicked[ind] = 1;
-                    if (ind < 6 || ind > cloudSize - 7)
+                    //将局部线附近的点覆盖 防止特征聚集
+                    if (ind_line < 5 || ind_line >  cloudInfo.Line2point[line].index.size()  -6)
                         continue;
                     for (int l = 1; l <= 4; l++)
                     {
-                        cloudNeighborPicked[ind + l] = 1;
+                        cloudNeighborPicked[cloudInfo.Line2point[line].index[ind_line+l]]=1;
                     }
                     for (int l = -1; l >= -4; l--)
                     {
-                        cloudNeighborPicked[ind + l] = 1;
+                        cloudNeighborPicked[cloudInfo.Line2point[line].index[ind_line+l]]=1;
                     }
                 }
             }

src/mapOptmization.cpp

@@ -1303,7 +1303,11 @@ class mapOptimization : public ParamServer
                 combineOptimizationCoeffs();
                 ROS_DEBUG("M4.4");
                 if (LMOptimization(iterCount) == true)
-                    break;              
+                    break;           
+                if(iterCount==30)
+                {
+                    ROS_DEBUG("NOT fitness");
+                }   
             }
              ROS_DEBUG("M4.5");
              transformUpdate();