Demo visual odometry

launch/demo_visual_inertial_odometry.launch

+<!-- -->
+<launch>
+    <!--USER ARGS-->
+    <arg name="rviz" default="true" />
+    <arg name="speed" default="1" />
+    <arg name="sec" default="0" />
+    <arg name="sim_time" default="true" />
+
+    <arg name="profiling" default="false" />
+    <arg name="gdb" default="false" />
+    <arg name="launch_pref" default="" unless="$(eval profiling or gdb)"/>
+    <arg name="launch_pref" value="valgrind --tool=callgrind --callgrind-out-file='callgrind.wolf.%p'" if="$(arg profiling)" />
+    <arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" />
+
+    <!--VISUALIZATION-->
+    <group if="$(arg rviz)">
+        <node name="rviz"
+             pkg="rviz"
+             type="rviz"
+             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"
+          pkg="wolf_ros_node"
+          output="screen"
+          required="true"
+          launch-prefix="$(arg launch_pref)">
+        <param name="~yaml_file_path" value="$(find wolf_demo_visual_odometry)/yaml/demo_visual_inertial_odometry_euroc.yaml" />
+    </node>
+</launch>

yaml/demo_visual_inertial_odometry_euroc.yaml

@@ -22,6 +22,7 @@ config:
         # P: [0,0,0.5]
         O: [0,0,0,1]
         # O: [0, 0.20791169, 0, 0.9781476]  # x,y,z,w
+#        O: [0, -0.78770522698, 0, -0.61605233169] # -105 deg along Y axes
         V: [0,0,0]
       $sigma:
        P: [0.00001, 0.00001, 0.00001]
@@ -63,7 +64,6 @@ config:
       name: "IMU PROC"
       sensor_name: "IMU"
       plugin: "imu"
-      apply_loss_function: false
       follow: "processor_imu.yaml"
       state_getter: true
       state_priority: 1
@@ -119,4 +119,10 @@ config:
       package: "wolf_ros_vision"
       period: 0.05
       follow: "publisher_vision_debug.yaml"
-
+    - 
+      type: "PublisherImuBias"
+      sensor_name: "IMU"
+      processor_name: "IMU"
+      topic: "/imu_bias"
+      package: "wolf_ros_imu"
+      period: 0.05

yaml/processor_imu.yaml

 time_tolerance: 0.0025         # Time tolerance for joining KFs (half of 5ms or 200Hz)
 unmeasured_perturbation_std: 0.000001
+apply_loss_function: false
 keyframe_vote:
     voting_active:      false
     max_time_span:      1.0   # seconds