Adding OpenCV's GPU GFTT/ OpticalFlow and CudaSift support

CMakeLists.txt

@@ -177,6 +177,7 @@ option(WITH_TORCH         "Include Torch support (SuperPoint)"   OFF)
 option(WITH_PYTHON        "Include Python3 support (PyMatcher, PyDetector)"  OFF)
 option(WITH_PYTHON_THREADING  "Use more than one Python interpreter."  OFF)
 option(WITH_PDAL          "Include PDAL support"                 ON)
+option(WITH_CUDASIFT      "Include CudaSift support (this fork https://github.com/matlabbe/CudaSift)" OFF) 
 option(WITH_FREENECT      "Include Freenect support"             ON)
 option(WITH_FREENECT2     "Include Freenect2 support"            ON)
 option(WITH_K4W2          "Include Kinect for Windows v2 support" ON)
@@ -232,7 +233,7 @@ option(BUILD_WITH_RPATH_NOT_RUNPATH "Explicitly disable usage of RUNPATH for the
 set(RTABMAP_QT_VERSION AUTO CACHE STRING "Force a specific Qt version.")
 set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5 6)
 
-FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video videoio OPTIONAL_COMPONENTS aruco objdetect xfeatures2d nonfree gpu cudafeatures2d)
+FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video videoio OPTIONAL_COMPONENTS aruco objdetect xfeatures2d nonfree gpu cudafeatures2d cudaoptflow cudaimgproc)
 
 IF(WITH_QT)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
@@ -415,6 +416,13 @@ IF(WITH_PDAL)
     ENDIF(PDAL_FOUND)
 ENDIF(WITH_PDAL)
 
+IF(WITH_CUDASIFT)
+    FIND_PACKAGE(CudaSift 3 QUIET)
+    IF(CudaSift_FOUND)
+       MESSAGE(STATUS "Found CudaSift")
+    ENDIF(CudaSift_FOUND)
+ENDIF(WITH_CUDASIFT)
+
 IF(WITH_FREENECT)
     FIND_PACKAGE(Freenect QUIET)
     IF(Freenect_FOUND)
@@ -813,7 +821,7 @@ IF(NOT (APPLE OR WIN32) AND BUILD_WITH_RPATH_NOT_RUNPATH)
 ENDIF()
 
 IF(NOT MSVC)
-    IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR TORCH_FOUND)
+    IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR TORCH_FOUND OR MRPT_FOUND)
       # Qt6 requires c++17
       include(CheckCXXCompilerFlag)
       CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
@@ -942,6 +950,9 @@ ENDIF(NOT opengv_FOUND OR NOT WITH_OPENGV)
 IF(NOT PDAL_FOUND)
    SET(PDAL "//")
 ENDIF(NOT PDAL_FOUND)
+IF(NOT CudaSift_FOUND)
+   SET(CUDASIFT "//")
+ENDIF(NOT CudaSift_FOUND)
 IF(NOT loam_velodyne_FOUND)
    SET(LOAM "//")
 ENDIF(NOT loam_velodyne_FOUND)
@@ -1416,6 +1427,14 @@ ELSE()
 MESSAGE(STATUS "  With PDAL                 = NO (PDAL not found)")
 ENDIF()
 
+IF(CudaSift_FOUND)
+MESSAGE(STATUS "  With CudaSift             = YES (License: MIT)")
+ELSEIF(NOT WITH_CUDASIFT)
+MESSAGE(STATUS "  With CudaSift             = NO (WITH_CUDASIFT=OFF)")
+ELSE()
+MESSAGE(STATUS "  With CudaSift             = NO (CudaSift not found, use https://github.com/matlabbe/CudaSift fork)")
+ENDIF()
+
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Solvers:")
 IF(WITH_TORO)

Version.h.in

@@ -58,6 +58,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @FASTCV@#define RTABMAP_FASTCV
 @OPENGV@#define RTABMAP_OPENGV
 @PDAL@#define RTABMAP_PDAL
+@CUDASIFT@#define RTABMAP_CUDASIFT
 @LOAM@#define RTABMAP_LOAM
 @FLOAM@#define RTABMAP_FLOAM
 @DC1394@#define RTABMAP_DC1394

archive/2010-LoopClosure/ShowLogs/precisionRecallAllDetectors.m

@@ -0,0 +1,56 @@
+
+#set(0,'defaultAxesFontName', 'Times')
+#set(0,'defaultTextFontName', 'Times')
+
+Prefix = 'loop_closure_detection_datasets';
+Dataset= 'CityCentre'
+Detectors = {'Surf'; 'Sift'; 'CudaSift'; 'GfttBrief'};
+
+% The Ground Truth is a squared bmp file (size must match the log files 
+% length) where white dots mean loop closures.
+% Grey dots mean 'loop closures to ignore', this happens when the rehearsal
+% doesn't match consecutive images together.
+GroundTruthFile = [Prefix '/' Dataset '.png'];
+
+colors = 'kbgrcm';
+figure
+xlabel('Recall (%)')
+ylabel('Precision (%)')
+hold on;
+
+Results = {};
+TimeResults = {};
+
+for i=1:length(Detectors)
+    LogI = importfile([Prefix '/' Dataset '/' Detectors{i} 'LogI.txt']);
+    LogF = importfile([Prefix '/' Dataset '/' Detectors{i} 'LogF.txt']);
+
+    PR = getPrecisionRecall(LogI, LogF, GroundTruthFile, 0.07);
+    plot(100*PR(:,2), 100*PR(:,1), colors(mod(i,6)+1));
+   % hold on;
+   Results{i} = PR;
+   time = sum(LogF(:,2:7),2)+LogF(:,17);%LogF(:,1)
+   TimeResults{i} = time;
+   meanTime = mean(time)
+   meanWm = mean(LogI(:,7))
+   meanDict = mean(LogI(:,6))
+   maxTime = max(time)
+   maxWm = max(LogI(:,7))
+   maxDict = max(LogI(:,6))
+
+    %figure(2)
+    %plot(PR(:,4), PR(:,3), colors(mod(i,6)+1));
+    %hold on;
+end
+legend(Detectors)
+title(Dataset)
+
+figure
+rows=floor(length(Detectors)/2 )+ mod(length(Detectors),  2)
+for i=1:length(TimeResults)
+    subplot(rows, 2, i)
+    plot(TimeResults{i})
+    ylabel('Time (s)')
+    title([Detectors{i} ' (' num2str(mean(TimeResults{i})) 's)'])
+end
+xlabel('Location indexes')

archive/2010-LoopClosure/ShowLogs/showlogs.m

@@ -1,4 +1,4 @@
-function [LogF LogI] = showlogs(PathPrefix, GT_file)
+function [LogF LogI] = showlogs(PathPrefix, GT_file, LogPrefix)
 % [LogF LogI] = showlogs(PathPrefix, GT_file)
 % SHOWLOGS  Plot a RTAB-Map results (LogI.txt, LogF.txt). Just put this 
 %           file in the same directory as LogF.txt and LogI.txt files 
@@ -24,14 +24,15 @@
 
 close all
 
+if nargin < 3, LogPrefix = ''; end
 if nargin < 2, GT_file = ''; end
 if nargin < 1, PathPrefix = '.'; end
 
 %---------------------------------------------------------
 
 display(' ');
 display('Loading log files...');
-LogF = importfile([PathPrefix '/' 'LogF.txt']);
+LogF = importfile([PathPrefix '/' LogPrefix 'LogF.txt']);
 % COLUMN HEADERS : 
 % 1 totalTime
 % 2 timeMemoryUpdate,
@@ -54,7 +55,7 @@
 % 19 timeEmptyingTrash
 % 20 timeRetrievalDbAccess
 
-LogI = importfile([PathPrefix '/' 'LogI.txt']);
+LogI = importfile([PathPrefix '/' LogPrefix 'LogI.txt']);
 % COLUMN HEADERS : 
 % 1 lcHypothesisId,
 % 2 highestHypothesisId,

archive/2010-LoopClosure/run_all_datasets.sh

@@ -0,0 +1,29 @@
+#!/bin/bash
+
+DATASETS_FOLDER=""
+if [ $# -eq 1 ] 
+then
+    DATASETS_FOLDER=$1
+else
+echo "Usage: run_all_datasets.sh \"datasets folder\" "
+exit
+fi
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+
+$SCRIPT_DIR/run_bow.sh $DATASETS_FOLDER/NewCollege $DATASETS_FOLDER/NewCollege.png
+if [ -s LogF.txt ]; then
+mv LogF.txt $DATASETS_FOLDER/NewCollege/LogF.txt
+mv LogI.txt $DATASETS_FOLDER/NewCollege/LogI.txt
+fi
+$SCRIPT_DIR/run_bow.sh $DATASETS_FOLDER/CityCentre $DATASETS_FOLDER/CityCentre.png
+if [ -s LogF.txt ]; then
+mv LogF.txt $DATASETS_FOLDER/CityCentre/LogF.txt
+mv LogI.txt $DATASETS_FOLDER/CityCentre/LogI.txt
+fi
+$SCRIPT_DIR/run_bow.sh $DATASETS_FOLDER/UdeS_1Hz $DATASETS_FOLDER/UdeS_1Hz.png
+if [ -s LogF.txt ]; then
+mv LogF.txt $DATASETS_FOLDER/UdeS_1Hz/LogF.txt
+mv LogI.txt $DATASETS_FOLDER/UdeS_1Hz/LogI.txt
+fi
+

archive/2010-LoopClosure/run_all_detectors.sh

@@ -0,0 +1,37 @@
+#!/bin/bash
+
+DATASET_FOLDER=""
+GT_FILE=""
+MEMORY_THR=0
+if [ $# -ge 2 ] 
+then
+    DATASET_FOLDER=$1
+    GT_FILE=$2
+else
+echo "Usage: run_all_detectors.sh \"dataset folder\" \"ground truth file\" [\"Rtabmap/MemoryThr=0\"]"
+exit
+fi
+
+if [ $# -ge 3 ] 
+then
+    MEMORY_THR=$3
+fi
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+
+$SCRIPT_DIR/run_bow.sh $DATASET_FOLDER $GT_FILE $MEMORY_THR 0 false
+mv LogF.txt $DATASET_FOLDER/SurfLogF.txt
+mv LogI.txt $DATASET_FOLDER/SurfLogI.txt
+$SCRIPT_DIR/run_bow.sh $DATASET_FOLDER $GT_FILE $MEMORY_THR 1 false
+mv LogF.txt $DATASET_FOLDER/SiftLogF.txt
+mv LogI.txt $DATASET_FOLDER/SiftLogI.txt
+$SCRIPT_DIR/run_bow.sh $DATASET_FOLDER $GT_FILE $MEMORY_THR 1 true
+mv LogF.txt $DATASET_FOLDER/CudaSiftLogF.txt
+mv LogI.txt $DATASET_FOLDER/CudaSiftLogI.txt
+$SCRIPT_DIR/run_bow.sh $DATASET_FOLDER $GT_FILE $MEMORY_THR 6 false
+mv LogF.txt $DATASET_FOLDER/GfttBriefLogF.txt
+mv LogI.txt $DATASET_FOLDER/GfttBriefLogI.txt
+#$SCRIPT_DIR/run_bow.sh $DATASET_FOLDER $GT_FILE $MEMORY_THR 11 false
+#mv LogF.txt $DATASET_FOLDER/SuperPointLogF.txt
+#mv LogI.txt $DATASET_FOLDER/SuperPointLogI.txt
+

archive/2010-LoopClosure/run_bow.sh

@@ -2,22 +2,38 @@
 
 DATASET_FOLDER=""
 GT_FILE=""
-if [ $# -eq 2 ] 
+MEMORY_THR=300
+DETECTOR=0
+GPU=false
+if [ $# -ge 2 ] 
 then
     DATASET_FOLDER=$1
     GT_FILE=$2
 else
-echo "Usage: run_bow.sh \"dataset folder\" \"ground truth file\""
+echo "Usage: run_bow.sh \"dataset folder\" \"ground truth file\" [\"Rtabmap/MemoryThr=300\"] [\"Kp/DetectorStrategy=0\"] [\"GPU=false\"]"
 exit
 fi
 
+if [ $# -ge 3 ] 
+then
+    MEMORY_THR=$3
+fi
+if [ $# -ge 4 ] 
+then
+    DETECTOR=$4
+fi 
+if [ $# -ge 5 ] 
+then
+    GPU=$5
+fi 
+
 rtabmap-console \
        -quiet \
        --Rtabmap/StatisticLogged true\
        --Rtabmap/StatisticLoggedHeaders false\
-       --Kp/DetectorStrategy 0\
+       --Kp/DetectorStrategy $DETECTOR\
        --SURF/HessianThreshold 150\
-       --Rtabmap/MemoryThr 300\
+       --Rtabmap/MemoryThr $MEMORY_THR\
        --Rtabmap/LoopRatio 0.9\
        --Mem/STMSize 30\
        --Vis/MaxFeatures 400\
@@ -27,6 +43,11 @@ rtabmap-console \
        --Mem/BadSignaturesIgnored true\
        --Mem/RehearsalSimilarity 0.20\
        --Mem/RecentWmRatio 0.20\
+       --FAST/Gpu $GPU\
+       --GFTT/Gpu $GPU\
+       --ORB/Gpu $GPU\
+       --SIFT/Gpu $GPU\
+       --SURF/GpuVersion $GPU\
        -gt "$GT_FILE"\
        "$DATASET_FOLDER"
 

corelib/include/rtabmap/core/Features2d.h

@@ -45,6 +45,7 @@ namespace gpu {
 	class SURF_GPU;
 	class ORB_GPU;
 	class FAST_GPU;
+	class GoodFeaturesToTrackDetector_GPU;
 }
 }
 typedef cv::SIFT CV_SIFT;
@@ -57,13 +58,14 @@ typedef cv::BRISK CV_BRISK;
 typedef cv::gpu::SURF_GPU CV_SURF_GPU;
 typedef cv::gpu::ORB_GPU CV_ORB_GPU;
 typedef cv::gpu::FAST_GPU CV_FAST_GPU;
+typedef cv::gpu::GoodFeaturesToTrackDetector_GPU CV_GFTT_GPU;
 #else
 namespace cv{
 namespace xfeatures2d {
 class FREAK;
 class DAISY;
 class BriefDescriptorExtractor;
-#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
+#if (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 class SIFT;
 #endif
 class SURF;
@@ -72,9 +74,10 @@ namespace cuda {
 class FastFeatureDetector;
 class ORB;
 class SURF_CUDA;
+class CornersDetector;
 }
 }
-#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
+#if (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 typedef cv::xfeatures2d::SIFT CV_SIFT;
 #else
 typedef cv::SIFT CV_SIFT; // SIFT is back in features2d since 4.4.0 / 3.4.11
@@ -90,8 +93,12 @@ typedef cv::ORB CV_ORB;
 typedef cv::cuda::SURF_CUDA CV_SURF_GPU;
 typedef cv::cuda::ORB CV_ORB_GPU;
 typedef cv::cuda::FastFeatureDetector CV_FAST_GPU;
+typedef cv::cuda::CornersDetector CV_GFTT_GPU;
 #endif
 
+// CudaSift fork: https://github.com/matlabbe/CudaSift
+class SiftData;
+
 namespace rtabmap {
 
 class ORBextractor;
@@ -294,9 +301,18 @@ class RTABMAP_CORE_EXPORT SIFT : public Feature2D
 	double contrastThreshold_;
 	double edgeThreshold_;
 	double sigma_;
+	bool preciseUpscale_;
 	bool rootSIFT_;
-
-	cv::Ptr<CV_SIFT> _sift;
+	bool gpu_;
+	float guaussianThreshold_;
+	bool upscale_;
+
+	cv::Ptr<CV_SIFT> sift_;
+	SiftData * cudaSiftData_;
+	float * cudaSiftMemory_;
+	cv::Size cudaSiftMemorySize_;
+	cv::Mat cudaSiftDescriptors_;
+	bool cudaSiftUpscaling_;
 };
 
 //ORB
@@ -425,8 +441,10 @@ class RTABMAP_CORE_EXPORT GFTT : public Feature2D
 	int _blockSize;
 	bool _useHarrisDetector;
 	double _k;
+	bool _gpu;
 
 	cv::Ptr<CV_GFTT> _gftt;
+	cv::Ptr<CV_GFTT_GPU> _gpuGftt;
 };
 
 //GFTT_BRIEF