diff --git a/rviz/online.rviz b/rviz/online.rviz
index d98ed5adfeb243464cf439f093c191ff852da306..f62772260ee44a5eba7cc7c53121d05b797407d3 100644
--- a/rviz/online.rviz
+++ b/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
diff --git a/yaml/camera_euroc_mav0.yaml b/yaml/camera_euroc_mav0.yaml
index c689897c35a232f0879ee4be4c98facc62b53e43..d7b4703eabc462bca36f7a0f6998ad7001e078b6 100644
--- a/yaml/camera_euroc_mav0.yaml
+++ b/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]
diff --git a/yaml/camera_euroc_mav0_notangdist.yaml b/yaml/camera_euroc_mav0_notangdist.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7c237676855cc910c65a44b724e398763c3ee635
--- /dev/null
+++ b/yaml/camera_euroc_mav0_notangdist.yaml
@@ -0,0 +1,26 @@
+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]
diff --git a/yaml/demo_visual_odometry_euroc.yaml b/yaml/demo_visual_odometry_euroc.yaml
index f0650cdbd52bed3bf86cf58c4e8a01072e6c4c51..707d1b9b85a0af2a8dad7373bf737bc1f5bc90d8 100644
--- a/yaml/demo_visual_odometry_euroc.yaml
+++ b/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:
diff --git a/yaml/processor_visual_odometry.yaml b/yaml/processor_visual_odometry.yaml
index 40720c4199489340e61ab6c6d6aeb07092942590..e17e32b85df545d72f52ab024a4a17ad38f350f8 100644
--- a/yaml/processor_visual_odometry.yaml
+++ b/yaml/processor_visual_odometry.yaml
@@ -1,9 +1,15 @@
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