Skip to content
Snippets Groups Projects
Commit fd967285 authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

[skip-ci] adding a new d435i configuration

parent 6a653031
No related branches found
No related tags found
No related merge requests found
Pipeline #12107 skipped
Hide whitespace changes
Inline Side-by-side
Showing
with 850 additions and 24 deletions
launch/demo_visual_odometry.launch → launch/demo_vio.launch
+ 86
− 0
View file @ fd967285
......@@ -12,24 +12,67 @@
<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/online.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/publisher_camera_info.launch" />
<!-- <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="cam0" />
ns="$(arg namespace_cam)" />
<!--WOLF-->
<node type="wolf_ros_node"
......@@ -38,6 +81,6 @@
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" />
<param name="~yaml_file_path" value="$(find wolf_demo_visual_odometry)/yaml/$(arg param_file)" />
</node>
</launch>
launch/demo_visual_inertial_odometry.launch → launch/demo_vo.launch
+ 86
− 0
View file @ fd967285
......@@ -12,24 +12,67 @@
<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/online.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/publisher_camera_info.launch" />
<!-- <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="cam0" />
ns="$(arg namespace_cam)" />
<!--WOLF-->
<node type="wolf_ros_node"
......@@ -38,6 +81,6 @@
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" />
<param name="~yaml_file_path" value="$(find wolf_demo_visual_odometry)/yaml/$(arg param_file)" />
</node>
</launch>
launch/rs435i.launch 0 → 100644
+ 150
− 0
View file @ fd967285
<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>
rviz/online.rviz → rviz/d435i.rviz
+ 1
− 1
View file @ fd967285
......@@ -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
......
rviz/euroc.rviz 0 → 100644
+ 244
− 0
View file @ fd967285
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
yaml/camera_d435i.yaml 0 → 100644
+ 20
− 0
View file @ fd967285
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]
yaml/camera_euroc_mav0.yaml → yaml/camera_euroc.yaml
+ 1
− 1
View file @ fd967285
width: 752
height: 480
camera_name: narrow_stereo
camera_name: euroc_mav0
camera_matrix:
rows: 3
cols: 3
......
yaml/demo_vio_d435i.yaml 0 → 100644
+ 121
− 0
View file @ fd967285
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
yaml/demo_visual_inertial_odometry_euroc.yaml → yaml/demo_vio_euroc.yaml
+ 7
− 10
View file @ fd967285
......@@ -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:
-
......
yaml/demo_visual_odometry_euroc.yaml → yaml/demo_vo_d435i.yaml
+ 9
− 12
View file @ fd967285
......@@ -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:
......
yaml/demo_vo_euroc.yaml 0 → 100644
+ 118
− 0
View file @ fd967285
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
yaml/vi_sensor_imu.yaml → yaml/imu_d435i.yaml
+ 0
− 0
View file @ fd967285
File moved
yaml/imu_euroc.yaml 0 → 100644
+ 7
− 0
View file @ fd967285
# 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)
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment