ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Intel realsense d435i - ros - rtabmap, slam gets lost if camera is moved slightly faster

asked 2022-04-05 05:10:49 -0500

goldgunner gravatar image

updated 2022-04-05 05:11:26 -0500


I'm having troubles with slam on intel d435i using ros and rtabmap "opensource tracking launch file", i'm able to get a 3d map when running the opensource tracking launch file from this tutorial : the problem is if i move or rotate the camera very slowly, the map is created with no the real dimensions, however, if i move or rotate it slightly faster, the TF of the camera starts moving and rotating randomly everywhere, and the mapcloud stops recording the map, i also get quality: 0 on the terminal. I suppose that the camera loses the point clouds. I'm trying to implement slam on a drone using this camera, and the drone moves somehow fast, how can i improve or tune the launch file to prevent the loss of the point clouds by the camera when moving faster? Thank you in advance

edit retag flag offensive close merge delete


Hi, The same happens to me. I am guessing it is a divergence problem of the ufk filter. When visual odometry is lost, the input to UFK is only the IMU of the D435i. My guess is that this Kalman filter does not converge. It seems it is somehow a "normal" behavior. I am doing more research in case I am able to solve it, or at least improve the performance

Bernat Gaston gravatar image Bernat Gaston  ( 2022-05-04 06:42:44 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2022-05-05 03:07:18 -0500

Bernat Gaston gravatar image

updated 2022-05-05 03:08:54 -0500

Hi, after some tests I have found what it seems a pretty stable version of the system. The key is to use the two IR of the d435i separately and hence, use rtabmap with IR configuration instead of depth one. Overall the opensource_tracking.launch file looks like this:

<arg name="offline"          default="false"/>
<include unless="$(arg offline)" 
    file="$(find realsense2_camera)/launch/rs_camera.launch">
    <arg name="align_depth" value="true"/>
    <arg name="linear_accel_cov" value="1.0"/>
    <arg name="enable_gyro" value="true"/>
    <arg name="enable_accel" value="true"/>
    <arg name="unite_imu_method" value="linear_interpolation"/>
    <arg name="enable_infra1" value="true"/>
    <arg name="enable_infra2" value="true"/>

<node pkg="imu_filter_madgwick" type="imu_filter_node" name="ImuFilter">
    <param name="use_mag" type="bool" value="false" />
    <param name="_publish_tf" type="bool" value="false" />
    <param name="_world_frame" type="string" value="enu" />
    <remap from="/imu/data_raw" to="/camera/imu"/>

<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
    <arg name="args" value="--delete_db_on_start"/>
    <!--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="depth_camera_info_topic" value="/camera/depth/camera_info"/-->
    <arg name="rtabmapviz" value="false"/>
    <arg name="rviz" value="true"/>
    <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"/>

<include file="$(find robot_localization)/launch/ukf_template.launch"/>
<param name="/ukf_se/frequency" value="300"/>
<param name="/ukf_se/base_link_frame" value="camera_link"/>
<param name="/ukf_se/odom0" value="rtabmap/odom"/>
<rosparam param="/ukf_se/odom0_config">[true,true,true,
<param name="/ukf_se/odom0_relative" value="true"/>
<param name="/ukf_se/odom0_pose_rejection_threshold" value="10000000"/>
<param name="/ukf_se/odom0_twist_rejection_threshold" value="10000000"/>

<param name="/ukf_se/imu0" value="/imu/data"/>
<rosparam param="/ukf_se/imu0_config">[false, false, false,
                                       true,  true,  true,
                                       true,  true,  true,
                                       true,  true,  true,
                                       true,  true,  true]
<param name="/ukf_se/imu0_differential" value="true"/>
<param name="/ukf_se/imu0_relative" value="false"/>
<param name="/ukf_se/use_control" value="false"/>
<!-- <param name="/ukf_se/odom0_config" value="{true,true,true,}"/> -->
edit flag offensive delete link more


Hi Mermy, Thank you for sharing your solution, i'll try it out today and then i'll let you know if it works on my setup.

goldgunner gravatar image goldgunner  ( 2022-05-10 08:24:52 -0500 )edit

I just tested your launch file and it works much more better, there's some motion drifts when the camera is moved quickly, but it retreives it's original position quick enough, i'll try to test this system on a drone and evaluate the precision of the slam, thank you sir.

goldgunner gravatar image goldgunner  ( 2022-05-11 10:25:06 -0500 )edit

Fantastic, glad to help. If you think that the answer was good enough, please mark the green tick so that it get ranked :)

Bernat Gaston gravatar image Bernat Gaston  ( 2022-05-11 10:32:51 -0500 )edit

Question Tools



Asked: 2022-04-05 05:10:49 -0500

Seen: 1,080 times

Last updated: May 05 '22