diff --git a/launch/demo_vio.launch b/launch/demo_vio.launch
new file mode 100644
index 0000000000000000000000000000000000000000..52ca3d3f2c0ac8cb8470c96f93882bada7a30838
--- /dev/null
+++ b/launch/demo_vio.launch
@@ -0,0 +1,86 @@
+<!-- -->
+<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)" />
+
+ <!-- Convention used by all files -->
+ <!-- TODO should be able to only set the setup_name and then set the param and arg accordingly, did not manage though -->
+ <!-- EUROC -->
+ <!-- <arg name="setup_name" default="euroc" />
+ <arg name="namespace_cam" value="cam0" />
+ <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.yaml" /> -->
+ <!-- -->
+
+ <!-- D453I -->
+ <arg name="setup_name" default="d435i" />
+ <arg name="namespace_cam" value="camera/infra1" />
+ <param name="topic_image_raw_in" value="/camera/infra1/image_rect_raw" />
+ <param name="topic_image_info_out" value="/camera/infra1/camera_info" />
+ <param name="camera_yaml_path" value="$(find wolf_demo_visual_odometry)/yaml/camera_d435i.yaml" />
+ <!-- -->
+
+ <arg name="param_file" default="demo_vio_$(arg setup_name).yaml" />
+
+
+
+ <!--ROSBAG PLAY-->
+ <!--if the bag argument is not set, no bag is read here-->
+ <arg name="bag" default=""/>
+ <group if="$(eval arg('bag') != '')">
+ <param name="use_sim_time" value="$(arg sim_time)" />
+ <node pkg="rosbag"
+ type="play"
+ name="player"
+ required="true"
+ args="-r $(arg speed)
+ -s $(arg sec)
+ --clock
+ $(find wolf_demo_visual_odometry)/bag/$(arg bag).bag"/>
+ </group>
+
+ <!--VISUALIZATION-->
+ <group if="$(arg rviz)">
+ <node name="rviz"
+ pkg="rviz"
+ type="rviz"
+ args="-d $(find wolf_demo_visual_odometry)/rviz/$(arg setup_name).rviz" />
+ </group>
+
+
+ <!--Publish a CameraInfo topic reading at a standard wolf camera yaml file-->
+ <!-- <include file="$(find wolf_demo_visual_odometry)/launch/publish_camera_info.launch" /> -->
+
+ <node type="publisher_camera_info.py"
+ name="publisher_camera_info"
+ pkg="wolf_ros_vision"
+ >
+ </node>
+
+
+ <!--Undistort the image using the published camera info and raw images-->
+ <node type="image_proc"
+ name="image_proc"
+ pkg="image_proc"
+ ns="$(arg namespace_cam)" />
+
+ <!--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/$(arg param_file)" />
+ </node>
+</launch>
diff --git a/launch/demo_visual_inertial_odometry.launch b/launch/demo_visual_inertial_odometry.launch
deleted file mode 100644
index f77432bfdcaf9fcf2247cb2991dc8210bb9117e5..0000000000000000000000000000000000000000
--- a/launch/demo_visual_inertial_odometry.launch
+++ /dev/null
@@ -1,43 +0,0 @@
-<!-- -->
-<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>
diff --git a/launch/demo_visual_odometry.launch b/launch/demo_visual_odometry.launch
deleted file mode 100644
index 3e02387e44e4512efcd2c845de6f90a29fbb238b..0000000000000000000000000000000000000000
--- a/launch/demo_visual_odometry.launch
+++ /dev/null
@@ -1,43 +0,0 @@
-<!-- -->
-<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_odometry_euroc.yaml" />
- </node>
-</launch>
diff --git a/launch/demo_vo.launch b/launch/demo_vo.launch
new file mode 100644
index 0000000000000000000000000000000000000000..017f57de62321002418e45db6de29ba435f2ba08
--- /dev/null
+++ b/launch/demo_vo.launch
@@ -0,0 +1,86 @@
+<!-- -->
+<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)" />
+
+ <!-- Convention used by all files -->
+ <!-- TODO should be able to only set the setup_name and then set the param and arg accordingly, did not manage though -->
+ <!-- EUROC -->
+ <!-- <arg name="setup_name" default="euroc" />
+ <arg name="namespace_cam" value="cam0" />
+ <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.yaml" /> -->
+ <!-- -->
+
+ <!-- D453I -->
+ <arg name="setup_name" default="d435i" />
+ <arg name="namespace_cam" value="camera/infra1" />
+ <param name="topic_image_raw_in" value="/camera/infra1/image_rect_raw" />
+ <param name="topic_image_info_out" value="/camera/infra1/camera_info" />
+ <param name="camera_yaml_path" value="$(find wolf_demo_visual_odometry)/yaml/camera_d435i.yaml" />
+ <!-- -->
+
+ <arg name="param_file" default="demo_vo_$(arg setup_name).yaml" />
+
+
+
+ <!--ROSBAG PLAY-->
+ <!--if the bag argument is not set, no bag is read here-->
+ <arg name="bag" default=""/>
+ <group if="$(eval arg('bag') != '')">
+ <param name="use_sim_time" value="$(arg sim_time)" />
+ <node pkg="rosbag"
+ type="play"
+ name="player"
+ required="true"
+ args="-r $(arg speed)
+ -s $(arg sec)
+ --clock
+ $(find wolf_demo_visual_odometry)/bag/$(arg bag).bag"/>
+ </group>
+
+ <!--VISUALIZATION-->
+ <group if="$(arg rviz)">
+ <node name="rviz"
+ pkg="rviz"
+ type="rviz"
+ args="-d $(find wolf_demo_visual_odometry)/rviz/$(arg setup_name).rviz" />
+ </group>
+
+
+ <!--Publish a CameraInfo topic reading at a standard wolf camera yaml file-->
+ <!-- <include file="$(find wolf_demo_visual_odometry)/launch/publish_camera_info.launch" /> -->
+
+ <node type="publisher_camera_info.py"
+ name="publisher_camera_info"
+ pkg="wolf_ros_vision"
+ >
+ </node>
+
+
+ <!--Undistort the image using the published camera info and raw images-->
+ <node type="image_proc"
+ name="image_proc"
+ pkg="image_proc"
+ ns="$(arg namespace_cam)" />
+
+ <!--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/$(arg param_file)" />
+ </node>
+</launch>
diff --git a/launch/publisher_camera_info.launch b/launch/publish_camera_info.launch
similarity index 100%
rename from launch/publisher_camera_info.launch
rename to launch/publish_camera_info.launch
diff --git a/launch/rs435i.launch b/launch/rs435i.launch
new file mode 100644
index 0000000000000000000000000000000000000000..991f29c7d1e992e13e927b624d01b2338830a61d
--- /dev/null
+++ b/launch/rs435i.launch
@@ -0,0 +1,150 @@
+<launch>
+ <arg name="serial_no" default=""/>
+ <arg name="usb_port_id" default=""/>
+ <arg name="device_type" default=""/>
+ <arg name="json_file_path" default=""/>
+ <arg name="camera" default="camera"/>
+ <arg name="tf_prefix" default="$(arg camera)"/>
+ <arg name="external_manager" default="false"/>
+ <arg name="manager" default="realsense2_camera_manager"/>
+ <arg name="output" default="screen"/>
+ <arg name="respawn" default="false"/>
+
+ <arg name="fisheye_width" default="-1"/>
+ <arg name="fisheye_height" default="-1"/>
+ <arg name="enable_fisheye" default="false"/>
+
+ <arg name="depth_width" default="-1"/>
+ <arg name="depth_height" default="-1"/>
+ <arg name="enable_depth" default="false"/>
+
+ <arg name="confidence_width" default="-1"/>
+ <arg name="confidence_height" default="-1"/>
+ <arg name="enable_confidence" default="true"/>
+ <arg name="confidence_fps" default="-1"/>
+
+ <arg name="infra_width" default="848"/>
+ <arg name="infra_height" default="480"/>
+ <arg name="enable_infra" default="true"/>
+ <arg name="enable_infra1" default="true"/>
+ <arg name="enable_infra2" default="false"/>
+ <arg name="infra_rgb" default="false"/>
+
+ <arg name="color_width" default="-1"/>
+ <arg name="color_height" default="-1"/>
+ <arg name="enable_color" default="false"/>
+
+ <arg name="fisheye_fps" default="-1"/>
+ <arg name="depth_fps" default="-1"/>
+ <arg name="infra_fps" default="30"/>
+ <arg name="color_fps" default="-1"/>
+ <arg name="gyro_fps" default="-1"/>
+ <arg name="accel_fps" default="-1"/>
+ <arg name="enable_gyro" default="true"/>
+ <arg name="enable_accel" default="true"/>
+
+ <arg name="enable_pointcloud" default="false"/>
+ <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
+ <arg name="pointcloud_texture_index" default="0"/>
+ <arg name="allow_no_texture_points" default="false"/>
+ <arg name="ordered_pc" default="false"/>
+
+ <arg name="enable_sync" default="false"/>
+ <arg name="align_depth" default="false"/>
+
+ <arg name="publish_tf" default="true"/>
+ <arg name="tf_publish_rate" default="0"/>
+
+ <arg name="filters" default=""/>
+ <arg name="clip_distance" default="-2"/>
+ <arg name="linear_accel_cov" default="0.01"/>
+ <arg name="initial_reset" default="true"/>
+ <arg name="reconnect_timeout" default="6.0"/>
+ <arg name="wait_for_device_timeout" default="-1.0"/>
+ <arg name="unite_imu_method" default="linear_interpolation"/>
+ <arg name="topic_odom_in" default="odom_in"/>
+ <arg name="calib_odom_file" default=""/>
+ <arg name="publish_odom_tf" default="true"/>
+
+ <arg name="stereo_module/exposure/1" default="7500"/>
+ <arg name="stereo_module/gain/1" default="16"/>
+ <arg name="stereo_module/exposure/2" default="1"/>
+ <arg name="stereo_module/gain/2" default="16"/>
+
+
+
+ <group ns="$(arg camera)">
+ <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
+ <arg name="tf_prefix" value="$(arg tf_prefix)"/>
+ <arg name="external_manager" value="$(arg external_manager)"/>
+ <arg name="manager" value="$(arg manager)"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="respawn" value="$(arg respawn)"/>
+ <arg name="serial_no" value="$(arg serial_no)"/>
+ <arg name="usb_port_id" value="$(arg usb_port_id)"/>
+ <arg name="device_type" value="$(arg device_type)"/>
+ <arg name="json_file_path" value="$(arg json_file_path)"/>
+
+ <arg name="enable_pointcloud" value="$(arg enable_pointcloud)"/>
+ <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
+ <arg name="pointcloud_texture_index" value="$(arg pointcloud_texture_index)"/>
+ <arg name="enable_sync" value="$(arg enable_sync)"/>
+ <arg name="align_depth" value="$(arg align_depth)"/>
+
+ <arg name="fisheye_width" value="$(arg fisheye_width)"/>
+ <arg name="fisheye_height" value="$(arg fisheye_height)"/>
+ <arg name="enable_fisheye" value="$(arg enable_fisheye)"/>
+
+ <arg name="depth_width" value="$(arg depth_width)"/>
+ <arg name="depth_height" value="$(arg depth_height)"/>
+ <arg name="enable_depth" value="$(arg enable_depth)"/>
+
+ <arg name="confidence_width" value="$(arg confidence_width)"/>
+ <arg name="confidence_height" value="$(arg confidence_height)"/>
+ <arg name="enable_confidence" value="$(arg enable_confidence)"/>
+ <arg name="confidence_fps" value="$(arg confidence_fps)"/>
+
+ <arg name="color_width" value="$(arg color_width)"/>
+ <arg name="color_height" value="$(arg color_height)"/>
+ <arg name="enable_color" value="$(arg enable_color)"/>
+
+ <arg name="infra_width" value="$(arg infra_width)"/>
+ <arg name="infra_height" value="$(arg infra_height)"/>
+ <arg name="enable_infra" value="$(arg enable_infra)"/>
+ <arg name="enable_infra1" value="$(arg enable_infra1)"/>
+ <arg name="enable_infra2" value="$(arg enable_infra2)"/>
+ <arg name="infra_rgb" value="$(arg infra_rgb)"/>
+
+ <arg name="fisheye_fps" value="$(arg fisheye_fps)"/>
+ <arg name="depth_fps" value="$(arg depth_fps)"/>
+ <arg name="infra_fps" value="$(arg infra_fps)"/>
+ <arg name="color_fps" value="$(arg color_fps)"/>
+ <arg name="gyro_fps" value="$(arg gyro_fps)"/>
+ <arg name="accel_fps" value="$(arg accel_fps)"/>
+ <arg name="enable_gyro" value="$(arg enable_gyro)"/>
+ <arg name="enable_accel" value="$(arg enable_accel)"/>
+
+ <arg name="publish_tf" value="$(arg publish_tf)"/>
+ <arg name="tf_publish_rate" value="$(arg tf_publish_rate)"/>
+
+ <arg name="filters" value="$(arg filters)"/>
+ <arg name="clip_distance" value="$(arg clip_distance)"/>
+ <arg name="linear_accel_cov" value="$(arg linear_accel_cov)"/>
+ <arg name="initial_reset" value="$(arg initial_reset)"/>
+ <arg name="reconnect_timeout" value="$(arg reconnect_timeout)"/>
+ <arg name="wait_for_device_timeout" value="$(arg wait_for_device_timeout)"/>
+ <arg name="unite_imu_method" value="$(arg unite_imu_method)"/>
+ <arg name="topic_odom_in" value="$(arg topic_odom_in)"/>
+ <arg name="calib_odom_file" value="$(arg calib_odom_file)"/>
+ <arg name="publish_odom_tf" value="$(arg publish_odom_tf)"/>
+ <arg name="stereo_module/exposure/1" value="$(arg stereo_module/exposure/1)"/>
+ <arg name="stereo_module/gain/1" value="$(arg stereo_module/gain/1)"/>
+ <arg name="stereo_module/exposure/2" value="$(arg stereo_module/exposure/2)"/>
+ <arg name="stereo_module/gain/2" value="$(arg stereo_module/gain/2)"/>
+
+ <arg name="allow_no_texture_points" value="$(arg allow_no_texture_points)"/>
+ <arg name="ordered_pc" value="$(arg ordered_pc)"/>
+
+ </include>
+ </group>
+</launch>
diff --git a/rviz/online.rviz b/rviz/d435i.rviz
similarity index 99%
rename from rviz/online.rviz
rename to rviz/d435i.rviz
index 1512577da9f6bdd894857701211f96ae736589fc..0efb7f4c7eaff146be54c4214df8c3633e6d22da 100644
--- a/rviz/online.rviz
+++ b/rviz/d435i.rviz
@@ -136,7 +136,7 @@ Visualization Manager:
Value: true
- Class: rviz/Image
Enabled: false
- Image Topic: /cam0/image_raw
+ Image Topic: /camera/infra1/image_rect_raw
Max Value: 1
Median window: 5
Min Value: 0
diff --git a/rviz/euroc.rviz b/rviz/euroc.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..0efb7f4c7eaff146be54c4214df8c3633e6d22da
--- /dev/null
+++ b/rviz/euroc.rviz
@@ -0,0 +1,244 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 0
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /TF1/Frames1
+ - /Factors1
+ - /Factors1/Status1
+ - /Factors1/Namespaces1
+ - /Landmarks1/Namespaces1
+ - /Trajectory1/Namespaces1
+ Splitter Ratio: 0.5
+ Tree Height: 280
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: debug
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 1
+ Class: rviz/Axes
+ Enabled: true
+ Length: 0.10000000149011612
+ Name: Axes
+ Radius: 0.009999999776482582
+ Reference Frame: base_footprint
+ Show Trail: true
+ Value: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: false
+ base_footprint:
+ Value: true
+ map:
+ Value: true
+ odom:
+ Value: true
+ Marker Alpha: 1
+ Marker Scale: 0.5
+ Name: TF
+ Show Arrows: false
+ Show Axes: true
+ Show Names: false
+ Tree:
+ map:
+ odom:
+ base_footprint:
+ {}
+ Update Interval: 0
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /wolf_apriltag_vio/marker_array
+ Name: MarkerArray
+ Namespaces:
+ {}
+ Queue Size: 1
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /wolf_ros_node/graph_factors
+ Name: Factors
+ Namespaces:
+ factors_VISUALODOMETRY PROC: true
+ factors_text_VISUALODOMETRY PROC: false
+ factors_text_unnamed_processor: true
+ factors_unnamed_processor: true
+ Queue Size: 1
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /wolf_ros_node/graph_landmarks
+ Name: Landmarks
+ Namespaces:
+ landmarks: true
+ landmarks_text: true
+ Queue Size: 1
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /wolf_ros_node/graph_trajectory
+ Name: Trajectory
+ Namespaces:
+ frames: true
+ frames_text: true
+ Queue Size: 1
+ Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: visualization_marker
+ Name: Ground truth
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/Image
+ Enabled: false
+ Image Topic: /camera/infra1/image_rect_raw
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: image_raw
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: false
+ - Class: rviz/Image
+ Enabled: true
+ Image Topic: /debug_image
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: debug
+ Normalize Range: true
+ Queue Size: 2
+ 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
+ Default Light: true
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 3.074009656906128
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.7853981852531433
+ Focal Point:
+ X: 0.6342065930366516
+ Y: 0.34021610021591187
+ Z: 0.2876497805118561
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.05979748070240021
+ Target Frame: <Fixed Frame>
+ Yaw: 4.310366153717041
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1016
+ Hide Left Dock: false
+ Hide Right Dock: false
+ Image:
+ collapsed: false
+ QMainWindow State: 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
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1920
+ X: 0
+ Y: 27
+ debug:
+ collapsed: false
+ image_raw:
+ collapsed: false
diff --git a/yaml/camera_d435i.yaml b/yaml/camera_d435i.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d78584c5001a806079079d84d96610858b6276a0
--- /dev/null
+++ b/yaml/camera_d435i.yaml
@@ -0,0 +1,20 @@
+width: 848
+height: 480
+camera_name: d435i_infra_left
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [419.52520751953125, 0.0, 427.8790588378906, 0.0, 419.52520751953125, 241.32180786132812, 0.0, 0.0, 1.0]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.0, 0.0, 0.0, 0.0, 0.0] # tangential distortion not supported by wolf
+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: [419.52520751953125, 0.0, 427.8790588378906, 0.0, 0.0, 419.52520751953125, 241.32180786132812, 0.0, 0.0, 0.0, 1.0, 0.0]
diff --git a/yaml/camera_euroc_mav0.yaml b/yaml/camera_euroc.yaml
similarity index 95%
rename from yaml/camera_euroc_mav0.yaml
rename to yaml/camera_euroc.yaml
index c10126c67a17f6089dd3eaa92f1dc139079c06ac..3d6a265b90db4dd6c8fdcfc9366429f5370e5f5f 100644
--- a/yaml/camera_euroc_mav0.yaml
+++ b/yaml/camera_euroc.yaml
@@ -1,6 +1,6 @@
width: 752
height: 480
-camera_name: narrow_stereo
+camera_name: euroc_mav0
camera_matrix:
rows: 3
cols: 3
diff --git a/yaml/demo_vio_d435i.yaml b/yaml/demo_vio_d435i.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..eca75992b0aa73af2e6c8ebdb53863b7d14fd3b4
--- /dev/null
+++ b/yaml/demo_vio_d435i.yaml
@@ -0,0 +1,121 @@
+config:
+
+ debug:
+ profiling: true
+ profiling_file: "~/wolf_demo_vio_profiling.txt"
+ print_problem: true
+ print_depth: 3 # only if print_problem
+ print_constr_by: true # only if print_problem
+ print_metric: true # only if print_problem
+ print_state_blocks: false # only if print_problem
+ print_period: 5 # only if print_problem
+
+ problem:
+ node_rate: 20
+ follow: "tree_manager_sliding.yaml"
+ frame_structure: "POV"
+ dimension: 3
+ prior:
+ mode: "initial_guess"
+ $state:
+ P: [0,0,0]
+ O: [1,0,0,0] #Â arbitrary
+ V: [0,0,0]
+ $sigma:
+ P: [0.00001, 0.00001, 0.00001]
+ O: [3, 3, 3]
+ V: [0.1, 0.1, 0.1]
+ time_tolerance: 0.01
+
+ solver:
+ follow: "solver.yaml"
+
+ sensors:
+ -
+ type: "SensorCamera"
+ name: "CAMERA"
+ plugin: "vision"
+ extrinsic:
+ pose: [-0.006, 0.005, 0.012, 0,0,0,1] # D435i left infra
+ # pose: [0,0,0, 0,0,0,1]
+ using_raw: false
+ follow: "camera_d435i.yaml"
+ -
+ type: "SensorImu"
+ name: "IMU"
+ plugin: "imu"
+ extrinsic:
+ pose: [0,0,0, 0,0,0,1]
+ follow: "imu_d435i.yaml"
+
+ processors:
+ -
+ type: "ProcessorVisualOdometry"
+ name: "VISUALODOMETRY PROC"
+ sensor_name: "CAMERA"
+ plugin: "vision"
+ follow: "processor_visual_odometry.yaml"
+ -
+ type: "ProcessorImu"
+ name: "IMU PROC"
+ sensor_name: "IMU"
+ plugin: "imu"
+ follow: "processor_imu.yaml"
+ state_getter: true
+ state_priority: 1
+
+ ROS subscriber:
+ -
+ package: "wolf_ros_vision"
+ type: "SubscriberCamera"
+ topic: "/camera/infra1/image_rect_raw"
+ sensor_name: "CAMERA"
+ -
+ package: "wolf_ros_imu"
+ type: "SubscriberImu"
+ topic: "/camera/imu"
+ sensor_name: "IMU"
+ cov_source: "sensor"
+ in_degrees: false
+
+ ROS publisher:
+ -
+ type: "PublisherGraph"
+ topic: "graph"
+ package: "wolf_ros_node"
+ period: 0.2
+ viz_scale: 0.3
+ text_scale: 0.1
+ landmark_text_z_offset: 0.3
+ landmark_length: 0.5
+ frame_width: 0.01
+ frame_length: 0.1
+ frame_vel_scale: 1.0
+ factors_width: 0.01
+ factor_lmk_color: [0, 1, 1, 0.005]
+
+ -
+ type: "PublisherTf"
+ topic: " "
+ package: "wolf_ros_node"
+ period: 0.2
+ map_frame_id: "map"
+ odom_frame_id: "odom"
+ base_frame_id: "base_footprint"
+ publish_odom_tf: true
+
+ -
+ type: "PublisherVisionDebug"
+ processor_name: "VISUALODOMETRY PROC"
+ topic: "/debug_image"
+ topic_preprocessor: "/debug_image_preprocessor"
+ 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
diff --git a/yaml/demo_visual_inertial_odometry_euroc.yaml b/yaml/demo_vio_euroc.yaml
similarity index 90%
rename from yaml/demo_visual_inertial_odometry_euroc.yaml
rename to yaml/demo_vio_euroc.yaml
index 618832c51434eae53fafe9b3c1cec6ae5b366e48..db5f9e80618974c82d20414d08793a74f699af9f 100644
--- a/yaml/demo_visual_inertial_odometry_euroc.yaml
+++ b/yaml/demo_vio_euroc.yaml
@@ -16,19 +16,16 @@ config:
frame_structure: "POV"
dimension: 3
prior:
- mode: "factor"
+ mode: "initial_guess"
$state:
P: [0,0,0]
- # 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
+ O: [1,0,0,0] #Â arbitrary
V: [0,0,0]
$sigma:
P: [0.00001, 0.00001, 0.00001]
- O: [0.01, 0.01, 0.01]
- V: [1,1,1]
- time_tolerance: 0.0025
+ O: [3, 3, 3]
+ V: [0.1, 0.1, 0.1]
+ time_tolerance: 0.01
solver:
follow: "solver.yaml"
@@ -43,14 +40,14 @@ config:
pose: [0.0098,0.0647,-0.0216, 0.7071068, 0, 0.7071068, 0] # orientation wrt IMU in vi-sensor, https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets
# pose: [-0.0216,-0.0647,0.0098, -0.0077072, 0.0104967, 0.7017528, 0.7123015] # according to mav0/cam0/sensor.yaml in dataset
using_raw: false
- follow: "camera_euroc_mav0.yaml"
+ follow: "camera_euroc.yaml"
-
type: "SensorImu"
name: "IMU"
plugin: "imu"
extrinsic:
pose: [0,0,0, 0,0,0,1]
- follow: "vi_sensor_imu.yaml"
+ follow: "imu_euroc.yaml"
processors:
-
diff --git a/yaml/demo_visual_odometry_euroc.yaml b/yaml/demo_vo_d435i.yaml
similarity index 73%
rename from yaml/demo_visual_odometry_euroc.yaml
rename to yaml/demo_vo_d435i.yaml
index c5e6927b7baad7667ede59323255987f425cd591..0348d09a559aa067a8afb2042d5ca510cfd21487 100644
--- a/yaml/demo_visual_odometry_euroc.yaml
+++ b/yaml/demo_vo_d435i.yaml
@@ -2,7 +2,7 @@ config:
debug:
profiling: true
- profiling_file: "~/wolf_demo_visual_odometry_profiling.txt"
+ profiling_file: "~/wolf_demo_vio_profiling.txt"
print_problem: true
print_depth: 3 # only if print_problem
print_constr_by: true # only if print_problem
@@ -16,15 +16,13 @@ config:
frame_structure: "PO"
dimension: 3
prior:
- mode: "factor"
+ mode: "initial_guess"
$state:
P: [0,0,0]
- # P: [0,0,0.5]
- O: [0,0,0,1]
- # O: [0, 0.20791169, 0, 0.9781476] # x,y,z,w
+ O: [1,0,0,0] #Â arbitrary
$sigma:
- P: [0.0001, 0.0001, 0.0001]
- O: [0.01, 0.01, 0.01]
+ P: [0.00001, 0.00001, 0.00001]
+ O: [3, 3, 3]
time_tolerance: 0.01
solver:
@@ -36,11 +34,10 @@ config:
name: "CAMERA"
plugin: "vision"
extrinsic:
- # pose: [0,0,0, 0.5,-0.5,0.5,-0.5]
- pose: [0.0098,0.0647,-0.0216, 0.7071068, 0, 0.7071068, 0] # orientation wrt IMU in vi-sensor, https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets
- # pose: [-0.0216,-0.0647,0.0098, -0.0077072, 0.0104967, 0.7017528, 0.7123015] # according to mav0/cam0/sensor.yaml in dataset
+ pose: [-0.006, 0.005, 0.012, 0,0,0,1] # D435i left infra
+ # pose: [0,0,0, 0,0,0,1]
using_raw: false
- follow: "camera_euroc_mav0.yaml"
+ follow: "camera_d435i.yaml"
processors:
-
@@ -54,7 +51,7 @@ config:
-
package: "wolf_ros_vision"
type: "SubscriberCamera"
- topic: "/cam0/image_rect"
+ topic: "/camera/infra1/image_rect_raw"
sensor_name: "CAMERA"
ROS publisher:
diff --git a/yaml/demo_vo_euroc.yaml b/yaml/demo_vo_euroc.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..578a9661cccb1e4e6ef4eb9db23848fba05b650c
--- /dev/null
+++ b/yaml/demo_vo_euroc.yaml
@@ -0,0 +1,118 @@
+config:
+
+ debug:
+ profiling: true
+ profiling_file: "~/wolf_demo_visual_odometry_profiling.txt"
+ print_problem: false
+ print_depth: 4 # only if print_problem
+ print_constr_by: true # only if print_problem
+ print_metric: false # only if print_problem
+ print_state_blocks: false # only if print_problem
+ print_period: 1 # only if print_problem
+
+ problem:
+ node_rate: 20
+ follow: "tree_manager_sliding.yaml"
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ # 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
+ $sigma:
+ P: [0.00001, 0.00001, 0.00001]
+ O: [0.01, 0.01, 0.01]
+ time_tolerance: 0.0025
+
+ solver:
+ follow: "solver.yaml"
+
+ sensors:
+ -
+ type: "SensorCamera"
+ name: "CAMERA"
+ plugin: "vision"
+ extrinsic:
+# pose: [0,0,0, 0.5,-0.5,0.5,-0.5]
+ # pose: [0.0098,0.0647,-0.0216, 0.7071068, 0, 0.7071068, 0] # orientation wrt IMU in vi-sensor, https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets ???
+ pose: [-0.0216,-0.0647,0.0098, -0.0077072, 0.0104967, 0.7017528, 0.7123015] # according to mav0/cam0/sensor.yaml in dataset
+ using_raw: false
+ follow: "camera_euroc.yaml"
+ -
+ type: "SensorImu"
+ name: "IMU"
+ plugin: "imu"
+ extrinsic:
+ pose: [0,0,0, 0,0,0,1]
+ follow: "imu_euroc.yaml"
+
+ processors:
+ -
+ type: "ProcessorVisualOdometry"
+ name: "VISUALODOMETRY PROC"
+ sensor_name: "CAMERA"
+ plugin: "vision"
+ follow: "processor_visual_odometry.yaml"
+
+ ROS subscriber:
+ -
+ package: "wolf_ros_vision"
+ type: "SubscriberCamera"
+ topic: "/cam0/image_rect"
+ sensor_name: "CAMERA"
+ -
+ package: "wolf_ros_imu"
+ type: "SubscriberImu"
+ topic: "/imu0"
+ sensor_name: "IMU"
+ # imu_x_axis: 3 # See VI-sensor axes in https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets#platform_and_sensors ???
+ # imu_y_axis: -2
+ # imu_z_axis: 1
+ cov_source: "sensor"
+ in_degrees: false
+
+ ROS publisher:
+ -
+ type: "PublisherGraph"
+ topic: "graph"
+ package: "wolf_ros_node"
+ period: 0.2
+ viz_scale: 0.3
+ text_scale: 0.1
+ landmark_text_z_offset: 0.3
+ landmark_length: 0.5
+ frame_width: 0.01
+ frame_length: 0.1
+ frame_vel_scale: 1.0
+ factors_width: 0.01
+ factor_lmk_color: [0, 1, 1, 0.005]
+
+ -
+ type: "PublisherTf"
+ topic: " "
+ package: "wolf_ros_node"
+ period: 0.2
+ map_frame_id: "map"
+ odom_frame_id: "odom"
+ base_frame_id: "base_footprint"
+ publish_odom_tf: true
+
+ -
+ type: "PublisherVisionDebug"
+ processor_name: "VISUALODOMETRY PROC"
+ topic: "/debug_image"
+ topic_preprocessor: "/debug_image_preprocessor"
+ 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
diff --git a/yaml/vi_sensor_imu.yaml b/yaml/imu_d435i.yaml
similarity index 100%
rename from yaml/vi_sensor_imu.yaml
rename to yaml/imu_d435i.yaml
diff --git a/yaml/imu_euroc.yaml b/yaml/imu_euroc.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..45c454be0f33f75a4a40448087765c2a40817acb
--- /dev/null
+++ b/yaml/imu_euroc.yaml
@@ -0,0 +1,7 @@
+# IMU intrinsics:
+a_noise: 0.03 # standard deviation of Acceleration noise (same for all the axis) in m/s2
+w_noise: 0.05 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+ab_initial_stdev: 0.1 # m/s2 - initial bias
+wb_initial_stdev: 0.1 # rad/sec - initial bias
+ab_rate_stdev: 0.001 # m/s2/sqrt(s)
+wb_rate_stdev: 0.001 # rad/s/sqrt(s)