feat(slam): add rtabmap_ros

2025-07-14 11:34:38 +08:00
parent 3b6641c1fb
commit 943ce5b06f
1635 changed files with 603092 additions and 0 deletions
@@ -0,0 +1,156 @@
+#!/bin/bash
+
+PWD=$(pwd)
+DOCKER=1 # set to 0 to use host rtabmap tool and relative datasets to this script
+
+DATASET_ROOT_PATH='./datasets/euroc'
+EUROC_RESULTS_PATH='./results/euroc'
+mkdir -p $EUROC_RESULTS_PATH
+if [ $DOCKER -eq 1 ]
+then
+    DATASET_ROOT_PATH='/root/datasets/euroc'
+    EUROC_RESULTS_PATH='/root/results/euroc'
+fi
+
+LIST=( 'V1_01_easy' 'V1_02_medium' 'V1_03_difficult' 'V2_01_easy' 'V2_02_medium' 'V2_03_difficult' 'MH_01_easy' 'MH_02_easy' 'MH_03_medium' 'MH_04_difficult' 'MH_05_difficult')
+
+STRATEGY=0
+OUTPUT="test"
+DISP=0
+if [ $# -eq 4 ] 
+then
+    OUTPUT=$1
+    STRATEGY=$2
+    DISP=$3
+    LIST=($4)
+elif [ $# -eq 3 ] 
+then
+    OUTPUT=$1
+    STRATEGY=$2
+    DISP=$3
+else
+echo "Usage: run_euroc_datasets.sh \"output name\" \"odom strategy: 0=f2m 1=f2f 11=f2f_optflow 2=fovis 3=viso2 4=dvo 5=orbslam2 6=okvis(rect) 66=okvis(raw) 8=msckf_vio(rect) 88=msckf_vio(raw) 9=vins_vio(rect) 99=vins_vio(raw) 10=vins_stereo(rect) 100=vins_stereo(raw)\" \"Disparity: 0 or 1\" [sequence]"
+exit
+fi
+
+#uncomment for graph gravity optimization
+#F2F_params="--Optimizer/GravitySigma 0.01 --g2o/PixelVariance 5"
+
+if [ $STRATEGY -eq 11 ] # F2F optical flow
+then
+    STRATEGY=1
+    F2F_params="--Vis/CorType 1 --Vis/MinInliers 20 --RGBD/OptimizeMaxError 0"
+fi
+
+if [ $STRATEGY -eq 1 ] # F2F
+then
+    F2F_params="--Odom/KeyFrameThr 0.6 $F2F_params"
+fi
+
+if [ $DISP -eq 1 ]
+then
+    F2F_params="--disp --StereoBM/NumDisparities 32 --StereoBM/UniquenessRatio 5 $F2F_params"
+else
+    F2F_params="--exposure_comp $F2F_params"
+fi
+
+if [ $STRATEGY -eq 66 ] # okvis raw images
+then
+    STRATEGY=6
+    F2F_params="--raw --Odom/GuessMotion false --OdomOKVIS/ConfigPath ./config_fpga_p2_euroc.yaml $F2F_params"
+fi
+if [ $STRATEGY -eq 6 ] # okvis
+then
+    F2F_params="--Odom/GuessMotion false --OdomOKVIS/ConfigPath ./config_fpga_p2_euroc.yaml $F2F_params"
+fi
+
+if [ $STRATEGY -eq 88 ] # msckf_vio raw images
+then
+    STRATEGY=8
+    F2F_params="--raw --Odom/GuessMotion false $F2F_params"
+fi
+if [ $STRATEGY -eq 8 ] # msckf_vio
+then
+    F2F_params="--Odom/GuessMotion false $F2F_params"
+fi
+
+if [ $STRATEGY -eq 99 ] # vins raw images
+then
+    STRATEGY=9
+    F2F_params="--raw --Odom/GuessMotion false --OdomVINS/ConfigPath ./euroc_stereo_imu_config.yaml $F2F_params"
+fi
+if [ $STRATEGY -eq 9 ] # vins
+then
+    F2F_params="--Odom/GuessMotion false --OdomVINS/ConfigPath ./euroc_stereo_imu_config.yaml $F2F_params"
+fi
+
+if [ $STRATEGY -eq 100 ] # vins (stereo-only) raw images
+then
+    STRATEGY=9
+    F2F_params="--raw --Odom/GuessMotion false --OdomVINS/ConfigPath ./euroc_stereo_config.yaml $F2F_params"
+fi
+if [ $STRATEGY -eq 10 ] # vins (stereo-only)
+then
+    F2F_params="--Odom/GuessMotion false --OdomVINS/ConfigPath ./euroc_stereo_config.yaml $F2F_params"
+fi
+
+RTABMAP_EUROC_TOOL="rtabmap-euroc_dataset"
+if [ $DOCKER -eq 1 ]
+then
+    # Select rtabmap built with os2 support or not
+    TOOL_PREFIX="/usr/local/bin"
+    if [ $STRATEGY -eq 5 ]
+    then
+        TOOL_PREFIX="/root/rtabmap_os2/bin"
+    elif [ $STRATEGY -eq 8 ]
+    then
+        TOOL_PREFIX="/root/rtabmap_msckf/bin"
+    fi
+    RTABMAP_EUROC_TOOL="docker run -v $PWD/datasets/euroc:$DATASET_ROOT_PATH -v $PWD/results/euroc:$EUROC_RESULTS_PATH -i -t --rm introlab3it/rtabmap:jfr2018 $TOOL_PREFIX/rtabmap-euroc_dataset"
+    if [ $STRATEGY -eq 6 ]
+    then
+        xhost +
+        RTABMAP_EUROC_TOOL="docker run -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix -v $PWD/datasets/euroc:$DATASET_ROOT_PATH -v $PWD/results/euroc:$EUROC_RESULTS_PATH -i -t --rm introlab3it/rtabmap:jfr2018 $TOOL_PREFIX/rtabmap-euroc_dataset"
+    fi
+fi
+echo $RTABMAP_EUROC_TOOL
+
+for d in "${LIST[@]}"
+do
+    V203_params=""
+    if [ "$d" == "V2_03_difficult" ]
+    then
+       if [[ $STRATEGY -eq 0 || $STRATEGY -eq 1 ]]
+       then
+           V203_params="--Vis/MinInliers 10 --Vis/PnPReprojError 3"
+       fi
+    fi
+
+    taskset 0x1 $RTABMAP_EUROC_TOOL \
+       --Rtabmap/PublishRAMUsage true\
+       --RGBD/LinearUpdate 0\
+       --RGBD/AngularUpdate 0\
+       --RGBD/ProximityBySpace false\
+       --Rtabmap/DetectionRate 2\
+       --Kp/FlannRebalancingFactor 1.0\
+       --GFTT/MinDistance 3\
+       --Odom/Strategy $STRATEGY\
+       --Mem/STMSize 30\
+       --Mem/UseOdomFeatures false \
+       --Mem/BinDataKept false \
+       --Rtabmap/CreateIntermediateNodes false\
+       --Vis/CorNNDR 0.6 \
+       --Vis/CorGuessWinSize 20 \
+       $V203_params\
+       $F2F_params\
+       --OdomORBSLAM2/VocPath /root/ORBvoc.txt\
+       --OdomORBSLAM2/ThDepth 35\
+       --OdomORBSLAM2/MaxFeatures 1200\
+       --output "$EUROC_RESULTS_PATH/$d"\
+       --output_name $OUTPUT\
+       --quiet\
+       $DATASET_ROOT_PATH/$d
+done
+# For V2_03_difficult: --Vis/MinInliers 10 --disp StereoBM/NumDisparities 32 --StereoBM/UniquenessRatio 5 --Vis/PnPReprojError 3
+
+