From e06e9c705996d6a2f845236b9e7b0eeac3a648cd Mon Sep 17 00:00:00 2001
From: Mederic Fourmy <mederic.fourmy@gmail.com>
Date: Sat, 23 Apr 2022 12:55:28 +0200
Subject: [PATCH] Automate the undistortion of images
---
launch/demo_visual_odometry.launch | 11 +++++++++++
launch/publisher_camera_info.launch | 11 +++++++++++
rviz/online.rviz | 11 +++++++----
yaml/camera_euroc_mav0.yaml | 4 ++--
yaml/camera_euroc_mav0_notangdist.yaml | 6 +++---
yaml/demo_visual_odometry_euroc.yaml | 4 +++-
6 files changed, 37 insertions(+), 10 deletions(-)
create mode 100644 launch/publisher_camera_info.launch
diff --git a/launch/demo_visual_odometry.launch b/launch/demo_visual_odometry.launch
index 2acef1e..3e02387 100644
--- a/launch/demo_visual_odometry.launch
+++ b/launch/demo_visual_odometry.launch
@@ -20,6 +20,17 @@
args="-d $(find wolf_demo_visual_odometry)/rviz/online.rviz" />
</group>
+
+ <!--Publish a CameraInfo topic reading at a standard wolf camera yaml file-->
+ <include file="$(find wolf_demo_visual_odometry)/launch/publisher_camera_info.launch" />
+
+ <!--Undistort the image using the published camera info and raw images-->
+ <node type="image_proc"
+ name="image_proc"
+ pkg="image_proc"
+ ns="cam0" />
+
+
<!--WOLF-->
<node type="wolf_ros_node"
name="wolf_ros_node"
diff --git a/launch/publisher_camera_info.launch b/launch/publisher_camera_info.launch
new file mode 100644
index 0000000..b03ed81
--- /dev/null
+++ b/launch/publisher_camera_info.launch
@@ -0,0 +1,11 @@
+<launch>
+ <param name="topic_image_raw_in" value="/cam0/image_raw" />
+ <param name="topic_image_info_out" value="/cam0/camera_info" />
+ <param name="camera_yaml_path" value="$(find wolf_demo_visual_odometry)/yaml/camera_euroc_mav0.yaml" />
+
+ <node type="publisher_camera_info.py"
+ name="publisher_camera_info"
+ pkg="wolf_ros_vision"
+ >
+ </node>
+</launch>
diff --git a/rviz/online.rviz b/rviz/online.rviz
index 0cad8af..9f3701e 100644
--- a/rviz/online.rviz
+++ b/rviz/online.rviz
@@ -92,6 +92,8 @@ Visualization Manager:
Marker Topic: /wolf_ros_node/graph_factors
Name: Factors
Namespaces:
+ factors_VISUALODOMETRY PROC: true
+ factors_text_VISUALODOMETRY PROC: true
factors_text_unnamed_processor: true
factors_unnamed_processor: true
Queue Size: 1
@@ -101,7 +103,8 @@ 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
@@ -193,9 +196,9 @@ Visualization Manager:
Value: false
Field of View: 0.7853981852531433
Focal Point:
- X: 0.17180414497852325
- Y: 0.24833546578884125
- Z: -0.264117956161499
+ X: -0.06866473704576492
+ Y: 0.5332714319229126
+ Z: 0.13671444356441498
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
diff --git a/yaml/camera_euroc_mav0.yaml b/yaml/camera_euroc_mav0.yaml
index d7b4703..1fdeacd 100644
--- a/yaml/camera_euroc_mav0.yaml
+++ b/yaml/camera_euroc_mav0.yaml
@@ -21,6 +21,6 @@ rectification_matrix:
projection_matrix:
rows: 3
cols: 4
- data: [458, 0.000000, 367.215, 0.000000,
- 0.000000, 457.296, 248.375, 0.000000,
+ data: [355.63, 0.000000, 362.27, 0.000000,
+ 0.000000, 417.16, 249.65, 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
index 7c23767..ace8329 100644
--- a/yaml/camera_euroc_mav0_notangdist.yaml
+++ b/yaml/camera_euroc_mav0_notangdist.yaml
@@ -11,7 +11,7 @@ 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, 0.0, 0.000000]
rectification_matrix:
rows: 3
cols: 3
@@ -21,6 +21,6 @@ rectification_matrix:
projection_matrix:
rows: 3
cols: 4
- data: [458, 0.000000, 367.215, 0.000000,
- 0.000000, 457.296, 248.375, 0.000000,
+ data: [355.63, 0.000000, 362.27, 0.000000,
+ 0.000000, 417.16, 249.65, 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 391eb7a..185118f 100644
--- a/yaml/demo_visual_odometry_euroc.yaml
+++ b/yaml/demo_visual_odometry_euroc.yaml
@@ -35,6 +35,7 @@ config:
plugin: "vision"
extrinsic:
pose: [0,0,0, 0,0,0,1]
+ using_raw: false
follow: "camera_euroc_mav0_notangdist.yaml"
processors:
@@ -49,7 +50,8 @@ config:
-
package: "wolf_ros_vision"
type: "SubscriberCamera"
- topic: "/cam0/image_raw"
+ # topic: "/cam0/image_raw"
+ topic: "/cam0/image_rect"
sensor_name: "CAMERA"
ROS publisher:
--
GitLab