rtabmap_ros
rtabmap_ros copied to clipboard
introlab
odometry: waiting imu (/rtabmap/imu) to initialize orientation (wait_imu_to_init=true)
environment: Ubuntu 20.04, ROS noetic / realsense435i / vins-fusion / rtabmap
HELLO！ I want to use VINS-Fusion to provide external /odom input for RTABMAP for mapping.
My command to
start the camera is: roslaunch realsense2_camera rs_camera_vins.launch
`
<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="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_infra1" value="$(arg enable_infra1)"/>
<arg name="enable_infra2" value="$(arg enable_infra2)"/>
<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="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="allow_no_texture_points" value="$(arg allow_no_texture_points)"/>
</include>
start vins-fusion: rosrun vins vins_node ~/vins_fusion_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_config.yaml
`
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 0
num_of_cam: 2
imu_topic: "/camera/imu"
image0_topic: "/camera/infra1/image_rect_raw"
image1_topic: "/camera/infra2/image_rect_raw"
output_path: "/home/dji/output/"
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 480
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -5.7586305857286746e-03, -4.0463318787729019e-03,
9.9997523237933461e-01, 2.0329267950355900e-02,
-9.9998287214160420e-01, -1.0224590553211677e-03,
-5.7628118925283633e-03, 7.9325209639615653e-03,
1.0457519809151661e-03, -9.9999129084997906e-01,
-4.0403746097850135e-03, 2.8559824645148020e-03, 0., 0., 0., 1. ]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -1.0021770212322867e-03, 3.6313480322730518e-04,
9.9999943188700535e-01, 1.5285779565991807e-02,
-9.9999216342926500e-01, -3.8303422615924010e-03,
-1.0007788055728661e-03, -5.2435791444330505e-02,
3.8299766679101843e-03, -9.9999259827824449e-01,
3.6697063849344680e-04, 8.6931302450199057e-03, 0., 0., 0., 1. ]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 0 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.002
gyr_w: 0.0001 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.805 # gravity magnitude
#unsynchronization parameters
estimate_td: 1 # online estimate time offset between camera and imu
td: 0.00 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/dji/output/pose_graph/" # save and load path
save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0 `
to start RTABMAP: roslaunch rtabmap_ros mytest_d435i_vio.launch rtabmap_args:="--delete_db_on_start"
`<launch>
<!-- Example usage of RTAB-Map with VINS-Fusion support for realsense D435i.
Make sure to disable the IR emitter or put a tape on the IR emitter to
avoid VINS tracking the fixed IR points (that would cause large drifts) -->
<arg name="rtabmapviz" default="false"/>
<arg name="rviz" default="true"/>
<arg name="depth_mode" default="false"/>
<arg name="odom_strategy" default="9"/> <!-- default VINS -->
<arg name="unite_imu_method" default="copy"/> <!-- "copy" or "linear_interpolation" -->
<!-- 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 $(arg odom_strategy) --OdomVINS/ConfigPath /home/agx/VINS-Fusion/src/config/realsense_d435i/realsense_stereo_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"/>
<arg name="imu_topic" value="/rtabmap/imu"/>
<!--arg name="imu_topic" value="/camera/imu"/-->
<param name="frame_id" value="camera_link"/>
<param name="wait_imu_to_init" value="false"/>
</node>
</group>
<include if="$(arg depth_mode)" file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="rtabmap_args" value="--delete_db_on_start --Optimizer/GravitySigma 0.3"/>
<arg name="rgb_topic" value="/camera/color/image_raw"/>
<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"/>
<!--arg name="imu_topic" value="/camera/imu"/-->
<arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
<arg name="rviz" value="$(arg rviz)"/>
</include>
<!-- 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 $(arg odom_strategy) --OdomVINS/ConfigPath /home/meng/vins_fusion_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_config.yaml" />
<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"/>
<!--arg name="imu_topic" value="/camera/imu"/-->
<arg name="wait_imu_to_init" value="true"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
<arg name="rviz" value="$(arg rviz)"/>
</include>
</launch>
`
After start RTABMAP My error is [ WARN] [1653304384.207397999]: odometry: waiting imu (/rtabmap/imu) to initialize orientation (wait_imu_to_init=true)
Can you help me solve this problem？
If you are using external VINS-Fusion odometry, you don't need to start rtabmap_ros/stereo_odometry node, just remap vins-fusion output odom topic to rtabmap node. That example is using VINS-fusion internally.
environment: Ubuntu 20.04, ROS noetic / realsense435i / vins-fusion / rtabmap
HELLO！ I want to use VINS-Fusion to provide external /odom input for RTABMAP for mapping.
My command to start the camera is: roslaunch realsense2_camera rs_camera_vins.launch `
/camera/stereo_module/emitter_enabled: 0 /camera/stereo_module/emitter_enabled: 1
<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="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_infra1" value="$(arg enable_infra1)"/> <arg name="enable_infra2" value="$(arg enable_infra2)"/> <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="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="allow_no_texture_points" value="$(arg allow_no_texture_points)"/> </include>` start vins-fusion: rosrun vins vins_node ~/vins_fusion_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_config.yaml
` %YAML:1.0 #common parameters #support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; imu: 0 num_of_cam: 2 imu_topic: "/camera/imu" image0_topic: "/camera/infra1/image_rect_raw" image1_topic: "/camera/infra2/image_rect_raw" output_path: "/home/dji/output/" cam0_calib: "left.yaml" cam1_calib: "right.yaml" image_width: 640 image_height: 480 # Extrinsic parameter between IMU and Camera. estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. body_T_cam0: !!opencv-matrix rows: 4 cols: 4 dt: d data: [ -5.7586305857286746e-03, -4.0463318787729019e-03, 9.9997523237933461e-01, 2.0329267950355900e-02, -9.9998287214160420e-01, -1.0224590553211677e-03, -5.7628118925283633e-03, 7.9325209639615653e-03, 1.0457519809151661e-03, -9.9999129084997906e-01, -4.0403746097850135e-03, 2.8559824645148020e-03, 0., 0., 0., 1. ] body_T_cam1: !!opencv-matrix rows: 4 cols: 4 dt: d data: [ -1.0021770212322867e-03, 3.6313480322730518e-04, 9.9999943188700535e-01, 1.5285779565991807e-02, -9.9999216342926500e-01, -3.8303422615924010e-03, -1.0007788055728661e-03, -5.2435791444330505e-02, 3.8299766679101843e-03, -9.9999259827824449e-01, 3.6697063849344680e-04, 8.6931302450199057e-03, 0., 0., 0., 1. ] #Multiple thread support multiple_thread: 1 #feature traker paprameters max_cnt: 150 # max feature number in feature tracking min_dist: 30 # min distance between two features freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image F_threshold: 1.0 # ransac threshold (pixel) show_track: 0 # publish tracking image as topic flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy #optimization parameters max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time max_num_iterations: 8 # max solver itrations, to guarantee real time keyframe_parallax: 10.0 # keyframe selection threshold (pixel) #imu parameters The more accurate parameters you provide, the better performance acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04 gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004 acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.002 gyr_w: 0.0001 # gyroscope bias random work noise standard deviation. #4.0e-5 g_norm: 9.805 # gravity magnitude #unsynchronization parameters estimate_td: 1 # online estimate time offset between camera and imu td: 0.00 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) #loop closure parameters load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' pose_graph_save_path: "/home/dji/output/pose_graph/" # save and load path save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0 `to start RTABMAP: roslaunch rtabmap_ros mytest_d435i_vio.launch rtabmap_args:="--delete_db_on_start"
`<launch> <!-- Example usage of RTAB-Map with VINS-Fusion support for realsense D435i. Make sure to disable the IR emitter or put a tape on the IR emitter to avoid VINS tracking the fixed IR points (that would cause large drifts) --> <arg name="rtabmapviz" default="false"/> <arg name="rviz" default="true"/> <arg name="depth_mode" default="false"/> <arg name="odom_strategy" default="9"/> <!-- default VINS --> <arg name="unite_imu_method" default="copy"/> <!-- "copy" or "linear_interpolation" --> <!-- 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 $(arg odom_strategy) --OdomVINS/ConfigPath /home/agx/VINS-Fusion/src/config/realsense_d435i/realsense_stereo_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"/> <arg name="imu_topic" value="/rtabmap/imu"/> <!--arg name="imu_topic" value="/camera/imu"/--> <param name="frame_id" value="camera_link"/> <param name="wait_imu_to_init" value="false"/> </node> </group> <include if="$(arg depth_mode)" file="$(find rtabmap_ros)/launch/rtabmap.launch"> <arg name="rtabmap_args" value="--delete_db_on_start --Optimizer/GravitySigma 0.3"/> <arg name="rgb_topic" value="/camera/color/image_raw"/> <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"/> <!--arg name="imu_topic" value="/camera/imu"/--> <arg name="rtabmapviz" value="$(arg rtabmapviz)"/> <arg name="rviz" value="$(arg rviz)"/> </include> <!-- 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 $(arg odom_strategy) --OdomVINS/ConfigPath /home/meng/vins_fusion_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_config.yaml" /> <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"/> <!--arg name="imu_topic" value="/camera/imu"/--> <arg name="wait_imu_to_init" value="true"/> <arg name="rtabmapviz" value="$(arg rtabmapviz)"/> <arg name="rviz" value="$(arg rviz)"/> </include> </launch>`
After start RTABMAP My error is [ WARN] [1653304384.207397999]: odometry: waiting imu (/rtabmap/imu) to initialize orientation (wait_imu_to_init=true)
Can you help me solve this problem？
do you slove this problem?
