Cuda optical flow tmp commit

introlab · Sep 1, 2024 · 03e98f2 · 03e98f2
1 parent 01e11f1
commit 03e98f2
Show file tree

Hide file tree

Showing 3 changed files with 48 additions and 12 deletions.
diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt
@@ -828,10 +828,6 @@ ADD_CUSTOM_COMMAND(
 
 add_definitions(${PCL_DEFINITIONS})
 
-
-# Make sure the compiler can find include files from our library.
-INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
-
 # Add binary that is built from the source file "main.cpp".
 # The extension is automatically found.
 ADD_LIBRARY(rtabmap_core ${SRC_FILES} ${RESOURCES_HEADERS})
@@ -841,7 +837,7 @@ generate_export_header(rtabmap_core
  DEPRECATED_MACRO_NAME RTABMAP_DEPRECATED)
 
 target_include_directories(rtabmap_core PUBLIC 
-  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../include;${CMAKE_CURRENT_BINARY_DIR}/include;${PUBLIC_INCLUDE_DIRS}>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../include;${CMAKE_CURRENT_BINARY_DIR}/include;${PUBLIC_INCLUDE_DIRS};${INCLUDE_DIRS}>"
   "$<INSTALL_INTERFACE:${INSTALL_INCLUDE_DIR};${PUBLIC_INCLUDE_DIRS}>")
 
 TARGET_LINK_LIBRARIES(rtabmap_core 

diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp
@@ -75,6 +75,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifdef HAVE_OPENCV_CUDAFEATURES2D
   #include <opencv2/cudafeatures2d.hpp>
 #endif
+#ifdef HAVE_OPENCV_CUDAIMGPROC
+#include <opencv2/cudaimgproc.hpp>
+#endif
 
 #ifdef RTABMAP_FASTCV
 #include <fastcv.h>

diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp
@@ -49,6 +49,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/xfeatures2d.hpp> // For GMS matcher
 #endif
 
+#ifdef HAVE_OPENCV_CUDAOPTFLOW
+#include <opencv2/cudaoptflow.hpp>
+#endif
+
 #include <rtflann/flann.hpp>
 
 
@@ -568,20 +572,53 @@ Transform RegistrationVis::computeTransformationImpl(
 				// Find features in the new left image
 				UDEBUG("guessSet = %d", guessSet?1:0);
 				std::vector<unsigned char> status;
-				std::vector<float> err;
-				UDEBUG("cv::calcOpticalFlowPyrLK() begin");
-				cv::calcOpticalFlowPyrLK(
+#ifdef HAVE_OPENCV_CUDAOPTFLOW
+				bool gpu = false;
+				if (gpu)
+				{
+					UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer host to device begin");
+					cv::cuda::GpuMat d_imageFrom(imageFrom);
+					cv::cuda::GpuMat d_imageTo(imageTo);
+					cv::cuda::GpuMat d_cornersFrom(cornersFrom);
+					cv::cuda::GpuMat d_cornersTo(cornersTo);
+					UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer host to device end");
+					cv::cuda::GpuMat d_status;
+					cv::Ptr<cv::cuda::SparsePyrLKOpticalFlow> d_pyrLK_sparse = cv::cuda::SparsePyrLKOpticalFlow::create(
+						cv::Size(_flowWinSize, _flowWinSize), guessSet ? 0 : _flowMaxLevel, _flowIterations, guessSet);
+
+					UDEBUG("cv::cuda::SparsePyrLKOpticalFlow calc begin");
+					d_pyrLK_sparse->calc(d_imageFrom, d_imageTo, d_cornersFrom, d_cornersTo, d_status);
+					UDEBUG("cv::cuda::SparsePyrLKOpticalFlow calc end");
+
+					UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer device to host begin");
+					// Transfer back data to CPU
+					cornersTo = std::vector<cv::Point2f>(d_cornersTo.cols);
+					cv::Mat matCornersTo(1, d_cornersTo.cols, CV_32FC2, (void*)&cornersTo[0]);
+					d_cornersTo.download(matCornersTo);
+
+					status = std::vector<unsigned char>(d_status.cols);
+					cv::Mat matStatus(1, d_status.cols, CV_8UC1, (void*)&status[0]);
+					d_status.download(matStatus);
+					UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer device to host end");
+				}
+				else
+#endif
+				{
+					std::vector<float> err;
+					UDEBUG("cv::calcOpticalFlowPyrLK() begin");
+					cv::calcOpticalFlowPyrLK(
 						imageFrom,
 						imageTo,
 						cornersFrom,
 						cornersTo,
 						status,
 						err,
 						cv::Size(_flowWinSize, _flowWinSize),
-						guessSet?0:_flowMaxLevel,
-						cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, _flowIterations, _flowEps),
-						cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
-				UDEBUG("cv::calcOpticalFlowPyrLK() end");
+						guessSet ? 0 : _flowMaxLevel,
+						cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, _flowIterations, _flowEps),
+						cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet ? cv::OPTFLOW_USE_INITIAL_FLOW : 0), 1e-4);
+					UDEBUG("cv::calcOpticalFlowPyrLK() end");
+				}
 
 				UASSERT(kptsFrom.size() == kptsFrom3D.size());
 				std::vector<cv::KeyPoint> kptsTo(kptsFrom.size());