RTAB-Map + VINS Fusion/ORB SLAM2 + EuRoC Datasets (2024)

<launch><!-- Examples: F2M (default VO): $ roslaunch rtabmap_ros euroc_datasets.launch $ rosbag play -.-clock V1_01_easy.bag For MH sequences, we should set MH_seq to true because the ground truth source is different. $ roslaunch rtabmap_ros euroc_datasets.launch MH_seq:=true $ rosbag play -.-clock MH_01_easy.bag MSCKF (VIO): $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 8" $ rosbag play -.-clock V1_01_easy.bag We need to ignore the first 24 seconds for correct VIO initialization (drone should not move). $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 8" MH_seq:=true $ rosbag play -.-clock -s 24 MH_01_easy.bag OKVIS (VIO): $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 6 OdomOKVIS/ConfigPath ~/okvis/config/config_fpga_p2_euroc.yaml" MH_seq:=true raw_images_for_odom:=true $ rosbag play -.-clock MH_01_easy.bag VINS (VIO): $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml" MH_seq:=true raw_images_for_odom:=true $ rosbag play -.-clock MH_01_easy.bag VINS (VO): $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml" MH_seq:=true raw_images_for_odom:=true $ rosbag play -.-clock MH_01_easy.bag--><param name="use_sim_time" value="true"/><arg name="feature_type" default="6"/><arg name="gravity_opt" default="false"/> <!-- Rtabmap will use IMU data to add gravity constraints to graph --><arg name="args" default=""/><arg if="$(arg gravity_opt)" name="common_args" default="-d --RGBD/CreateOccupancyGrid false --Odom/FeatureType $(arg feature_type) --Kp/DetectorStrategy $(arg feature_type) --Optimizer/GravitySigma 0.3 $(arg args)"/><arg unless="$(arg gravity_opt)" name="common_args" default="-d --RGBD/CreateOccupancyGrid false --Odom/FeatureType $(arg feature_type) --Kp/DetectorStrategy $(arg feature_type) $(arg args) "/><arg name="cfg" default=""/><arg name="MH_seq" default="false"/> <!-- For MH sequences, the ground truth is coming from a different topic --><arg name="raw_images_for_odom" default="false"/><arg name="record_ground_truth" default="false"/><arg name="rtabmapviz" default="true"/><arg name="rviz" default="false"/><!-- Image rectification and publishing synchronized camera_info--><group ns="stereo_camera"> <node pkg="rtabmap_ros" type="yaml_to_camera_info.py" name="yaml_to_camera_info_left"> <param name="yaml_path" value="$(find rtabmap_ros)/launch/calibration/euroc_left.yaml"/> <remap from="image" to="/cam0/image_raw"/> <remap from="camera_info" to="left/camera_info"/> </node> <node pkg="rtabmap_ros" type="yaml_to_camera_info.py" name="yaml_to_camera_info_right"> <param name="yaml_path" value="$(find rtabmap_ros)/launch/calibration/euroc_right.yaml"/> <param name="frame_id" value="cam1"/> <remap from="image" to="/cam1/image_raw"/> <remap from="camera_info" to="right/camera_info"/> </node> <node unless="$(arg raw_images_for_odom)" pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"> <remap from="left/image_raw" to="/cam0/image_raw"/> <remap from="right/image_raw" to="/cam1/image_raw"/> </node></group><!-- TF frames --><node pkg="tf" type="static_transform_publisher" name="imu_base_link" args="0 0 0 3.1415926 -1.570796 0 base_link imu4 5"/><node pkg="tf" type="static_transform_publisher" name="cam0_imu_link" args="-0.021640 -0.064677 0.009811 1.555925 0.025777 0.003757 imu4 cam0 50"/><node pkg="tf" type="static_transform_publisher" name="cam1_imu_link" args="-0.019844 0.045369 0.007862 1.558237 0.025393 0.017907 imu4 cam1 50"/> <!-- For MH sequences, /leica/position doesn't give the orientation, so minimal ground truth error could be as high as 12 cm --><node if="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="leica_base_link" args="0.120209 -0.0184772 -0.0748903 0 0 0 leica base_link_gt 100"/> <node unless="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="vicon_base_link" args="0.12395 -0.02781 -0.06901 0 0 0 vicon/firefly_sbx/firefly_sbx base_link_gt 100"/><node if="$(arg MH_seq)" pkg="rtabmap_ros" type="point_to_tf.py" name="point_to_tf"> <remap from="point" to="/leica/position"/> <param name="frame_id" value="leica"/> <param name="fixed_frame_id" value="world"/></node><node unless="$(arg MH_seq)" pkg="rtabmap_ros" type="transform_to_tf.py" name="transform_to_tf"> <remap from="transform" to="/vicon/firefly_sbx/firefly_sbx"/> <param name="frame_id" value="world"/> <param name="child_frame_id" value="vicon/firefly_sbx/firefly_sbx"/></node><node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" /><node pkg="imu_complementary_filter" type="complementary_filter_node" name="imu_filter" output="screen"> <remap from="imu/data_raw" to="/imu0"/> <param name="use_mag" value="false"/> <param name="world_frame" value="enu"/> <param name="publish_tf" value="false"/></node> <!-- RTAB-Map --><include file="$(find rtabmap_ros)/launch/rtabmap.launch"> <arg if="$(arg raw_images_for_odom)" name="rtabmap_args" value="$(arg common_args) --Rtabmap/ImagesAlreadyRectified false"/> <arg unless="$(arg raw_images_for_odom)" name="rtabmap_args" value="$(arg common_args)"/> <arg if="$(arg raw_images_for_odom)" name="odom_args" value="--Rtabmap/ImagesAlreadyRectified false"/> <arg if="$(arg raw_images_for_odom)" name="left_image_topic" value="/cam0/image_raw"/> <arg if="$(arg raw_images_for_odom)" name="right_image_topic" value="/cam1/image_raw"/> <arg name="stereo" value="true"/> <arg name="frame_id" value="base_link"/> <arg name="wait_for_transform" value="0.1"/> <arg if="$(arg record_ground_truth)" name="ground_truth_frame_id" value="world"/> <arg if="$(arg record_ground_truth)" name="ground_truth_base_frame_id" value="base_link_gt"/> <arg name="cfg" value="$(arg cfg)"/> <arg name="imu_topic" value="/imu/data"/> <arg name="rtabmapviz" value="$(arg rtabmapviz)"/> <arg name="rviz" value="$(arg rviz)"/> <arg name="wait_imu_to_init" value="true"/></include></launch>

source devel/setup.bashroslaunch rtabmap_ros euroc_datasets.launch MH_seq:=true

sudo apt-get install ros-melodic-imu-complementary-filter

rosbag play --clock MH_easy.bag

roslaunch rtabmap_ros euroc_datasets.launch

# Clone dependenciesRUN mkdir -p catkin_ws/srcWORKDIR /root/catkin_ws/srcRUN /bin/bash -c '. /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace'# OPENCV ROSRUN git clone https://github.com/ros-perception/vision_opencv.git && cd vision_opencv && git checkout kinetic# DVORUN git clone https://github.com/tum-vision/dvo_slam.git && cd dvo_slam && git checkout ${ROS_DISTRO}-devel && rm dvo_slam/package.xml && rm dvo_benchmark/package.xml && rm dvo_ros/package.xml# VISO2RUN git clone https://github.com/srv/viso2.git && cd viso2 && git checkout ${ROS_DISTRO}# MSCKF-VIORUN git clone https://github.com/KumarRobotics/msckf_vio.git && cd msckf_vio && git checkout a9386c5 && wget https://gist.githubusercontent.com/matlabbe/f2518d7427e7f6740af2110e540b1f2b/raw/a6d604e730bcbf3dcd7fd8a27302bed4bb94b799/msckf_vio_a9386c5_ros_commented.patch && git apply --ignore-space-change --ignore-whitespace msckf_vio_a9386c5_ros_commented.patch# FOVISRUN git clone https://github.com/srv/libfovis.git && cd libfovis && git checkout 896acc8425e9fd7c5609153b8bad349ae1abbb50# LOAMRUN git clone https://github.com/laboshinl/loam_velodyne.git && cd loam_velodyne && git checkout a4c364a677647f2a35831439032dc5a58378b3fd# VINS-FusionRUN git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git && cd VINS-Fusion && git checkout e72b5f7ae7437e8cf481cc0bcbfb5fe91bd3d640 && wget https://gist.githubusercontent.com/matlabbe/795ab37067367dca58bbadd8201d986c/raw/0657cc30fca6b1b61faeef42be9f3428861d2598/vins-fusion_e72b5f7.patch && git apply --ignore-space-change --ignore-whitespace vins-fusion_e72b5f7.patch# OKVISRUN git clone https://github.com/ethz-asl/okvis.git && cd okvis && git checkout 1dce9129f22dd4d21d944788cd9da3a4341586aa && wget https://gist.githubusercontent.com/matlabbe/383be55b5cb682fea217d45ef9a37ef8/raw/bd4f6886eeba3800f0e5171e77abc5dd6dc8bfd9/okvis_1dce912_marchnative_disabled.patch && git apply --ignore-space-change --ignore-whitespace okvis_1dce912_marchnative_disabled.patch# ORB_SLAM2RUN git clone https://github.com/stevenlovegrove/Pangolin.gitRUN git clone https://github.com/raulmur/ORB_SLAM2.git && cd ORB_SLAM2 && wget https://gist.githubusercontent.com/matlabbe/c10403c5d44af85cc3585c0e1c601a60/raw/48adf04098960d86ddf225f1a8c68af87bfcf56e/orbslam2_f2e6f51_marchnative_disabled.patch && git apply --ignore-space-change --ignore-whitespace orbslam2_f2e6f51_marchnative_disabled.patch

git clone https://github.com/raulmur/ORB_SLAM2.git

git apply --ignore-space-change --ignore-whitespace orbslam2_f2e6f51_marchnative_disabled.patch

cd ORB_SLAM2 && \ cd Thirdparty/DBoW2 && \ mkdir build && \ cd build && \ cmake .. -DCMAKE_BUILD_TYPE=Release && \ make -j8 && \ rm -rf * && \ cd ../../g2o && \ mkdir build && \ cd build && \ cmake .. -DCMAKE_BUILD_TYPE=Release && \ make -j8 && \ rm -rf * && \ cd ../../../ && \ cd Vocabulary && \ tar -xf ORBvoc.txt.tar.gz && \ cd .. && \ mkdir build && \ cd build && \ cmake .. -DCMAKE_BUILD_TYPE=Release && \ make -j8 && \ rm -rf *

/home/clark/Projects/ServiceRobotics/workspaces/ORB_SLAM2/include/System.h

cd Examples/ROS/ORB_SLAM2mkdir build && cd buildcmake .. -DROS_BUILD_TYPE=Release

CMake Error at /opt/ros/melodic/share/ros/core/rosbuild/private.cmake:99 (message): [rosbuild] rospack found package "ORB_SLAM2" at "/home/clark/Workspace/ORB_SLAM2-master/Examples/ROS/ORB_SLAM2", but the current directory is "/home/clark/Projects/ServiceRobotics/workspaces/ORB_SLAM2/Examples/ROS/ORB_SLAM2". You should double-check your ROS_PACKAGE_PATH to ensure that packages are found in the correct precedence order.

make -j8

source /opt/ros/melodic/setup.bashexport ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/clark/Projects/ServiceRobotics/workspaces/ORB_SLAM2/Examples/ROS:export LD_LIBRARY_PATH=/home/clark/Projects/ServiceRobotics/workspaces/ORB_SLAM2/Thirdparty/DBoW2/lib:/home/clark/Projects/ServiceRobotics/workspaces/ORB_SLAM2/Thirdparty/g2o/lib:/home/clark/Projects/ServiceRobotics/workspaces/ORB_SLAM2/lib:/home/clark/Projects/ServiceRobotics/workspaces/catkin_ws_rtabmap_dependencies/devel/lib:/opt/ros/melodic/lib:$LD_LIBRARY_PATHsource /home/clark/Projects/ServiceRobotics/workspaces/catkin_ws_rtabmap_dependencies/devel/setup.bashexport ORB_SLAM_ROOT_DIR=/home/clark/Projects/ServiceRobotics/workspaces/ORB_SLAM2

git clone https://github.com/introlab/rtabmap.git

cp -r rtabmap rtabmap_orbslam2

cmake -DWITH_G2O=OFF ..

make -j8

source devel/setup.bashroslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 5 OdomORBSLAM2/VocPath /home/clark/Projects/ServiceRobotics/workspaces/ORB_SLAM2/Vocabulary/ORBvoc.txt" MH_seq:=true raw_images_for_odom:=true

rosbag play --clock MH_01_easy.bag

rtabmap-databaseViewer ~/.ros/rtabmap.db

git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git cd VINS-Fusion git checkout e72b5f7ae7437e8cf481cc0bcbfb5fe91bd3d640

git apply --ignore-space-change --ignore-whitespace vins-fusion_e72b5f7.patch

WORKDIR /root# OKVISRUN git clone https://github.com/ethz-asl/okvis.git && cd okvis && git checkout 1dce9129f22dd4d21d944788cd9da3a4341586aa && wget https://gist.githubusercontent.com/matlabbe/383be55b5cb682fea217d45ef9a37ef8/raw/bd4f6886eeba3800f0e5171e77abc5dd6dc8bfd9/okvis_1dce912_marchnative_disabled.patch && git apply --ignore-space-change --ignore-whitespace okvis_1dce912_marchnative_disabled.patch# ORB_SLAM2RUN git clone https://github.com/stevenlovegrove/Pangolin.gitRUN git clone https://github.com/raulmur/ORB_SLAM2.git && cd ORB_SLAM2 && wget https://gist.githubusercontent.com/matlabbe/c10403c5d44af85cc3585c0e1c601a60/raw/48adf04098960d86ddf225f1a8c68af87bfcf56e/orbslam2_f2e6f51_marchnative_disabled.patch && git apply --ignore-space-change --ignore-whitespace orbslam2_f2e6f51_marchnative_disabled.patchRUN git clone https://github.com/introlab/rtabmap.git

roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 8" MH_seq:=truerosbag play --clock -s 24 MH_01_easy.bag

git clone https://github.com/KumarRobotics/msckf_vio.git

cd msckf_vio/ && git checkout a9386c5

roslaunch rtabmap_ros demo_stereo_outdoor.launch rtabmapviz:=true rviz:=truerosbag play --clock stereo_outdoorA.bag