diff --git a/launch/demo_visual_odometry.launch b/launch/demo_visual_odometry.launch
index 2acef1e7c8c920f52c3cee6b6acec2b799ca6ece..3e02387e44e4512efcd2c845de6f90a29fbb238b 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 0000000000000000000000000000000000000000..b03ed8162546cdfed01d73b480c49d80b8ff0085
--- /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 0cad8af391abd4ea08b1f60b51c6a4bdd62bb4c5..9f3701e62c4a754fcd2ead734580b9c841c4280c 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 d7b4703eabc462bca36f7a0f6998ad7001e078b6..1fdeacdb4705d6c1c4e43736578a01c697c2d7bb 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 7c237676855cc910c65a44b724e398763c3ee635..ace83297fb7c2ebca132d0f116ee46ecb4f0b18e 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 391eb7a437cd03419ceabc79beaca0a30ff8de35..185118f47c174768f8b92e619b459cb81e8baf6f 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: