Merge branch 'devel' of...

Merge branch 'devel' of https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_ros/demos/wolf_demo_visual_odometry into devel

rviz/online.rviz

@@ -10,7 +10,7 @@ Panels:
         - /Trajectory1/Namespaces1
         - /image_raw1
       Splitter Ratio: 0.5
-    Tree Height: 196
+    Tree Height: 140
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -148,6 +148,18 @@ Visualization Manager:
       Transport Hint: raw
       Unreliable: false
       Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /debug_image_preprocessor
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -202,7 +214,9 @@ Window Geometry:
   Height: 1016
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 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
+  Image:
+    collapsed: false
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -211,8 +225,8 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1920
-  X: 0
+  Width: 1848
+  X: 72
   Y: 27
   debug:
     collapsed: false

yaml/demo_visual_odometry_euroc.yaml

@@ -11,7 +11,7 @@ config:
     print_period: 1           # only if print_problem
 
   problem:
-    node_rate: 100
+    node_rate: 20
     follow: "tree_manager_sliding.yaml"
     frame_structure: "PO"
     dimension: 3
@@ -57,7 +57,7 @@ config:
       type: "PublisherGraph"
       topic: "graph"
       package: "wolf_ros_node"
-      period: 0.1
+      period: 0.2
       viz_scale: 0.3
       text_scale: 0.1
       landmark_text_z_offset: 0.3
@@ -82,6 +82,29 @@ config:
       topic: "/debug_image"
       topic_preprocessor: "/debug_image_preprocessor"
       package: "wolf_ros_vision"
-      period: 0.1
-      follow: "publisher_vision_debug.yaml"
+      period: 0.05
+      tracks:
+        show_id: false
+        size_id: 0.75
+        thickness: 1.5
+        color: "CYAN"
+        min_lum: 100
+        max_lum: 450
+        feature_last:
+          thickness: -1
+          size_pix: 2
+          color: "RED"
+        feature_kfs:
+          thickness: 0.1
+          size_pix: 2
+      tracks_preprocess:
+        show: true
+        show_on_diff_topic: false
+        thickness: 1.5
+        color: "GREEN"
+      landmarks:
+        show_id: false
+        size_id: 0.75
+        size_pix: 5
+        color: "GREY"
 

yaml/processor_visual_odometry.yaml

@@ -4,13 +4,13 @@ keyframe_vote:
     voting_active: true
     # Trigger a new keyframe creation as well as detection of new keypoints in last frame
     # when the track number goes below min_features_for_keyframe in incoming
-    min_features_for_keyframe: 50
+    min_features_for_keyframe: 25
 
 # Use a robust cost function
 apply_loss_function: true
 
 # Select the best new Keypoints when performing detection
-max_new_features: 100
+max_new_features: 10
 
 ####################################
 # ProcessorVisualOdometry parameters
@@ -18,7 +18,7 @@ max_new_features: 100
 fast_params:
     # Threshold on the keypoint pixel intensity (in uchar [0-255]) 
     # the higher, the more selective the detector is
-    threshold_fast: 20
+    threshold_fast: 10
     # Avoids getting multiple keypoints at the same place
     non_max_suppresion: true
     # number of cells used by the active search grid data structure
@@ -37,7 +37,7 @@ klt_params:
     klt_max_err:      0.3
 
 # Keep the number of tracks below 
-max_nb_tracks: 100
+max_nb_tracks: 50
 
 # standard deviation of the pixel reprojection factor 
 std_pix: 1

yaml/publisher_vision_debug.yaml

@@ -2,20 +2,21 @@ tracks:
     show_id: true
     size_id: 0.5
     thickness: 1.5
-    color: "CYAN"
-    min_lum: 100
-    max_lum: 450
-    feature_last:
-        thickness: -1
-        size_pix: 2
-        color: "RED"
-    feature_kfs:
-        thickness: 0.1
-        size_pix: 1
-
-tracks_preprocess:
-    show: false
-    show_on_diff_topic: false
+    min_max_on_alive_tracks: false  
+    color: "CYAN"                   # color of the tracks and feature_kfs
+    min_lum: 100                    # above 0
+    max_lum: 450                    # below 510
+    feature_last:                   # features of the last frame
+        thickness: -1               # -1 -> filled circle
+        size_pix: 2              
+        color: "RED"              
+    feature_kfs:                    # features of the previous keyframes
+        thickness: 0.1    
+        size_pix: 1    
+    
+tracks_preprocess:                  
+    show: false                     # tracks from the preprocess are visible or not
+    show_on_diff_topic: false       # tracks from the preprocess are visible in the same image as the others tracks or not
     thickness: 1.5
     color: "CYAN"
 

yaml/solver.yaml

@@ -5,7 +5,7 @@ max_num_iterations: 20
 function_tolerance: 1e-7
 gradient_tolerance: 1e-9
 use_nonmonotonic_steps: false
-period: 0.5                     
+period: 0.2                     
 n_threads: 8
 compute_cov: false
 verbose: 0
\ No newline at end of file