test kaist urban27, find lio-sam problem

coolaogege · Jan 1, 2023 · 204b042 · 204b042
1 parent 1d3d557
commit 204b042
Show file tree

Hide file tree

Showing 5 changed files with 34 additions and 36 deletions.
diff --git a/.gitignore b/.gitignore
@@ -0,0 +1 @@
+.vscode/*
diff --git a/config/kaist_camera.yaml → config/KAIST_camera.yaml b/config/kaist_camera.yaml → config/KAIST_camera.yaml
@@ -5,7 +5,7 @@ project_name: "lvi_sam"
 
 #common parameters
 imu_topic: "/imu/data_raw"
-image_topic: "/stereo/left/image_raw"
+image_topic: "/stereo/left/image_raw0"
 point_cloud_topic: "lvi_sam/lidar/deskew/cloud_deskewed"
 
 # Lidar Params
@@ -36,6 +36,7 @@ acc_w: 0.002        # accelerometer bias random work noise standard deviation.
 gyr_w: 4.0e-5       # gyroscope bias random work noise standard deviation.
 g_norm: 9.805       #
 
+## this param is for KAIST urban 27
 # Extrinsic parameter between IMU and Camera.
 estimate_extrinsic: 0   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                         # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
@@ -54,7 +55,7 @@ extrinsicTranslation: !!opencv-matrix
    rows: 3
    cols: 1
    dt: d
-   data: [1.14967300e-01, 0.00000000e+00, -2.69298700e+00]
+   data: [1.71239, 0.247401, -0.11589]  
 
 #feature traker paprameters
 max_cnt: 150            # max feature number in feature tracking

diff --git a/config/kaist_lidar.yaml → config/KAIST_lidar.yaml b/config/kaist_lidar.yaml → config/KAIST_lidar.yaml
@@ -4,7 +4,7 @@ PROJECT_NAME: "lvi_sam"
 lvi_sam:
 
   # Topics
-  pointCloudTopic: "/ns1/velodyne_points"               # Point cloud data
+  pointCloudTopic: "/ns2/velodyne_points"               # Point cloud data
   imuTopic: "/imu/data_raw"                         # IMU data
 
   # Heading
@@ -21,18 +21,19 @@ lvi_sam:
   downsampleRate: 1                           # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1 
 
   # IMU Settings
-  imuAccNoise: 3.9939570888238808e-03
-  imuGyrNoise: 1.5636343949698187e-03
-  imuAccBiasN: 6.4356659353532566e-05
-  imuGyrBiasN: 3.5640318696367613e-05
-  imuGravity: 9.80511
+  imuAccNoise: 2.0e-02
+  imuGyrNoise: 5.0e-03
+  imuAccBiasN: 5.0e-05
+  imuGyrBiasN: 3.0e-05
+  imuGravity: 9.81
 
+  ## this param is for KAIST urban 27
   ###################### extrinsic between IMU and LiDAR  ###########################
   ###################### T_IMU_LiDAR, LiDAR -> IMU       ###########################
-  extrinsicTranslation: [-0.02342361, 0, -3.298731]
+  extrinsicTranslation: [-0.264623, 0.431973, 0.24043]
   extrinsicRotation: [-0.516377, -0.702254, -0.490096,
-                       0.491997, -0.711704,  0.501414,
-                      -0.700923,  0.0177927, 0.713015 ]
+                       0.491997, -0.711704, 0.501414,
+                      -0.700923, 0.0177927, 0.713015]
 
   ########################## IMU property config  ###########################
   ## 对绝大多数IMU来说，下面三个值分别是"+z", "+y", "+x" (for most of IMUs, the following config is "+z", "+y", "+x")
@@ -60,22 +61,22 @@ lvi_sam:
   rotation_tollerance: 1000 # radians
 
   # CPU Params
-  numberOfCores: 4                              # number of cores for mapping optimization
+  numberOfCores: 10                              # number of cores for mapping optimization
   mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency
 
   # Surrounding map
   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)
+  surroundingkeyframeAddingAngleThreshold: 1.5  # radians, regulate keyframe adding threshold
+  surroundingKeyframeDensity: 0.5               # meters, downsample surrounding keyframe poses   
+  surroundingKeyframeSearchRadius: 30.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)
 
   # Loop closure
   loopClosureEnableFlag: true
-  surroundingKeyframeSize: 25                   # submap size (when loop closure enabled)
-  historyKeyframeSearchRadius: 20.0             # meters, key frame that is within n meters from current pose will be considerd for loop closure
+  surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
+  historyKeyframeSearchRadius: 30.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
-  historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment
+  historyKeyframeFitnessScore: 2.0              # icp threshold, the smaller the better alignment
 
   # Visualization
   globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius

diff --git a/launch/kaist.launch → launch/KAIST.launch b/launch/kaist.launch → launch/KAIST.launch
@@ -1,8 +1,8 @@
 <launch>
 	<!-- Lidar odometry param -->
-	<rosparam file="$(find lvi_sam)/config/kaist_lidar.yaml" command="load" />
+	<rosparam file="$(find lvi_sam)/config/KAIST_lidar.yaml" command="load" />
 	<!-- VINS config -->
-	<param name="vins_config_file" type="string" value="$(find lvi_sam)/config/kaist_camera.yaml" />
+	<param name="vins_config_file" type="string" value="$(find lvi_sam)/config/KAIST_camera.yaml" />
     <!-- Image conversion -->
 	<node pkg="image_transport" type="republish" name="republish" args="compressed in:=/camera/image_raw raw out:=/camera/image_raw" output="screen" respawn="true" />
 

diff --git a/launch/include/config/rviz.rviz b/launch/include/config/rviz.rviz
@@ -3,14 +3,9 @@ Panels:
     Help Height: 0
     Name: Displays
     Property Tree Widget:
-      Expanded:
-        - /Links1
-        - /Visual Feature1
-        - /Visual Odometry1
-        - /Lidar Odometry1
-        - /Loop Closure1
+      Expanded: ~
       Splitter Ratio: 0.45996275544166565
-    Tree Height: 328
+    Tree Height: 304
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -26,7 +21,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: Dense Map
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -285,7 +280,7 @@ Visualization Manager:
           Class: rviz/PointCloud2
           Color: 255; 255; 255
           Color Transformer: Intensity
-          Decay Time: 100
+          Decay Time: 1e+6
           Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
@@ -455,25 +450,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 31.42701530456543
+      Distance: 85.51264190673828
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
+        X: 10.70701789855957
+        Y: -1.1428730487823486
+        Z: -0.717384934425354
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7853981852531433
+      Pitch: 0.5653994083404541
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 0.7853981852531433
+      Yaw: 5.953587055206299
     Saved: ~
 Window Geometry:
   Displays:
@@ -487,7 +482,7 @@ Window Geometry:
     collapsed: false
   Loop image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000237000004d1fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000008600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000560000019e0000012600fffffffb0000001a0049006d0061006700650020004600650061007400750072006501000001fd0000018c0000001e00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000014004c006f006f007000200069006d00610067006500000004790000010a0000001e00fffffffb000000160049006d0061006700650020004400650070007400680100000392000001950000001e00fffffffb0000000a0049006d00610067006501000004a3000000dc0000000000000000000000010000011100000551fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002e00000551000000ec00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000040fc0100000002fb0000000800540069006d00650000000000000004b0000006d800fffffffb0000000800540069006d00650100000000000004500000000000000000000007c0000004d100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000237000004bffc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000008f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000005f000001980000013800fffffffb0000001a0049006d006100670065002000460065006100740075007200650100000200000001860000002700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000014004c006f006f007000200069006d00610067006500000004790000010a0000002700fffffffb000000160049006d006100670065002000440065007000740068010000038f0000018f0000002700fffffffb0000000a0049006d00610067006501000004a3000000dc0000000000000000000000010000011100000551fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002e000005510000010700fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000040fc0100000002fb0000000800540069006d00650000000000000004b00000074c00fffffffb0000000800540069006d00650100000000000004500000000000000000000007c0000004bf00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time: