For more information see https://vision.in.tum.de/dm-vio and https://github.com/lukasvst/dm-vio.
This is a ROS-Wrapper for DM-VIO, inspired by the ROS-Wrapper for DSO.
It interfaces with ROS topics to input images and IMU data and to output poses. Alternatively, it can read images and IMU data directly from a rosbag dataset.
First install DM-VIO as described here and make sure it works.
This version of the ROS wrapper was tested with commit d18fa15ba086043561361941b8e5298074b34b47 of DM-VIO.
You need to set the environment variable DMVIO_BUILD to point to the folder where you built DM-VIO (which is expected to be
a direct subfolder of DM-VIO).
The easiest way is to put the following into your .bashrc
export DMVIO_BUILD=/PATH/TO/dm-vio/cmake-build-relwithdebinfoThen you can clone this project into your workspace and build normally with catkin_make.
You can run with
rosrun dmvio_ros node calib=/PATH/TO/camera.txt \
imuCalib=/PATH_TO/camchain.yaml \
settingsFile=/PATH_TO/dm-vio-public/configs/t265_noise_tumvi.yaml \
mode=1 nogui=0 preset=1 quiet=1
# vignette=PATH_TO/vignette.png # highly recommended, especially for fisheye cameras (use mode=3 if vignette is set).Most commandline arguments are the same as for DM-VIO, you will at least need to pass camera calibration, IMU camchain, preset and mode.
The node will listen for images at cam0/image_raw and for IMU messages at /imu0.
Settings file: I recommend starting with t265_noise_tumvi, and in general with TUM-VI noise values. For more details and tips see here.
Notes on exposures and photometric calibration: The standard ROS image messages do not include exposure times. If you have a custom message with exposures you can modify the method vidCb and pass the exposure to undistorter->undistort.
If you do not have exposure times you cannot use mode=0!
Note that mode=1 completely disables photometric calibration, including the vignette, even if it is passed.
Hence, I have added mode=3, which uses photometric calibration, but does not assume that there are exposures.
To test your installation you can download this EuRoC bag: http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room2/V2_01_easy/V2_01_easy.bag
You need to create the camera.txt file using
echo -e "458.654 457.296 367.215 248.375 -0.28340811 0.07395907 0.00019359 1.76187114e-05\n752 480\ncrop\n640 480\n" > camera.txtThen you can run the following commands in separate terminals
rosrun dmvio_ros node calib=/PATH/TO/camera.txt settingsFile=/PATH/TO/dm-vio/configs/euroc.yaml mode=1 nogui=0 preset=1 useimu=1 quiet=1 init_requestFullResetNormalizedErrorThreshold=0.8 init_pgba_skipFirstKFs=1
rosbag play V2_01_easy.bagFor EuRoC, simply run the following command in a terminal. (Make sure that roscore is running in a separate terminal.)
rosrun dmvio_ros node calib=/PATH/TO/camera.txt settingsFile=/PATH/TO/dm-vio/configs/euroc.yaml mode=1 nogui=0 preset=1 useimu=1 quiet=1 init_requestFullResetNormalizedErrorThreshold=0.8 init_pgba_skipFirstKFs=1 rosbag=/PATH/TO/V2_01_easy.bag loadRosbagThread=1This command will run as fast as possible and exit once execution is complete, which makes it useful to run on rosbag datasets. If a dataset is available as a rosbag, this is more convenient than extracting the rosbag and running the non-ros version of DM-VIO. However, keep in mind that ROS does not support exposure times out of the box, which can result in slightly worse accuracy compared to a dataset with full photometric calibration (including exposure times).
You can run on the Realsense T265 with their provided ROS driver. Install it with
sudo apt-get install ros-$ROS_DISTRO-realsense2-cameraThen you can run the following commands in separate terminals
rosrun dmvio_ros node nogui=0 useimu=1 quiet=1 mode=3 `#we use mode 3 because we have vignette but no expusures` \
calib=/PATH_TO/RealsenseCalibration/camera.txt \
imuCalib=/PATH_TO/RealsenseCalibration/factory_camchain.yaml \
gamma=PATH_TO/dm-vio/configs/pcalib_linear_8bit.txt \
vignette=PATH_TO/dm-vio/configs/realsense/vignette_t265.png \
settingsFile=/PATH_TO/dm-vio/configs/t265_noise_tumvi.yaml \
resultsPrefix=/PATH_TO_RESULTS/ \
cam0/image_raw:=/camera/fisheye1/image_raw imu0:=/camera/imu # Remap topics
roslaunch realsense2_camera rs_t265.launch unite_imu_method:=linear_interpolation enable_fisheye1:=true enable_fisheye2:=true enable_pose:=falseThe camera.txt and factory_camchain.txt can be obtained by calibrating yourself or by first running the normal Realsense demo which will save the factory calibration.
Note that the ROS driver does not read out exposures, hence we need to use mode=3 instead of mode=1 and the accuracy can be a bit worse than with the normal Realsense demo.
When using this ROS wrapper you might want to change src/ROSOutputWrapper.cpp to publish information in a format suited to your application.
We publish the following topics by default:
dmvio/frame_tracked: DMVIOPoseMsgPublished for every tracked frame (except the inital two frames). It includes
geometry_msgs/Pose pose: Transformation from camera to world in DSO frame (meaning it can have an arbitrary scale).and all fields in transformDSOToIMU, which enables you to convert the pose (and all published poses prior to this) to the metric frame:
float64 scalegeometry_msgs/Quaternion rotationMetricToDSO: Gravity direction encoded as the rotation to convert from metric world to DSO worldgeometry_msgs/Pose imuToCam: Transformation between camera and IMUdmvio/unscaled_pose: PoseStampedThis is just a copy of /dmvio/frame_tracked/pose. It can be useful for visualizing in Rviz as PoseStamped is a standard message.
Note that the used coordinate system is camera-based (see below), which is why it can look strange in Rviz.
dmvio/metric_pose: PoseStampedThese poses are the transformation from IMU to world (in metric scale). We provide this for convenience but for many applications you should not directly subscribe to this topic, but rather convert the poses yourself using the newest scale.
The reason is that these metric poses are all converted with the scale estimated at the time they were published. But when you perform computations on the entire trajectory you want to convert all poses with the newest scale instead.
dmvio/system_status: Int32This is a number, indicating the status the system is in (0 means visual initializer, 1 means visual-only mode and 2 means visual-inertial mode).
For details see the enum dmvio::SystemStatus.
Whenever a full reset happens, 0 is sent to this topic again.
As mentioned the poses are either in metric or in DSO frame, for converting between the two see TransformDSOToIMU::transformPose or equation (5) in the DM-VIO paper.
We use the typical camera-based coordinate frame, where x points to the right of the image, y points down and z points forward (into the camera).
publishKeyframes in ROSOutputWrapper.cpp. You could take inspiration from how it was implemented in LSD-SLAM.This ROS wrapper is licensed under the GNU General Public License 3 (GPLv3). The main.cpp file reuses code from the main file of DSO.