diff --git a/yaml/demo_visual_inertial_odometry_euroc.yaml b/yaml/demo_visual_inertial_odometry_euroc.yaml new file mode 100644 index 0000000000000000000000000000000000000000..da1089213853d1ae2b714cc19b3e437f11bf3bcc --- /dev/null +++ b/yaml/demo_visual_inertial_odometry_euroc.yaml @@ -0,0 +1,122 @@ +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: "POV" + 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 + 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 + + 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_mav0.yaml" + - + type: "SensorImu" + name: "IMU" + plugin: "imu" + extrinsic: + pose: [0,0,0, 0,0,0,1] + follow: "vi_sensor_imu.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" + apply_loss_function: false + follow: "processor_imu.yaml" + state_getter: true + state_priority: 1 + + 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: 1 + imu_y_axis: 2 + imu_z_axis: 3 + 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" +