Rosanswers logo

Hello everyone,

I am trying to fuse a visual odometry with Pixhawk IMU. For those who don't know, Pixhawk is an autopilot used for drone in this case. There is ROS integration provided thanks to mavros package, so I can get standard imu message from the Pixhawk. I am using only the IMU data from Pixhawk. I am using ROS Kinetic.

Intel already has a camera with built-in IMU, which is D435i. They provided example of launch file with exact same config as I want to use (realsense, rtabmap, robot_localization). Only difference is the IMU used. The launch file is here: opensource_tracking.launch

Since I am using different IMU, I changed the code a little bit: look here I launch node for gathering data from Pixhawk elsewhere, so it cannot be found in the launch file above.

So to the problem. I am able to run the rtabmap, create the 3D map of environment, localize myself with the visual odometry itself. I am able to get the imu data from pixhawk, when I echo the pixhawk imu data topic. Everything runs without errors. Although I am not sure, whether the config is correct. I attached the Pixhawk and RealSense camera to a plastic plate. I thought it was operating correctly, but when I detach Pixhawk and move it around, odometry does not change (I visualize odometry in RViz). The odom topic I try to visualize is /odometry/filtered, which is output of robot_localization package, so I guess this odometry should be output of merged imu and visual odometry. So to sum up: I am not sure if the IMU affects the odometry at all. I attach rqt_graph and tf view_frames for better understanding what is happening.

P.S. I may have forgotten to write everything about this issue, so please ask for additional info, if it is needed. I really appreciate any help! Thanks RQT_GRAPH


Originally posted by eMrazSVK on ROS Answers with karma: 41 on 2019-03-04

Post score: 1

Original comments

Comment by gvdhoorn on 2019-03-05:
@eMrazSVK: please attach your images directly to your question. I've given you sufficient karma to do that.

Comment by eMrazSVK on 2019-03-05:
Thanks, done.


1 Answer 1


Rosanswers logo

I am answering my own question.

So far, I guess it is working, let's say in principle. It does not work how I, or anybody would like it to work. It means that it lacks tuning, which I have to do next, but it is not within the context of this question. So I will post steps, how you can run mapping and localization using following tools:

  • Use Pixhawk's IMU (mavros package)
  • Intel RealSense D435 for Visual Odometry and RGBD data (realsense2_camera package)
  • robot_localization package for fusing IMU and Visual Odometry with UKF
  • rtabmap package for creating map and running Visual Odometry

Below you can find steps which are needed to run all this. I write it as a single commands, which you should run in separate terminals. I know, that launch files are made for this, but I've just had a one big mess from this, so I needed to break the steps down. Here you are:

  1. Launch node for gathering data from Pixhawk: roslaunch mavros px4.launch
  2. Launch realsense node, gather data from D435 camera: roslaunch realsense2_camera rs_camera.launch align_depth:=true
  3. Launch rtabmap's rgbd_odometry node: rosrun rtabmap_ros rgbd_odometry depth/image:=/camera/aligned_depth_to_color/image_raw rgb/image:=/camera/color/image_raw rgb/camera_info:=/camera/color/camera_info _frame_id:=camera_link this publishes /odom topic.
  4. Launch robot_localization node for fusing IMU and Visual Odometry data: roslaunch robot_localization ukf_template.launch - you need to edit ukf_tamplate.yaml located in your_catkin_ws/src/robot_localization/params. I hope you all understand the parameters there. If not, please refer to robot_localisation docs.
  5. Finally, launch rtabmap itself. Here, you can make various decisions, if you want to use rviz, or rtabmapviz etc. Please look at the rtabmap.launch file, if you need to configure final interface. roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info odom_topic:=/odometry/filtered visual_odometry:=false. odom_topic specifies, if you want to use visual odometry from robot_localisation, or just visual odometry, or whatever odometry you want to use. We set visual_odometry to false, because we've already launched it in step 3.

One final advice - look at all launch files, config files and set base_link to camera_link. So your base will be camera base. If you have any questions regarding to this solution, just ask me.

Originally posted by eMrazSVK with karma: 41 on 2019-03-08

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by SahanGura on 2022-02-23:
I am going through this method to generate a 3D map from rtabmap to get a better accuracy with IMU. I am using a raspberry pi4, relasense d435, and a 9DoF IMU. But the issue is when I am using the filtered odoemetry from the ukf, map is getting distorted with repeated information of the environment. When I run the rtabmap without the robot localization, map is generated without any distortion. rtabmap visual odometry node publishes odometry in a rate around 1Hz. IMU data is perfectly calibrated and fused using Madgwick filter for orientation estimation. So, according to my understanding this issue is because of the performance of the rpi. Can I solve this issue by tuning some parameters?


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.