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,144 @@
+#!/bin/bash
+
+PWD=$(pwd)
+DOCKER=1 # set to 0 to use host rtabmap tool and relative datasets to this script
+
+KITTI_ROOT_PATH='./datasets/kitti'
+KITTI_RESULTS_PATH='./results/kitti'
+mkdir -p $KITTI_RESULTS_PATH
+if [ $DOCKER -eq 1 ]
+then
+    KITTI_ROOT_PATH='/root/datasets/kitti'
+    KITTI_RESULTS_PATH='/root/results/kitti'
+fi
+
+LIST=('00' '01' '02' '03' '04' '05' '06' '07' '08' '09' '10')
+#LIST=('11' '12' '13' '14' '15' '16' '17' '18' '19' '20' '21')
+
+STRATEGY=0
+REG=0
+OUTPUT="test"
+if [ $# -eq 4 ] 
+then
+    OUTPUT=$1
+    STRATEGY=$2
+    REG=$3
+    LIST=($4)
+elif [ $# -eq 3 ]
+then
+    OUTPUT=$1
+    STRATEGY=$2
+    REG=$3
+else
+echo "Usage: run_kitti_datasets.sh \"output name\" \"odom strategy: 0=f2m 1=f2f 11=f2f_optflow 2=fovis 3=viso2 4=dvo 5=orbslam2 7=LOAM 9=VINS\" \"reg strategy: 0=vis 1=icp-point2point 11=icp-point2plane\" [sequence]"
+exit
+fi
+
+F2F_params=""
+if [ $STRATEGY -eq 11 ]
+then
+    STRATEGY=1
+    F2F_params="--Vis/CorType 1"
+fi
+
+if [ $STRATEGY -eq 1 ]
+then
+    F2F_params="--Odom/KeyFrameThr 0.6 $F2F_params"
+fi
+
+if [ $STRATEGY -eq 3 ]
+then
+    F2F_params=" $F2F_params"
+fi
+
+if [ $STRATEGY -eq 9 ]
+then
+    F2F_params="--OdomVINS/ConfigPath ./kitti_config04-12.yaml $F2F_params"
+fi
+
+SCAN=""
+if [[ $REG -eq 1 || $REG -eq 11 ]]
+then
+   if [ $REG -eq 11 ]
+   then
+      REG=1
+      if [ $STRATEGY -eq 7 ]
+      then
+         SCAN="--Mem/LaserScanVoxelSize 0.5 --Mem/LaserScanNormalK 10 --scan_voxel 0.0"
+      else
+         SCAN="--scan_k 10 --scan_voxel 0.5"
+      fi
+      SCAN="--Icp/PointToPlane true --Icp/PointToPlaneK 10 --Icp/PointToPlaneRadius 0 --Icp/Iterations 5 --Odom/ScanKeyFrameThr 0.6 $SCAN"
+   else
+      if [ $STRATEGY -eq 7 ]
+      then
+         SCAN="--Mem/LaserScanVoxelSize 0.5 --scan_voxel 0.0"
+      else
+         SCAN="--scan_voxel 0.5"
+      fi
+      SCAN="--Icp/PointToPlane false --Icp/Iterations 10 --Odom/ScanKeyFrameThr 0.8 $SCAN"
+   fi
+   SCAN="--scan --Reg/Strategy $REG --OdomF2M/ScanMaxSize 10000 --OdomF2M/ScanSubtractRadius 0.5 --Odom/LOAMSensor 2 --Icp/MaxTranslation 2 --Icp/Epsilon 0.0001 --Icp/MaxCorrespondenceDistance 1.5 --Icp/CorrespondenceRatio 0.01 --Icp/PM true --Icp/PMOutlierRatio 0.7 --Icp/PMMatcherKnn 3 --Icp/PMMatcherEpsilon 1 --Icp/ReciprocalCorrespondences false  $SCAN"
+fi
+
+RTABMAP_KITTI_TOOL="rtabmap-kitti_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 7 ]
+   then
+       TOOL_PREFIX="/root/rtabmap_loam/bin"
+   fi
+   RTABMAP_KITTI_TOOL="docker run -v $PWD/datasets/kitti:$KITTI_ROOT_PATH -v $PWD/results/kitti:$KITTI_RESULTS_PATH -i -t --rm introlab3it/rtabmap:jfr2018 $TOOL_PREFIX/rtabmap-kitti_dataset"
+fi
+echo $RTABMAP_KITTI_TOOL
+
+for d in "${LIST[@]}"
+do
+    taskset 0x1 $RTABMAP_KITTI_TOOL \
+       --Rtabmap/PublishRAMUsage true\
+       --Vis/MaxFeatures 1500\
+       --RGBD/LinearUpdate 0\
+       --RGBD/AngularUpdate 0\
+       --RGBD/ProximityBySpace false\
+       --Rtabmap/DetectionRate 2\
+       --Kp/MaxFeatures 750\
+       --Kp/FlannRebalancingFactor 1.0\
+       --GFTT/QualityLevel 0.01\
+       --GFTT/MinDistance 7\
+       --OdomF2M/MaxSize 3000\
+       --Rtabmap/CreateIntermediateNodes true\
+       --Odom/Strategy $STRATEGY\
+       $F2F_params \
+       --OdomORBSLAM2/VocPath /root/ORBvoc.txt\
+       --OdomORBSLAM2/ThDepth 40\
+       --OdomORBSLAM2/Fps 10\
+       --OdomORBSLAM2/MaxFeatures 2000\
+       --OdomViso2/MatchNmsN 7\
+       --OdomViso2/MatchRadius 300\
+       --FAST/Threshold 12\
+       --Mem/STMSize 30\
+       --Mem/UseOdomFeatures false \
+       --Mem/BinDataKept false \
+       --Vis/CorNNDR 0.6 \
+       --Vis/CorGuessWinSize 20 \
+       $SCAN \
+       --gt $KITTI_ROOT_PATH"/devkit/cpp/data/odometry/poses/$d.txt"\
+       --output "$KITTI_RESULTS_PATH/$d"\
+       --output_name $OUTPUT\
+       --quiet\
+       $KITTI_ROOT_PATH/dataset/sequences/$d
+done
+
+# ORB_SLAM2:
+# 0-2: --OdomORBSLAM2/ThDepth 35
+# 3-12: --OdomORBSLAM2/ThDepth 40 (default)
+# 0-3: --FAST/Threshold 20 (default)
+# 4-12: --FAST/Threshold 12 --OdomORBSLAM2/ThDepth 40
+# --OdomORBSLAM2/MaxFeatures 2000
+# 
+