fix loop closure bug and add code optimization

tyrant-H · Jun 23, 2020 · 3e0bad6 · 3e0bad6
1 parent 4449db2
commit 3e0bad6
Show file tree

Hide file tree

Showing 13 changed files with 166 additions and 219 deletions.
diff --git a/include/iscGenerationClass.h b/include/iscGenerationClass.h
@@ -45,7 +45,7 @@
 
 //define threshold for loop closure detection
 #define GEOMETRY_THRESHOLD 0.67
-#define INTENSITY_THRESHOLD 0.93
+#define INTENSITY_THRESHOLD 0.91
 
 typedef cv::Mat ISCDescriptor; 
 

diff --git a/include/iscOptimizationClass.h b/include/iscOptimizationClass.h
@@ -44,7 +44,7 @@
 //local lib
 #include "lidarOptimization.h"
 
-#define LOOPCLOSURE_THRESHOLD 31
+#define LOOPCLOSURE_THRESHOLD 41
 //stop loop check for the next N frames if loop is identified
 #define STOP_LOOP_CHECK_COUNTER 40
 class ISCOptimizationClass

diff --git a/include/laserProcessingClass.h b/include/laserProcessingClass.h
@@ -1,11 +1,9 @@
-// Author of ISCLOAM: Wang Han 
+// Author of FLOAM: Wang Han 
 // Email [email protected]
 // Homepage https://wanghan.pro
-
 #ifndef _LASER_PROCESSING_CLASS_H_
 #define _LASER_PROCESSING_CLASS_H_
 
-
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
@@ -32,20 +30,17 @@ class PointsInfo{
 	PointsInfo(int layer_in, double time_in);
 };
 
+
+//这里所有的数据传递全部采用指针或引用的形式来提高传递效率
 class LaserProcessingClass 
 {
     public:
     	LaserProcessingClass();
 		void init(lidar::Lidar lidar_param_in);
-		void preFiltering(const pcl::PointCloud<pcl::PointXYZI>::Ptr pc_in, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out);
-		void featureExtraction(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_sharp, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_lessSharp, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_flat, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_lessFlat);
-		void featureExtractionFromSector(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, std::vector<Double2d>& cloudCurvature, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_edge_sharp, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_edge_lessSharp, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_surf_flat, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_surf_lessFlat);	
+		void featureExtraction(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_edge, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_surf);
+		void featureExtractionFromSector(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, std::vector<Double2d>& cloudCurvature, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_edge, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_surf);	
 	private:
      	lidar::Lidar lidar_param;
-
-     	pcl::CropBox<pcl::PointXYZI> closePointFilter;
-     	pcl::CropBox<pcl::PointXYZI> farPointFilter;
-     	pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;
 };
 
 

diff --git a/launch/iscloam.launch b/launch/iscloam.launch
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
 
-    <node pkg="rosbag" type="play" name="rosbag_play" args="--clock $(find iscloam)/dataset/2011_09_30_0018.bag"/> 
+    <node pkg="rosbag" type="play" name="rosbag_play" args="--clock $(find iscloam)/dataset/kitti/2011_09_30_0018.bag"/> 
 
     <!--- Sim Time -->
     <param name="/use_sim_time" value="true" />
@@ -10,7 +10,9 @@
     <param name="ring_height" type="int" value="20" />
     <param name="ring_width" type="int" value="60" />
     <param name="vertical_angle" type="double" value="2.0" />
-    <!--- -->
+    <param name="max_dis" type="double" value="90.0" />
+    <param name="min_dis" type="double" value="3.0" />
+
     <node pkg="iscloam" type="iscloam_odom_estimation_node" name="iscloam_odom_estimation_node" output="screen"/>
     <node pkg="iscloam" type="iscloam_laser_processing_node" name="iscloam_laser_processing_node" output="screen"/>
     <node pkg="iscloam" type="iscloam_isc_generation_node" name="iscloam_isc_generation_node" output="screen"/>
@@ -38,3 +40,4 @@
     </node>
 
 </launch>
+
diff --git a/launch/iscloam_mapping.launch b/launch/iscloam_mapping.launch
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
 
-    <node pkg="rosbag" type="play" name="rosbag_play" args="--clock $(find iscloam)/dataset/2011_09_30_0018.bag"/> 
+    <node pkg="rosbag" type="play" name="rosbag_play" args="--clock $(find iscloam)/dataset/kitti/2011_09_30_0018.bag"/> 
 
     <!--- Sim Time -->
     <param name="/use_sim_time" value="true" />
@@ -10,6 +10,8 @@
     <param name="ring_height" type="int" value="20" />
     <param name="ring_width" type="int" value="60" />
     <param name="vertical_angle" type="double" value="2.0" />
+    <param name="max_dis" type="double" value="90.0" />
+    <param name="min_dis" type="double" value="3.0" />
     <!--- -->
     <node pkg="iscloam" type="iscloam_odom_estimation_node" name="iscloam_odom_estimation_node" output="screen"/>
     <node pkg="iscloam" type="iscloam_laser_processing_node" name="iscloam_laser_processing_node" output="screen"/>
@@ -39,3 +41,4 @@
     </node>
 
 </launch>
+
diff --git a/rviz/iscloam.rviz b/rviz/iscloam.rviz
@@ -5,8 +5,9 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /TF1/Frames1
+        - /iscloam_result1
       Splitter Ratio: 0.5
-    Tree Height: 229
+    Tree Height: 430
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -64,7 +65,7 @@ Visualization Manager:
       Unreliable: false
       Value: true
     - Class: rviz/Image
-      Enabled: true
+      Enabled: false
       Image Topic: /isc
       Max Value: 1
       Median window: 5
@@ -74,12 +75,14 @@ Visualization Manager:
       Queue Size: 2
       Transport Hint: raw
       Unreliable: false
-      Value: true
+      Value: false
     - Class: rviz/TF
       Enabled: true
       Frame Timeout: 15
       Frames:
         All Enabled: false
+        aft_mapped:
+          Value: true
         cam00:
           Value: false
         cam01:
@@ -92,8 +95,6 @@ Visualization Manager:
           Value: false
         map:
           Value: false
-        odom:
-          Value: false
         odom_final:
           Value: true
         velodyne:
@@ -119,7 +120,7 @@ Visualization Manager:
             velodyne:
               {}
           map:
-            odom:
+            aft_mapped:
               {}
             odom_final:
               {}
@@ -165,7 +166,7 @@ Visualization Manager:
       Length: 0.30000001192092896
       Line Style: Lines
       Line Width: 0.029999999329447746
-      Name: GT
+      Name: ground_truth
       Offset:
         X: 0
         Y: 0
@@ -211,7 +212,7 @@ Visualization Manager:
       Length: 0.30000001192092896
       Line Style: Lines
       Line Width: 0.029999999329447746
-      Name: FinalPath
+      Name: iscloam_result
       Offset:
         X: 0
         Y: 0
@@ -252,7 +253,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 597.32763671875
+      Distance: 166.35633850097656
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -267,22 +268,22 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.1053975820541382
+      Pitch: 1.0253976583480835
       Target Frame: body
       Value: XYOrbit (rviz)
-      Yaw: 3.120398998260498
+      Yaw: 3.0853984355926514
     Saved: ~
 Window Geometry:
   Camera_View:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 956
+  Height: 1063
   Hide Left Dock: false
   Hide Right Dock: false
   ISC_Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001ef00000323fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000168000000c900fffffffb00000012004900530043005f0049006d00610067006501000001ab000000d30000001600fffffffb0000001600430061006d006500720061005f00560069006500770100000284000000dc0000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000600490053004300000002d9000000c70000000000000000000000010000010f00000323fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000323000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000083d00000039fc0100000002fb0000000800540069006d006501000000000000083d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005330000032300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001ef0000038efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000231000000c900fffffffb00000012004900530043005f0049006d00610067006500000001dc000000ef0000001600fffffffb0000001600430061006d006500720061005f00560069006500770100000274000001570000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000600490053004300000002d9000000c70000000000000000000000010000010f0000038efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000038e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000083d00000039fc0100000002fb0000000800540069006d006501000000000000083d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005330000038e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

diff --git a/rviz/iscloam_mapping.rviz b/rviz/iscloam_mapping.rviz
@@ -5,9 +5,11 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
-        - /TF1/Status1
+        - /TF1
+        - /TF1/Frames1
+        - /map1
       Splitter Ratio: 0.5
-    Tree Height: 296
+    Tree Height: 435
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -65,7 +67,7 @@ Visualization Manager:
       Unreliable: false
       Value: true
     - Class: rviz/Image
-      Enabled: true
+      Enabled: false
       Image Topic: /isc
       Max Value: 1
       Median window: 5
@@ -75,12 +77,14 @@ Visualization Manager:
       Queue Size: 2
       Transport Hint: raw
       Unreliable: false
-      Value: true
+      Value: false
     - Class: rviz/TF
       Enabled: true
       Frame Timeout: 15
       Frames:
         All Enabled: false
+        aft_mapped:
+          Value: false
         cam00:
           Value: false
         cam01:
@@ -93,15 +97,13 @@ Visualization Manager:
           Value: false
         map:
           Value: false
-        odom:
-          Value: false
         odom_final:
           Value: true
         velodyne:
           Value: false
         world:
           Value: true
-      Marker Scale: 5
+      Marker Scale: 100
       Name: TF
       Show Arrows: false
       Show Axes: true
@@ -120,7 +122,7 @@ Visualization Manager:
             velodyne:
               {}
           map:
-            odom:
+            aft_mapped:
               {}
             odom_final:
               {}
@@ -160,13 +162,13 @@ Visualization Manager:
       Buffer Length: 1
       Class: rviz/Path
       Color: 239; 41; 41
-      Enabled: false
+      Enabled: true
       Head Diameter: 0.30000001192092896
       Head Length: 0.20000000298023224
       Length: 0.30000001192092896
       Line Style: Lines
       Line Width: 0.029999999329447746
-      Name: GT
+      Name: ground_truth
       Offset:
         X: 0
         Y: 0
@@ -178,7 +180,7 @@ Visualization Manager:
       Shaft Length: 0.10000000149011612
       Topic: /gt/trajectory
       Unreliable: false
-      Value: false
+      Value: true
     - Alpha: 1
       Buffer Length: 1
       Class: rviz/Path
@@ -212,7 +214,7 @@ Visualization Manager:
       Length: 0.30000001192092896
       Line Style: Lines
       Line Width: 0.029999999329447746
-      Name: FinalPath
+      Name: iscloam_result
       Offset:
         X: 0
         Y: 0
@@ -228,27 +230,27 @@ Visualization Manager:
     - Alpha: 0.699999988079071
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 10.49564266204834
-        Min Value: -24.316038131713867
+        Max Value: 11.07189655303955
+        Min Value: -20.349206924438477
         Value: true
       Axis: Z
       Channel Name: intensity
       Class: rviz/PointCloud2
       Color: 255; 255; 255
-      Color Transformer: Intensity
+      Color Transformer: AxisColor
       Decay Time: 0
       Enabled: true
       Invert Rainbow: false
       Max Color: 0; 0; 0
-      Max Intensity: 1
+      Max Intensity: 0.9900000095367432
       Min Color: 238; 238; 236
       Min Intensity: 0
       Name: map
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
-      Size (Pixels): 4
-      Size (m): 0.6000000238418579
+      Size (Pixels): 5
+      Size (m): 0.699999988079071
       Style: Boxes
       Topic: /map
       Unreliable: false
@@ -282,26 +284,21 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Class: rviz/XYOrbit
-      Distance: 282.2178039550781
+      Angle: 4.700019359588623
+      Class: rviz/TopDownOrtho
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
-      Focal Point:
-        X: 30.172157287597656
-        Y: -19.642745971679688
-        Z: 0
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.1053975820541382
-      Target Frame: body
-      Value: XYOrbit (rviz)
-      Yaw: 3.125399112701416
+      Scale: 1.6136374473571777
+      Target Frame: <Fixed Frame>
+      Value: TopDownOrtho (rviz)
+      X: 109.91279602050781
+      Y: -27.7003116607666
     Saved: ~
 Window Geometry:
   Camera_View:
@@ -313,7 +310,7 @@ Window Geometry:
   Hide Right Dock: false
   ISC_Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001ef00000323fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001ab000000c900fffffffb00000012004900530043005f0049006d00610067006501000001ee000000be0000001600fffffffb0000001600430061006d006500720061005f005600690065007701000002b2000000ae0000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000600490053004300000002d9000000c70000000000000000000000010000010f00000323fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000323000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000083d00000039fc0100000002fb0000000800540069006d006501000000000000083d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005330000032300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001ef00000323fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000236000000c900fffffffb00000012004900530043005f0049006d00610067006500000001ee000000be0000001600fffffffb0000001600430061006d006500720061005f00560069006500770100000279000000e70000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000600490053004300000002d9000000c70000000000000000000000010000010f00000323fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000323000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000083d00000039fc0100000002fb0000000800540069006d006501000000000000083d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005330000032300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -324,4 +321,4 @@ Window Geometry:
     collapsed: false
   Width: 2109
   X: 539
-  Y: 55
+  Y: 27