Official RTAB-Map Forum

Add external IMU with D435i

Classic

List

Threaded

7 messages Options

BalajiRavi

Add external IMU with D435i

Hello! I had been doing SLAM with D435i so far and it's fine but not so accurate. Hence adding an external IMU for this case just by changing the existing IMU topic and now i get errors on the IMU data. Any Idea on how to approach with an external IMU?

matlabbe

Re: Add external IMU with D435i

Administrator

BalajiRavi wrote

i get errors on the IMU data

What are the errors? If the orientation is not set, you have to use an imu filter like complementary or Madgwick filters.

BalajiRavi

Re: Add external IMU with D435i

Hello Sir,
Thanks much for the reply. The external IMU topic i am using is - /olive/imu/x1691758366338/imu

And following is my launch file which worked well with D435i imu and i have replaced that topic with above mentioned topic in the filter part too, please let me know if what i did was right - https://drive.google.com/file/d/1U-7KRJ1nP_w0otQpryL6CmRas0Y1h9-_/view?usp=sharing

and the error which i got for the same is - https://drive.google.com/file/d/13s3-buL_4g_U48gInfKgdIENBXCAyXHg/view?usp=sharing

Please advice me on this.

P.S. My project is in ROS Noetic and the external IMU is compatible with only ROS2 hence i am getting this IMU data with ROS_BRIDGE and i beleive that wont be a problem

Regards,
Balaji Ravi

matlabbe

Re: Add external IMU with D435i

Administrator

What looks like the imu topic? ("rostopic echo /olive/imu/x1691758366338/imu ")

Putting the launch file here for quick reference:

<launch>
    <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="unite_imu_method" value="linear_interpolation"/>
    </include>
    
    <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="/olive/imu/x1691758366338/imu"/>
    </node>

    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
        <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="rtabmap_args" value="--delete_db_on_start --Vis/EstimationType 1 --Vis/MaxDepth 0 --GFTT/QualityLevel 0.00001 --Stereo/MinDisparity 0 --Stereo/MaxDisparity 64 --Vis/RoiRatios '0 0 0 .2' --Kp/RoiRatios '0 0 0 .2' --Odom/GuessMotion true --Vis/MinInliers 10 --Vis/BundleAdjustment 1 --OdomF2M/BundleAdjustment 1 --Vis/CorNNDR 0.6 --Vis/CorGuessWinSize 20 --Vis/PnPFlags 0"/>
        <arg name="localization" value="false"/>
        <arg name="rviz" value="true"/>
    </include>

    <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,
                                            true,true,true,
                                            true,true,true,
                                            true,true,true,
                                            true,true,true]
    </rosparam>
    <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="/olive/imu/x1691758366338/imu"/>
    <rosparam param="/ukf_se/imu0_config">[false, false, false,
                                           true,  true,  true,
                                           true,  true,  true,
                                           true,  true,  true,
                                           true,  true,  true]
    </rosparam>
    <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,}"/> -->
</launch>

For robot_localization config, rtabmap's odometry doesn't provide acceleration, I would set those components to false. For imu config, remove linear twist as it is not estimated. Note that if you are using already a filter to compute the quarternion you could fuse only its orientation values (removing angular velocities and acceleration).

To use the filtered imu topic in robot_localization, the output of the filter would be /imu/data.

For rtabmap.launch, you need to set "publish_tf_odom" to false if you are fusing the odometry topic in robot_localization. Make sure robot_localization fixed/map frame is "odom", not map to not conflict with rtabmap map->odom TF. You can provide imu to rtabmap's odometry to help it for motion prediction, set wait_imu_to_init to true.

BalajiRavi

Re: Add external IMU with D435i

Thanks much for your time and reply sir.
The imu topic after rostopic echo looks like following,

header:
seq: 77424
stamp:
secs: 1693748137
nsecs: 232894516
frame_id: "olive"
orientation:
x: -0.2145405346051309
y: -0.05454378395475155
z: -0.0009335842931194577
w: 0.9751904752732206
orientation_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
angular_velocity:
x: -0.0028605997935243553
y: -0.0016895555978834844
z: 0.0027800940664489235
angular_velocity_covariance: [1.2733008147084419e-05, 0.0, 0.0, 0.0, 1.2733008147084419e-05, 0.0, 0.0, 0.0, 1.2733008147084419e-05]
linear_acceleration:
x: 1.2451552612202097
y: -3.8861322950903388
z: -8.980847129012243
linear_acceleration_covariance: [0.0015663435754049997, 0.0, 0.0, 0.0, 0.0015663435754049997, 0.0, 0.0, 0.0, 0.0015663435754049997]
---

I will try what you advised previously today. Do you have any advice on how shall i proceed after looking at about imu topic?

Regards,
Balaji Ravi

BalajiRavi

Re: Add external IMU with D435i

This post was updated on .

Hello Sir,
Please refer previous message for echo of my external IMU topic
Now I have tried updating the launch file(below) as you said and errors cleared but new warnings as mentioned below,

Launch File :

<launch>
    <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="unite_imu_method" value="linear_interpolation"/>
    </include>
    
    <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="true" />
        <param name="_world_frame" type="string" value="enu" />
        <param name="fixed_frame" type="string" value="camera_link" />
        <remap from="/imu/data_raw" to="/olive/imu/x1691758366338/imu"/>
    </node>

    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
        <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="rtabmap_args" value="--delete_db_on_start --Vis/EstimationType 1 --Vis/MaxDepth 0 --GFTT/QualityLevel 0.00001 --Stereo/MinDisparity 0 --Stereo/MaxDisparity 64 --Vis/RoiRatios '0 0 0 .2' --Kp/RoiRatios '0 0 0 .2' --Odom/GuessMotion true --Vis/MinInliers 10 --Vis/BundleAdjustment 1 --OdomF2M/BundleAdjustment 1 --Vis/CorNNDR 0.6 --Vis/CorGuessWinSize 20 --Vis/PnPFlags 0"/>
        <arg name="localization" value="false"/>
        <arg name="approx_sync" value="true"/>
        <arg name="imu_topic" value="/imu/data"/>
        <arg name="rviz" value="true"/>
        <arg name="publish_tf_odom" value="false"/> <!-- Updated based on suggestion -->
        <arg name="wait_imu_to_init" value="true"/> <!-- Enable if using IMU data for motion prediction -->
    </include>

    <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,
                                            false,false,false,
                                            false,false,false,
                                            false,false,false,
                                            false,false,false]
    </rosparam>
    <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">[true, true, true,
                                           false,  false,  false,
                                           false,  false,  false,
                                           false,  false,  false,
                                           false,  false,  false]
    </rosparam>
    <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,}"/> -->
</launch>

Error Log:

[ WARN] [1702485111.240583820]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
   /rtabmap/odom \
   /camera/color/image_raw \
   /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info \
   /rtabmap/odom_info
[ WARN] [1702485114.995135151]: Still waiting for data on topic /imu/data_raw...
[ WARN] [1702485115.806988821]: /rtabmap/rgbd_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. 
/rtabmap/rgbd_odometry subscribed to (approx sync):
   /camera/color/image_raw \
   /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info
[ WARN] [1702485116.240834977]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
   /rtabmap/odom \
   /camera/color/image_raw \
   /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info \
   /rtabmap/odom_info
[ WARN] [1702485120.807146438]: /rtabmap/rgbd_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. 
/rtabmap/rgbd_odometry subscribed to (approx sync):
   /camera/color/image_raw \
   /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info
[ WARN] [1702485121.241035014]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
   /rtabmap/odom \
   /camera/color/image_raw \
   /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info \
   /rtabmap/odom_info

matlabbe

Re: Add external IMU with D435i

Administrator

Your imu topic seems to have already the orientaiton computed, so no need to use imu filter.

For robot_localization, this looks wrong:

<rosparam param="/ukf_se/imu0_config">[true, true, true,
                                           false,  false,  false,
                                           false,  false,  false,
                                           false,  false,  false,
                                           false,  false,  false]

it would be:

<rosparam param="/ukf_se/imu0_config">[false, false, false,
                                           true,  true,  true,
                                           false,  false,  false,
                                           false,  false,  false,
                                           false,  false,  false]

For the warning, do:

rostopic hz /camera/color/image_raw \
   /camera/aligned_depth_to_color/image_raw \
   /camera/color/camera_info

to see if the topics are published.