-
Notifications
You must be signed in to change notification settings - Fork 12
/
Copy pathpgeneva_serial_eth.launch
144 lines (125 loc) · 8.5 KB
/
pgeneva_serial_eth.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
<launch>
<!-- mono or stereo and what ros bag to play -->
<arg name="max_cameras" default="2" />
<arg name="use_stereo" default="true" />
<arg name="bag_start" default="7" /> <!-- v1-2: 0, mh1: 40, mh2: 35, mh3: 17.5, mh4-5: 15 -->
<arg name="bag" default="/Desktop/IMUDB/EuRoC/bags/MH_04_difficult.bag.new.imudb_0.01.bag" />
<arg name="bag_gt" default="$(find ov_data)/euroc_mav/MH_04_difficult.csv" /> <!-- $(find ov_data)/euroc_mav/V1_01_easy.csv -->
<!-- imu starting thresholds -->
<arg name="init_window_time" default="0.75" />
<arg name="init_imu_thresh" default="1.5" />
<!-- saving trajectory path and timing information -->
<arg name="dosave" default="true" />
<arg name="dotime" default="false" />
<arg name="path_est" default="/tmp/traj_estimate.txt" />
<arg name="path_time" default="/tmp/traj_timing.txt" />
<!-- MASTER NODE! -->
<!-- <node name="run_serial_msckf" pkg="ov_msckf" type="run_serial_msckf" output="screen" clear_params="true" required="true" launch-prefix="gdb -ex run --args">-->
<!-- <node name="run_serial_msckf" pkg="ov_msckf" type="run_serial_msckf" output="screen" clear_params="true" required="true" launch-prefix="valgrind --tool=callgrind --callgrind-out-file=/tmp/callgrind.txt">-->
<!-- <node name="run_serial_msckf" pkg="ov_msckf" type="run_serial_msckf" output="screen" clear_params="true" required="true" launch-prefix="valgrind --tool=memcheck --leak-check=yes --show-leak-kinds=definite">-->
<node name="run_serial_msckf" pkg="ov_msckf" type="run_serial_msckf" output="screen" clear_params="true" required="true">
<!-- bag topics -->
<param name="topic_imu" type="string" value="/imu0" />
<param name="topic_camera0" type="string" value="/cam0/image_raw" />
<param name="topic_camera1" type="string" value="/cam1/image_raw" />
<rosparam param="stereo_pairs">[0,1]</rosparam>
<!-- bag parameters -->
<param name="path_bag" type="string" value="$(arg bag)" />
<param name="path_gt" type="string" value="$(arg bag_gt)" />
<param name="bag_start" type="double" value="$(arg bag_start)" />
<param name="bag_durr" type="int" value="-1" />
<!-- world/filter parameters -->
<param name="use_fej" type="bool" value="true" />
<param name="use_imuavg" type="bool" value="true" />
<param name="use_rk4int" type="bool" value="true" />
<param name="use_stereo" type="bool" value="$(arg use_stereo)" />
<param name="calib_cam_extrinsics" type="bool" value="true" />
<param name="calib_cam_intrinsics" type="bool" value="true" />
<param name="calib_cam_timeoffset" type="bool" value="true" />
<param name="calib_camimu_dt" type="double" value="0.0" />
<param name="max_clones" type="int" value="11" />
<param name="max_slam" type="int" value="75" />
<param name="max_slam_in_update" type="int" value="25" /> <!-- 25 seems to work well -->
<param name="max_msckf_in_update" type="int" value="40" />
<param name="max_cameras" type="int" value="$(arg max_cameras)" />
<param name="dt_slam_delay" type="double" value="3" />
<param name="init_window_time" type="double" value="$(arg init_window_time)" />
<param name="init_imu_thresh" type="double" value="$(arg init_imu_thresh)" />
<param name="gravity_mag" type="double" value="9.81" />
<param name="feat_rep_msckf" type="string" value="GLOBAL_3D" />
<param name="feat_rep_slam" type="string" value="ANCHORED_FULL_INVERSE_DEPTH" />
<param name="feat_rep_aruco" type="string" value="ANCHORED_FULL_INVERSE_DEPTH" />
<!-- zero velocity update parameters -->
<!-- inertial and disparity based detection (inertial is key for dynamic environments) -->
<param name="try_zupt" type="bool" value="true" />
<param name="zupt_chi2_multipler" type="double" value="0" /> <!-- set to 0 for only disp-based -->
<param name="zupt_max_velocity" type="double" value="0.1" />
<param name="zupt_noise_multiplier" type="double" value="50" />
<param name="zupt_max_disparity" type="double" value="0.5" /> <!-- set to 0 for only imu-based -->
<param name="zupt_only_at_beginning" type="bool" value="false" />
<!-- timing statistics recording -->
<param name="record_timing_information" type="bool" value="false" />
<param name="record_timing_filepath" type="string" value="/tmp/ov_timing.txt" />
<!-- tracker/extractor properties -->
<param name="use_klt" type="bool" value="true" />
<param name="num_pts" type="int" value="250" />
<param name="fast_threshold" type="int" value="15" />
<param name="grid_x" type="int" value="5" />
<param name="grid_y" type="int" value="3" />
<param name="min_px_dist" type="int" value="8" />
<param name="knn_ratio" type="double" value="0.70" />
<param name="downsample_cameras" type="bool" value="false" />
<param name="multi_threading" type="bool" value="true" />
<!-- aruco tag/mapping properties -->
<param name="use_aruco" type="bool" value="false" />
<param name="num_aruco" type="int" value="1024" />
<param name="downsize_aruco" type="bool" value="true" />
<!-- sensor noise values / update -->
<param name="up_msckf_sigma_px" type="double" value="1" />
<param name="up_msckf_chi2_multipler" type="double" value="1" />
<param name="up_slam_sigma_px" type="double" value="1" />
<param name="up_slam_chi2_multipler" type="double" value="1" />
<param name="up_aruco_sigma_px" type="double" value="1" />
<param name="up_aruco_chi2_multipler" type="double" value="1" />
<param name="gyroscope_noise_density" type="double" value="1.6968e-04" />
<param name="gyroscope_random_walk" type="double" value="1.9393e-05" />
<param name="accelerometer_noise_density" type="double" value="2.0000e-3" />
<param name="accelerometer_random_walk" type="double" value="3.0000e-3" />
<!-- camera intrinsics -->
<rosparam param="cam0_wh">[752, 480]</rosparam>
<rosparam param="cam1_wh">[752, 480]</rosparam>
<param name="cam0_is_fisheye" type="bool" value="false" />
<param name="cam1_is_fisheye" type="bool" value="false" />
<rosparam param="cam0_k">[458.654,457.296,367.215,248.375]</rosparam>
<rosparam param="cam0_d">[-0.28340811,0.07395907,0.00019359,1.76187114e-05]</rosparam>
<rosparam param="cam1_k">[457.587,456.134,379.999,255.238]</rosparam>
<rosparam param="cam1_d">[-0.28368365,0.07451284,-0.00010473,-3.55590700e-05]</rosparam>
<!-- camera extrinsics -->
<rosparam param="T_C0toI">
[
0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
0.0, 0.0, 0.0, 1.0
]
</rosparam>
<rosparam param="T_C1toI">
[
0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556,
0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024,
-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038,
0.0, 0.0, 0.0, 1.0
]
</rosparam>
</node>
<!-- play the dataset -->
<node pkg="rosbag" type="play" name="rosbag" args="-d 1 -s $(arg bag_start) $(arg bag)" required="true"/>
<!-- record the trajectory if enabled -->
<group if="$(arg dosave)">
<node name="recorder_estimate" pkg="ov_eval" type="pose_to_file" output="screen" required="true">
<param name="topic" type="str" value="/ov_msckf/poseimu" />
<param name="topic_type" type="str" value="PoseWithCovarianceStamped" />
<param name="output" type="str" value="$(arg path_est)" />
</node>
</group>
</launch>