Skip to content

Commit

Permalink
Adding OpenCV's GPU GFTT/ OpticalFlow and CudaSift support (#1330)
Browse files Browse the repository at this point in the history
* Adding GFTT and SIFT Cuda support

* Working CudaSift

* Disable cudasift option when not available

* Added check to avoid re-allocating gpu memory everytime parseParameters is called. Added workaround of to detect/ignore invalid descriptors

* Added SIFT/PreciseUpscale and SIFT/Upscale parameters. Adjusted max octave to behave more like opencv

* Refactored how maximum features are thresholded, to be more similar to OpenCV version

* Updated loop closure benchmark scripts

* Cuda optical flow tmp commit

* Added Stereo/Gpu Vis/CorFlowGpu parameters (optical flow gpu integration for F2F odom and stereo correspondences)

* Fixed build without opencv cuda

* Fixed build with Opencv 4.10

* ZED: updated parameters to match zed sdk 4

* MRPT requires C++17

* updated max octave limit CudaSift
  • Loading branch information
matlabbe authored Sep 13, 2024
1 parent f3ccfcb commit 69ac21f
Show file tree
Hide file tree
Showing 37 changed files with 1,745 additions and 867 deletions.
23 changes: 21 additions & 2 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions Version.h.in
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
File renamed without changes.
File renamed without changes.
File renamed without changes.
86 changes: 0 additions & 86 deletions archive/2010-LoopClosure/DumpMemorySign.txt

This file was deleted.

56 changes: 56 additions & 0 deletions archive/2010-LoopClosure/ShowLogs/precisionRecallAllDetectors.m
Original file line number Diff line number Diff line change
@@ -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')
7 changes: 4 additions & 3 deletions archive/2010-LoopClosure/ShowLogs/showlogs.m
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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,
Expand All @@ -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,
Expand Down
8 changes: 0 additions & 8 deletions archive/2010-LoopClosure/run_all.sh

This file was deleted.

29 changes: 29 additions & 0 deletions archive/2010-LoopClosure/run_all_datasets.sh
Original file line number Diff line number Diff line change
@@ -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

37 changes: 37 additions & 0 deletions archive/2010-LoopClosure/run_all_detectors.sh
Original file line number Diff line number Diff line change
@@ -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

29 changes: 25 additions & 4 deletions archive/2010-LoopClosure/run_bow.sh
Original file line number Diff line number Diff line change
Expand Up @@ -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\
Expand All @@ -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"

26 changes: 22 additions & 4 deletions corelib/include/rtabmap/core/Features2d.h
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ namespace gpu {
class SURF_GPU;
class ORB_GPU;
class FAST_GPU;
class GoodFeaturesToTrackDetector_GPU;
}
}
typedef cv::SIFT CV_SIFT;
Expand All @@ -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;
Expand All @@ -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
Expand All @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
Loading

0 comments on commit 69ac21f

Please sign in to comment.