i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

OdometryROS: refactored imu sync (#331). Using tf to compute baseline if Rtabmap/ImagesAlreadyRectified is true (for convenience with realsense rectified IR images but right camera info Tx not set)

Browse Source
This commit is contained in:
matlabbe
2020-07-30 12:43:44 -04:00
parent 0f7044f6ff
commit ae854dfb4c
10 changed files with 134 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/tests/test_d435i_vio.launch
+15 -9
View File
@@ -5,10 +5,18 @@
<arg name="rtabmapviz" default="true"/>
<arg name="rviz" default="false"/>
<arg name="depth_mode" default="true"/>
<arg name="unite_imu_method" default="copy"/> <!-- "copy" or "linear_interpolation" -->
<include file="$(find realsense2_camera)/launch/rs_camera.launch">
<arg name="align_depth" value="true"/>
<arg name="unite_imu_method" value="linear_interpolation"/>
<arg name="align_depth" value="$(arg depth_mode)"/>
<arg name="unite_imu_method" value="$(arg unite_imu_method)"/>
<arg name="enable_gyro" value="true"/>
<arg name="enable_accel" value="true"/>
<arg name="enable_infra1" value="true"/>
<arg name="enable_infra2" value="true"/>
<arg name="gyro_fps" value="200"/>
<arg name="accel_fps" value="250"/>
<arg name="enable_sync" value="true"/>
</include>
<node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_node">
@@ -19,17 +27,15 @@
<remap from="/imu/data" to="/rtabmap/imu"/>
</node>
<node pkg="topic_tools" type="throttle" name="imu_throttle" args="messages /rtabmap/imu 100.0"/>
<!-- RTAB-Map: depth mode -->
<!-- We have to launch stereo_odometry externally from rtabmap.launch so that rtabmap can use RGB-D input -->
<group ns="rtabmap">
<node if="$(arg depth_mode)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" args="--Optimizer/GravitySigma 0.3 --Odom/Strategy 9 --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml --Rtabmap/ImagesAlreadyRectified false" output="screen">
<node if="$(arg depth_mode)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" args="--Optimizer/GravitySigma 0.3 --Odom/Strategy 9 --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml" output="screen">
<remap from="left/image_rect" to="/camera/infra1/image_rect_raw"/>
<remap from="right/image_rect" to="/camera/infra2/image_rect_raw"/>
<remap from="left/camera_info" to="/camera/infra1/camera_info"/>
<remap from="right/camera_info" to="/camera/infra2/camera_info"/>
<remap from="imu" to="/rtabmap/imu_throttle"/>
<remap from="imu" to="/rtabmap/imu"/>
<param name="frame_id" value="camera_link"/>
<param name="wait_imu_to_init" value="true"/>
</node>
@@ -40,8 +46,9 @@
<arg name="depth_topic" value="/camera/aligned_depth_to_color/image_raw"/>
<arg name="camera_info_topic" value="/camera/color/camera_info"/>
<arg name="visual_odometry" value="false"/>
<arg name="approx_sync" value="false"/>
<arg name="frame_id" value="camera_link"/>
<arg name="imu_topic" value="/rtabmap/imu_throttle"/>
<arg name="imu_topic" value="/rtabmap/imu"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
<arg name="rviz" value="$(arg rviz)"/>
</include>
@@ -49,14 +56,13 @@
<!-- RTAB-Map: Stereo mode -->
<include unless="$(arg depth_mode)" file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="rtabmap_args" value="--delete_db_on_start --Optimizer/GravitySigma 0.3 --Odom/Strategy 9 --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml"/>
<arg name="odom_args" value="--Rtabmap/ImagesAlreadyRectified false"/>
<arg name="left_image_topic" value="/camera/infra1/image_rect_raw"/>
<arg name="right_image_topic" value="/camera/infra2/image_rect_raw"/>
<arg name="left_camera_info_topic" value="/camera/infra1/camera_info"/>
<arg name="right_camera_info_topic" value="/camera/infra2/camera_info"/>
<arg name="stereo" value="true"/>
<arg name="frame_id" value="camera_link"/>
<arg name="imu_topic" value="/rtabmap/imu_throttle"/>
<arg name="imu_topic" value="/rtabmap/imu"/>
<arg name="wait_imu_to_init" value="true"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
<arg name="rviz" value="$(arg rviz)"/>