Added yaml concerning the euroc dataset

rviz/online.rviz

@@ -9,7 +9,7 @@ Panels:
         - /Landmarks1/Namespaces1
         - /Trajectory1/Namespaces1
       Splitter Ratio: 0.5
-    Tree Height: 242
+    Tree Height: 474
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -28,7 +28,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Input image
+    SyncSource: Image
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -54,13 +54,14 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Class: rviz/Axes
+    - Alpha: 1
+      Class: rviz/Axes
       Enabled: true
-      Length: 0.1
+      Length: 0.10000000149011612
       Name: Axes
-      Radius: 0.01
-      Show Trail: true
+      Radius: 0.009999999776482582
       Reference Frame: base_footprint
+      Show Trail: true
       Value: true
     - Class: rviz/TF
       Enabled: true
@@ -99,10 +100,6 @@ Visualization Manager:
       Marker Topic: /wolf_ros_node/graph_factors
       Name: Factors
       Namespaces:
-        factors_APRILTAG PROC: true
-        factors_IMU PROC: true
-        factors_text_APRILTAG PROC: false
-        factors_text_IMU PROC: false
         factors_text_unnamed_processor: false
         factors_unnamed_processor: true
       Queue Size: 1
@@ -112,8 +109,7 @@ Visualization Manager:
       Marker Topic: /wolf_ros_node/graph_landmarks
       Name: Landmarks
       Namespaces:
-        landmarks: true
-        landmarks_text: true
+        {}
       Queue Size: 1
       Value: true
     - Class: rviz/MarkerArray
@@ -125,38 +121,26 @@ Visualization Manager:
         frames_text: false
       Queue Size: 1
       Value: true
-    - Class: rviz/Image
+    - Class: rviz/Marker
       Enabled: true
-      Image Topic: /camera_simu/image_raw
-      Max Value: 1
-      Median window: 5
-      Min Value: 0
-      Name: Input image
-      Normalize Range: true
-      Queue Size: 2
-      Transport Hint: raw
-      Unreliable: false
+      Marker Topic: visualization_marker
+      Name: Ground truth
+      Namespaces:
+        {}
+      Queue Size: 100
       Value: true
     - Class: rviz/Image
       Enabled: true
-      Image Topic: /image_debug
+      Image Topic: /cam0/image_raw
       Max Value: 1
       Median window: 5
       Min Value: 0
-      Name: AprilTag Detections
+      Name: Image
       Normalize Range: true
       Queue Size: 2
       Transport Hint: raw
       Unreliable: false
       Value: true
-    - Class: rviz/Marker
-      Enabled: true
-      Marker Topic: visualization_marker
-      Name: Ground truth
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -206,16 +190,14 @@ Visualization Manager:
       Yaw: 5.045364856719971
     Saved: ~
 Window Geometry:
-  AprilTag Detections:
-    collapsed: false
   Displays:
     collapsed: false
-  Height: 998
+  Height: 1016
   Hide Left Dock: false
   Hide Right Dock: false
-  Input image:
+  Image:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -224,6 +206,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1267
-  X: 653
+  Width: 960
+  X: 0
   Y: 27

yaml/camera_euroc_mav0.yaml

@@ -4,21 +4,23 @@ camera_name: narrow_stereo
 camera_matrix:
   rows: 3
   cols: 3
-  data: [458, 0.000000, 367.215, 
-         0.000000, 457.296, 248.375, 
+  data: [458, 0.000000, 367.215,
+         0.000000, 457.296, 248.375,
          0.000000, 0.000000, 1.000000]
 distortion_model: plumb_bob
 distortion_coefficients:
   rows: 1
   cols: 5
-  data: [-0.28340811, 0.07395907, 0.000000, 0.000000, 0.000000]
+  data: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.000000]
 rectification_matrix:
   rows: 3
   cols: 3
-  data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
+  data: [1.000000, 0.000000, 0.000000,
+         0.000000, 1.000000, 0.000000,
+         0.000000, 0.000000, 1.000000]
 projection_matrix:
   rows: 3
   cols: 4
-  data: [458, 0.000000, 367.215, 0.000000, 
-         0.000000, 457.296, 248.375, 0.000000, 
+  data: [458, 0.000000, 367.215, 0.000000,
+         0.000000, 457.296, 248.375, 0.000000,
          0.000000, 0.000000, 1.000000, 0.000000]

yaml/camera_euroc_mav0_notangdist.yaml

+width: 752
+height: 480
+camera_name: narrow_stereo
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [458, 0.000000, 367.215,
+         0.000000, 457.296, 248.375,
+         0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.28340811, 0.07395907, 0.000000, 0.000000, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.000000, 0.000000, 0.000000,
+         0.000000, 1.000000, 0.000000,
+         0.000000, 0.000000, 1.000000]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [458, 0.000000, 367.215, 0.000000,
+         0.000000, 457.296, 248.375, 0.000000,
+         0.000000, 0.000000, 1.000000, 0.000000]

yaml/demo_visual_odometry_euroc.yaml

@@ -14,18 +14,16 @@ config:
   problem:
     node_rate: 100
     follow: "tree_manager_full_history.yaml"
-    frame_structure: "POV"
+    frame_structure: "PO"
     dimension: 3
     prior:
       mode: "factor"
       $state:
         P: [0,0,0]
         O: [0,0,0,1]  # x,y,z,w
-        V: [0,0,0]
       $sigma:
        P: [0.00001, 0.00001, 0.00001]
        O: [0.01, 0.01, 0.01]
-       V: [0.1, 0.1, 0.1]
       time_tolerance: 0.01
 
   solver:
@@ -38,7 +36,7 @@ config:
       plugin: "vision"
       extrinsic:
         pose: [0,0,0,  0,0,0,1]
-      follow: "camera_euroc_mav0.yaml"
+      follow: "camera_euroc_mav0_notangdist.yaml"
 
   processors:
     -
@@ -52,7 +50,7 @@ config:
     -
       package: "wolf_ros_vision"
       type: "SubscriberCamera"
-      topic: "/TODO"
+      topic: "/cam0/image_raw"
       sensor_name: "CAMERA"
 
   ROS publisher:

yaml/processor_visual_odometry.yaml

 algorithm:
     time tolerance: 0.005
     voting_active: true
-    
+
 klt_params:
     tracker_width: 21
     tracker_height: 21
     nlevels_pyramids: 3
-    klt_max_err: 0.2
\ No newline at end of file
+    klt_max_err: 0.2
+
+fast_params:
+    threshold_fast: 30
+    non_max_suppresion: true
+
+min_thresh_tracks: 500