Merge branch 'non-cuda' into main

HKUST-Aerial-Robotics · Dec 11, 2024 · a9f1b07 · a9f1b07
2 parents 374cfae + c2a5b7c
commit a9f1b07
Show file tree

Hide file tree

Showing 95 changed files with 2,795 additions and 1,737 deletions.
diff --git a/.gitignore b/.gitignore
@@ -27,4 +27,6 @@ data_analysis/.ipynb_checkpoints/
 d2pgo/posegraph-graphviz.*
 d2pgo/scripts/notebooks/posegraph-graphviz.*
 */.DS_Store
-.DS_Store
+.DS_Store
+build/
+*.trt
diff --git a/.vscode/launch.json b/.vscode/launch.json
@@ -0,0 +1,21 @@
+{
+    // 使用 IntelliSense 了解相关属性。 
+    // 悬停以查看现有属性的描述。
+    // 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [{
+        "name": "(gdb) d2vins",
+        "type": "cppdbg",
+        "request": "launch",
+        "program": "/home/xuhao/swarm_ws/devel/lib/d2vins/d2vins_node",
+        "MIMode": "gdb",
+        "cwd": "${workspaceFolder}",
+        "setupCommands": [
+            {
+                "description": "Enable pretty-printing for gdb",
+                "text": "-enable-pretty-printing",
+                "ignoreFailures": true
+            }
+        ]
+    }]
+}
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -0,0 +1,3 @@
+{
+    "cmake.sourceDirectory": "/home/xuhao/swarm_ws/src/D2SLAM/d2vins"
+}
diff --git a/README.md b/README.md
@@ -117,8 +117,6 @@ max_sld_win_size: 11 # This parameter specifies the maximum length of the slidin
 landmark_estimate_tracks: 4 # This parameter specifies the threshold for the number of measurements of a landmark to be used for estimation in D2SLAM.
 min_solve_frames: 6 # This parameter specifies the number of keyframes in the sliding window to start the estimation process in D2SLAM.
 
-#solver
-multiple_thread: 1 #  This parameter specifies whether to use multiple threads in the Ceres solver in D2VINS.
 
 #outlier rejection
 thres_outlier : 10.0 # This parameter specifies the threshold to enable outlier detection in D2SLAM.

diff --git a/config/GRACO/GRACO_multi.yaml b/config/GRACO/GRACO_multi.yaml
@@ -61,9 +61,6 @@ max_sld_win_size: 11
 landmark_estimate_tracks: 4 #when use depth or stereo, 3 is OK.
 min_solve_frames: 5
 
-#solver
-multiple_thread: 1
-
 #outlier rejection
 thres_outlier : 10.0
 perform_outlier_rejection_num: 10

diff --git a/config/GRACO/GRACO_single.yaml b/config/GRACO/GRACO_single.yaml
@@ -61,9 +61,6 @@ max_sld_win_size: 11
 landmark_estimate_tracks: 4 #when use depth or stereo, 3 is OK.
 min_solve_frames: 5
 
-#solver
-multiple_thread: 1
-
 #outlier rejection
 thres_outlier : 10.0
 perform_outlier_rejection_num: 10

diff --git a/config/GRACO/GRACO_single_gnd.yaml b/config/GRACO/GRACO_single_gnd.yaml
@@ -61,9 +61,6 @@ max_sld_win_size: 11
 landmark_estimate_tracks: 4 #when use depth or stereo, 3 is OK.
 min_solve_frames: 10
 
-#solver
-multiple_thread: 1
-
 #outlier rejection
 thres_outlier : 10.0
 perform_outlier_rejection_num: 100

diff --git a/config/NTU_VIRAL/NTU_VIRAL_multi.yaml b/config/NTU_VIRAL/NTU_VIRAL_multi.yaml
@@ -82,9 +82,6 @@ max_sld_win_size: 11
 landmark_estimate_tracks: 4 #when use depth or stereo, 3 is OK.
 min_solve_frames: 9
 
-#solver
-multiple_thread: 1
-
 #outlier rejection
 thres_outlier : 10.0
 perform_outlier_rejection_num: 10

diff --git a/config/NTU_VIRAL/NTU_VIRAL_single.yaml b/config/NTU_VIRAL/NTU_VIRAL_single.yaml
@@ -82,9 +82,6 @@ max_sld_win_size: 11
 landmark_estimate_tracks: 4 #when use depth or stereo, 3 is OK.
 min_solve_frames: 9
 
-#solver
-multiple_thread: 1
-
 #outlier rejection
 thres_outlier : 10.0
 perform_outlier_rejection_num: 10

diff --git a/config/d2slam.rviz b/config/d2slam.rviz
@@ -7,12 +7,13 @@ Panels:
         - /Global Options1
         - /Grid1
         - /Axes1
+        - /VIOGroup1
+        - /VIOGroup1/VIOPath1
+        - /VIOGroup1/MarginedCloud1
         - /VIOGroup1/MarginedCloud1/Autocompute Value Bounds1
-        - /Remote1
-        - /PGO1
         - /DenseMapping1/PointCloud21/Autocompute Value Bounds1
       Splitter Ratio: 0.5429141521453857
-    Tree Height: 916
+    Tree Height: 592
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -45,7 +46,7 @@ Panels:
         - /DenseMapping1/PointCloud21
         - /DenseMapping1/PointCloud21/Autocompute Value Bounds1
       Splitter Ratio: 0.5
-    Tree Height: 443
+    Tree Height: 441
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -85,13 +86,13 @@ Visualization Manager:
         - Alpha: 1
           Buffer Length: 1
           Class: rviz/Path
-          Color: 25; 255; 0
-          Enabled: false
+          Color: 0; 0; 0
+          Enabled: true
           Head Diameter: 0.30000001192092896
           Head Length: 0.20000000298023224
           Length: 0.30000001192092896
           Line Style: Billboards
-          Line Width: 0.05000000074505806
+          Line Width: 0.5
           Name: VIOPath
           Offset:
             X: 0
@@ -105,7 +106,7 @@ Visualization Manager:
           Shaft Length: 0.10000000149011612
           Topic: /d2vins/path
           Unreliable: false
-          Value: false
+          Value: true
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
@@ -137,31 +138,31 @@ Visualization Manager:
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
-            Max Value: 3.3091039657592773
-            Min Value: -0.583119809627533
+            Max Value: 7.037196636199951
+            Min Value: 0
             Value: true
           Axis: Z
           Channel Name: intensity
           Class: rviz/PointCloud
           Color: 255; 255; 255
           Color Transformer: AxisColor
           Decay Time: 1000
-          Enabled: false
+          Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
           Min Color: 0; 0; 0
           Name: MarginedCloud
           Position Transformer: XYZ
           Queue Size: 10
           Selectable: true
-          Size (Pixels): 2
+          Size (Pixels): 3
           Size (m): 0.029999999329447746
-          Style: Boxes
+          Style: Points
           Topic: /d2vins/margined_cloud
           Unreliable: false
           Use Fixed Frame: true
           Use rainbow: true
-          Value: false
+          Value: true
         - Class: rviz/MarkerArray
           Enabled: true
           Marker Topic: /d2vins/slding_window
@@ -1031,6 +1032,18 @@ Visualization Manager:
           Value: true
       Enabled: true
       Name: DenseMapping
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /gopro/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: compressed
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 255; 255; 255
@@ -1057,25 +1070,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 352.3196105957031
+      Distance: 391.67437744140625
       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: 54.529605865478516
-        Y: -113.03623962402344
-        Z: 0.6943408250808716
+        X: -23.375328063964844
+        Y: -61.1906623840332
+        Z: 0.6943391561508179
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3452037572860718
+      Pitch: 0.46020346879959106
       Target Frame: world
-      Yaw: 4.141939640045166
+      Yaw: 2.6519391536712646
     Saved:
       - Class: rviz/XYOrbit
         Distance: 9.527398109436035
@@ -1100,10 +1113,12 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1022
+  Height: 704
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 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
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000028dfc0200000013fc000000160000028d0000000000fffffffa000000000100000002fb0000000a0049006d00610067006502ffff8020ffff8020000001df000003a2fb000000100044006900730070006c0061007900730000000000000001df0000015600fffffffb0000000a0049006d0061006700650100000016000001d30000000000000000fb0000001200530065006c0065006300740069006f006e0000000028000003990000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000017900000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001200720061007700200049006d0061006700650000000028000000300000000000000000fb00000012007200610077005f0069006d0061006700650000000028000000f90000000000000000fb0000001a0074007200610063006b0065006400200069006d0061006700650000000028000001d50000000000000000fb00000020006c006f006f0070005f006d0061007400630068005f0069006d0061006700650000000192000000d60000000000000000fb000000100044006900730070006c0061007900730000000027000005fa000000c900fffffffc000000280000011e0000000000fffffffa000000000100000002fb0000001200720061007700200049006d0061006700650000000000ffffffff0000000000000000fb0000001a0074007200610063006b0065006400200069006d0061006700650100000000000002370000000000000000fb0000001000410052005f0069006d0061006700650100000373000000160000000000000000fb0000001200720061007700200069006d006100670065010000038f000000160000000000000000fb00000020006c006f006f0070005f006d0061007400630068005f0049006d0061006700650000000027000003420000000000000000fb0000000a0049006d0061006700650000000000ffffffff0000000000000000000000010000015f0000052afc0200000004fc000000160000019b0000000000fffffffa000000000100000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0049006d006100670065000000027d000000ad0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000270000052a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000048800000158fc0100000002fb0000000a0049006d0061006700650300000077000000cd0000012b000000d4fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009a000000060fc0100000002fb0000000800540069006d00650000000000000009a0000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004880000028d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -1112,6 +1127,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 2928
-  X: 1376
-  Y: 157
+  Width: 1160
+  X: 72
+  Y: 27
diff --git a/config/gopro/camera.yaml b/config/gopro/camera.yaml
@@ -0,0 +1,16 @@
+%YAML:1.0
+---
+model_type: PINHOLE
+camera_name: camera
+image_width: 960
+image_height: 540
+distortion_parameters:
+   k1: 0.004565659722216166
+   k2: -1.4395510119349526e-05
+   p1: -0.0009920698300344784
+   p2: 0.0011732462327059988
+projection_parameters:
+   fx: 459.5570201339875
+   fy: 459.14663571215164
+   cx: 481.0475510558175
+   cy: 266.7803445517729